ParticleHistory.h

Go to the documentation of this file.

#pragma once

 
#include <cstddef>
#include <list>
#include <optional>
#include <span>
#include <utility>
 
#include <EmrysFilter/Core/Allocation.h>
#include <EmrysFilter/Core/Execution.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Distributions/ParticleSet.h>
#include <EmrysFilter/Smoothers/Lag.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{

template <Space Space_, RealScalar Scalar_ = double, typename Allocator_ = DefaultAllocator>
class ParticleHistory
{
public:
 
    using Scalar = Scalar_;                                            
    using SpaceType = Space_;                                          
    using ParticleSetType = ParticleSet<Space_, Scalar_, Allocator_>;  
    using allocator_type = Allocator_;                                 
    using Index = Eigen::Index;                                        
 
private:
 
    using IndexBuffer = internal::Buffer<Index, Allocator_>;      
    using IndexAllocator = typename IndexBuffer::allocator_type;  
 
public:

    class Step
    {
    public:
 
        using allocator_type = Allocator_;  

        explicit Step(ParticleSetType const & recorded, Allocator_ const & allocator = {})
            : particles{recorded, allocator}
            , smoothed{recorded, allocator}
            , parents(IndexAllocator(allocator))
        {
            parents.reserve(std::size_t(recorded.size()));
        }

        Step(Step const & other, Allocator_ const & allocator)
            : particles{other.particles, allocator}
            , smoothed{other.smoothed, allocator}
            , parents(other.parents, IndexAllocator(allocator))
            , resampled{other.resampled}
        {}

        Step(Step && other, Allocator_ const & allocator)
            : particles{std::move(other.particles), allocator}
            , smoothed{std::move(other.smoothed), allocator}
            , parents(std::move(other.parents), IndexAllocator(allocator))
            , resampled{other.resampled}
        {}
 
        Step(Step const &) = default;  
        Step(Step &&) = default;       

        Step &
        operator=(Step const &) = default;

        Step &
        operator=(Step &&) = default;
 
        ~Step() = default;

        ParticleSetType particles;

        ParticleSetType smoothed;

        IndexBuffer parents;
 
        bool resampled = false;  
    };
 
private:
 
    using StepAllocator = internal::ReBound<Allocator_, Step>;  
    using ListType = std::list<Step, StepAllocator>;            
 
public:
 
    using const_iterator = typename ListType::const_iterator;  

    ParticleHistory() = default;

    explicit ParticleHistory(Allocator_ const & allocator)
        : m_steps(StepAllocator(allocator))
        , m_lineage(IndexAllocator(allocator))
        , m_traced(internal::bufferOf<Scalar>(allocator))
    {}

    explicit ParticleHistory(Lag lag, Allocator_ const & allocator = {})
        : m_steps(StepAllocator(allocator))
        , m_lineage(IndexAllocator(allocator))
        , m_traced(internal::bufferOf<Scalar>(allocator))
        , m_lag{lag.steps}
    {
        EMRYS_FILTER_ASSERT(lag.steps > 0 && "a fixed-lag history keeps at least one step");
    }

    ParticleHistory(ParticleHistory const & other)
        : ParticleHistory{other, other.get_allocator()}
    {}

    ParticleHistory(ParticleHistory const & other, Allocator_ const & allocator)
        : m_steps(other.m_steps, StepAllocator(allocator))
        , m_lineage(IndexAllocator(allocator))
        , m_traced(internal::bufferOf<Scalar>(allocator))
        , m_lag{other.m_lag}
        , m_resampleCount{other.m_resampleCount}
    {}

    ParticleHistory(ParticleHistory && other) = default;

    ParticleHistory(ParticleHistory && other, Allocator_ const & allocator)
        : m_steps(std::move(other.m_steps), StepAllocator(allocator))
        , m_lineage(IndexAllocator(allocator))
        , m_traced(internal::bufferOf<Scalar>(allocator))
        , m_lag{other.m_lag}
        , m_resampleCount{other.m_resampleCount}
    {}

    ParticleHistory &
    operator=(ParticleHistory const & other) = default;

    ParticleHistory &
    operator=(ParticleHistory && other) = default;
 
    ~ParticleHistory() = default;

    [[nodiscard]] allocator_type
    get_allocator() const
    {
        return allocator_type(m_steps.get_allocator());
    }

    [[nodiscard]] std::optional<Index>
    lag() const
    {
        return m_lag > 0 ? std::optional<Index>{m_lag} : std::nullopt;
    }

    [[nodiscard]] Index
    size() const
    {
        return Index(m_steps.size());
    }

    [[nodiscard]] bool
    empty() const
    {
        return m_steps.empty();
    }

    [[nodiscard]] Step const &
    front() const
    {
        EMRYS_FILTER_ASSERT(!empty() && "the history is empty");
        return m_steps.front();
    }

    [[nodiscard]] Step const &
    back() const
    {
        EMRYS_FILTER_ASSERT(!empty() && "the history is empty");
        return m_steps.back();
    }

    [[nodiscard]] const_iterator
    begin() const
    {
        return m_steps.begin();
    }

    [[nodiscard]] const_iterator
    end() const
    {
        return m_steps.end();
    }

    void
    push(ParticleSetType const & particles)
    {
        bool const resampled = !empty() && newResample(particles);
        EMRYS_FILTER_ASSERT(
            (empty() || resampled || particles.size() == back().particles.size())
            && "without a resample the particles must be those of the step before"
        );
        if (m_lag > 0 && size() == m_lag)
        {
            m_steps.splice(m_steps.end(), m_steps, m_steps.begin());
            Step & step = m_steps.back();
            step.particles = particles;
            step.smoothed = particles;
        }
        else
        {
            m_steps.emplace_back(particles);
        }
        Step & step = m_steps.back();
        step.resampled = resampled;
        if (resampled)
        {
            std::span<Index const> const ancestors = particles.ancestors();
            step.parents.assign(ancestors.begin(), ancestors.end());
        }
        m_resampleCount = particles.resampleCount();
    }

    void
    smooth(ParticleSetType const & latest)
    {
        smooth(execution::sequential, latest);
    }

    template <ExecutionPolicy P>
    void
    smooth(P && policy, ParticleSetType const & latest)
    {
        if (empty())
        {
            return;
        }
        bool const resampled = newResample(latest);
        EMRYS_FILTER_ASSERT(
            (resampled || latest.size() == back().particles.size())
            && "without a resample the particles must be those of the newest step"
        );
        Index const count = latest.size();
        m_lineage.resize(std::size_t(count));
        std::span<Index const> const ancestors = latest.ancestors();
        internal::forEachIndex(policy, count, [&](Index j) {
            m_lineage[std::size_t(j)] = resampled ? ancestors[std::size_t(j)] : j;
        });
        Step const * next = nullptr;
        for (auto step = m_steps.rbegin(); step != m_steps.rend(); ++step)
        {
            if (next != nullptr && next->resampled)
            {
                internal::forEachIndex(policy, count, [&](Index j) {
                    Index const here = m_lineage[std::size_t(j)];
                    EMRYS_FILTER_ASSERT(here < Index(next->parents.size()) && "a traced index is out of range");
                    m_lineage[std::size_t(j)] = next->parents[std::size_t(here)];
                });
            }
            auto const states = step->particles.states();
            m_traced.resize(std::size_t(states.rows() * count));
            auto traced = SampleSetMap<Space_, Scalar_>{m_traced.data(), states.rows(), count};
            internal::gather(policy, std::span<Index const>{m_lineage}, states, traced);
            step->smoothed.assign(traced, latest.logWeights());
            step->smoothed.clearBandwidth();
            next = &*step;
        }
    }

    void
    clear()
    {
        m_steps.clear();
    }
 
private:

    [[nodiscard]] bool
    newResample(ParticleSetType const & particles) const
    {
        Index const resamples = particles.resampleCount() - m_resampleCount;
        EMRYS_FILTER_ASSERT(
            resamples >= 0 && resamples <= 1 && "at most one resample may fall between two records of the particles"
        );
        return resamples == 1;
    }
 
    ListType m_steps;                               
    IndexBuffer m_lineage;                          
    internal::Buffer<Scalar, Allocator_> m_traced;  
    Index m_lag = 0;                                
    Index m_resampleCount = 0;                      
};
 
}  // namespace Emrys