osrm-backend/include/engine/map_matching/hidden_markov_model.hpp

#ifndef HIDDEN_MARKOV_MODEL
#define HIDDEN_MARKOV_MODEL

#include "util/integer_range.hpp"

#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include <cmath>

#include <limits>
#include <vector>

namespace osrm
{
namespace engine
{
namespace map_matching
{

static const double log_2_pi = std::log(2. * boost::math::constants::pi<double>());
static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
static const double MINIMAL_LOG_PROB = std::numeric_limits<double>::lowest();
static const std::size_t INVALID_STATE = std::numeric_limits<std::size_t>::max();

// closures to precompute log -> only simple floating point operations
struct EmissionLogProbability
{
    double sigma_z;
    double log_sigma_z;

    EmissionLogProbability(const double sigma_z) : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
    {
    }

    double operator()(const double distance) const
    {
        return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z;
    }
};

struct TransitionLogProbability
{
    double beta;
    double log_beta;
    TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}

    double operator()(const double d_t) const { return -log_beta - d_t / beta; }
};

template <class CandidateLists> struct HiddenMarkovModel
{
    std::vector<std::vector<double>> viterbi;
    std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
    std::vector<std::vector<float>> path_lengths;
    std::vector<std::vector<bool>> pruned;
    std::vector<std::vector<bool>> suspicious;
    std::vector<bool> breakage;

    const CandidateLists &candidates_list;
    const EmissionLogProbability &emission_log_probability;

    HiddenMarkovModel(const CandidateLists &candidates_list,
                      const EmissionLogProbability &emission_log_probability)
        : breakage(candidates_list.size()), candidates_list(candidates_list),
          emission_log_probability(emission_log_probability)
    {
        viterbi.resize(candidates_list.size());
        parents.resize(candidates_list.size());
        path_lengths.resize(candidates_list.size());
        suspicious.resize(candidates_list.size());
        pruned.resize(candidates_list.size());
        breakage.resize(candidates_list.size());
        for (const auto i : util::irange<std::size_t>(0u, candidates_list.size()))
        {
            const auto &num_candidates = candidates_list[i].size();
            // add empty vectors
            if (num_candidates > 0)
            {
                viterbi[i].resize(num_candidates);
                parents[i].resize(num_candidates);
                path_lengths[i].resize(num_candidates);
                suspicious[i].resize(num_candidates);
                pruned[i].resize(num_candidates);
            }
        }

        clear(0);
    }

    void clear(std::size_t initial_timestamp)
    {
        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_lengths.size() &&
                     path_lengths.size() == pruned.size() && pruned.size() == breakage.size());

        for (const auto t : util::irange(initial_timestamp, viterbi.size()))
        {
            std::fill(viterbi[t].begin(), viterbi[t].end(), IMPOSSIBLE_LOG_PROB);
            std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0u, 0u));
            std::fill(path_lengths[t].begin(), path_lengths[t].end(), 0);
            std::fill(suspicious[t].begin(), suspicious[t].end(), true);
            std::fill(pruned[t].begin(), pruned[t].end(), true);
        }
        std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
    }

    std::size_t initialize(std::size_t initial_timestamp)
    {
        auto num_points = candidates_list.size();
        do
        {
            BOOST_ASSERT(initial_timestamp < num_points);

            for (const auto s : util::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
            {
                viterbi[initial_timestamp][s] =
                    emission_log_probability(candidates_list[initial_timestamp][s].distance);
                parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < MINIMAL_LOG_PROB;
                suspicious[initial_timestamp][s] = false;

                breakage[initial_timestamp] =
                    breakage[initial_timestamp] && pruned[initial_timestamp][s];
            }

            ++initial_timestamp;
        } while (initial_timestamp < num_points && breakage[initial_timestamp - 1]);

        if (initial_timestamp >= num_points)
        {
            return INVALID_STATE;
        }

        BOOST_ASSERT(initial_timestamp > 0);
        --initial_timestamp;

        BOOST_ASSERT(breakage[initial_timestamp] == false);

        return initial_timestamp;
    }
};
}
}
}

#endif // HIDDEN_MARKOV_MODEL