From a9c3b343fc25708f57d7b8542f2448662efa6857 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 12:48:33 +0100 Subject: [PATCH] separate model and computation in HMM matching --- data_structures/hidden_markov_model.hpp | 154 ++++++++++++++ plugins/match.hpp | 8 +- routing_algorithms/map_matching.hpp | 270 +++--------------------- util/matching_debug_info.hpp | 152 +++++++++++++ 4 files changed, 342 insertions(+), 242 deletions(-) create mode 100644 data_structures/hidden_markov_model.hpp create mode 100644 util/matching_debug_info.hpp diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp new file mode 100644 index 000000000..affaf3013 --- /dev/null +++ b/data_structures/hidden_markov_model.hpp @@ -0,0 +1,154 @@ +/* + +Copyright (c) 2015, Project OSRM contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +#ifndef HIDDEN_MARKOV_MODEL +#define HIDDEN_MARKOV_MODEL + +#include + +#include + +#include +#include + +namespace osrm +{ +namespace matching +{ +// FIXME this value should be a table based on samples/meter (or samples/min) +constexpr static const double log_2_pi = 1.837877066409346; // std::log(2. * M_PI); + +constexpr static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits::infinity(); +constexpr static const double MINIMAL_LOG_PROB = std::numeric_limits::lowest(); +constexpr static const unsigned INVALID_STATE = std::numeric_limits::max(); +} // namespace matching +} // namespace osrm + +// 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 * (osrm::matching::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 struct HiddenMarkovModel +{ + std::vector> viterbi; + std::vector>> parents; + std::vector> path_lengths; + std::vector> pruned; + std::vector 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) + { + for (const auto &l : candidates_list) + { + viterbi.emplace_back(l.size()); + parents.emplace_back(l.size()); + path_lengths.emplace_back(l.size()); + pruned.emplace_back(l.size()); + } + + clear(0); + } + + void clear(unsigned initial_timestamp) + { + BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_lengths.size() && + path_lengths.size() == pruned.size() && pruned.size() == breakage.size()); + + for (unsigned t = initial_timestamp; t < viterbi.size(); t++) + { + std::fill(viterbi[t].begin(), viterbi[t].end(), osrm::matching::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(pruned[t].begin(), pruned[t].end(), true); + } + std::fill(breakage.begin() + initial_timestamp, breakage.end(), true); + } + + unsigned initialize(unsigned initial_timestamp) + { + BOOST_ASSERT(initial_timestamp < candidates_list.size()); + + do + { + for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) + { + viterbi[initial_timestamp][s] = + emission_log_probability(candidates_list[initial_timestamp][s].second); + parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s); + pruned[initial_timestamp][s] = + viterbi[initial_timestamp][s] < osrm::matching::MINIMAL_LOG_PROB; + + breakage[initial_timestamp] = + breakage[initial_timestamp] && pruned[initial_timestamp][s]; + } + + ++initial_timestamp; + } while (breakage[initial_timestamp - 1]); + + if (initial_timestamp >= viterbi.size()) + { + return osrm::matching::INVALID_STATE; + } + + BOOST_ASSERT(initial_timestamp > 0); + --initial_timestamp; + + BOOST_ASSERT(breakage[initial_timestamp] == false); + + return initial_timestamp; + } +}; + +#endif // HIDDEN_MARKOV_MODEL diff --git a/plugins/match.hpp b/plugins/match.hpp index b67a4f344..88454eeb9 100644 --- a/plugins/match.hpp +++ b/plugins/match.hpp @@ -100,7 +100,7 @@ template class MapMatchingPlugin : public BasePlugin bool getCandiates(const std::vector &input_coords, std::vector &sub_trace_lengths, - Matching::CandidateLists &candidates_lists) + osrm::matching::CandidateLists &candidates_lists) { double last_distance = coordinate_calculation::great_circle_distance(input_coords[0], input_coords[1]); @@ -165,7 +165,7 @@ template class MapMatchingPlugin : public BasePlugin return true; } - osrm::json::Object submatchingToJSON(const Matching::SubMatching &sub, + osrm::json::Object submatchingToJSON(const osrm::matching::SubMatching &sub, const RouteParameters &route_parameters, const InternalRouteResult &raw_route) { @@ -222,7 +222,7 @@ template class MapMatchingPlugin : public BasePlugin } std::vector sub_trace_lengths; - Matching::CandidateLists candidates_lists; + osrm::matching::CandidateLists candidates_lists; const auto &input_coords = route_parameters.coordinates; const auto &input_timestamps = route_parameters.timestamps; if (input_timestamps.size() > 0 && input_coords.size() != input_timestamps.size()) @@ -248,7 +248,7 @@ template class MapMatchingPlugin : public BasePlugin osrm::json::Logger::get()->initialize("matching"); // call the actual map matching - Matching::SubMatchingList sub_matchings; + osrm::matching::SubMatchingList sub_matchings; search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, route_parameters.matching_beta, route_parameters.gps_precision, sub_matchings); diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index d5fb30e12..2000242d3 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "routing_base.hpp" #include "../data_structures/coordinate_calculation.hpp" +#include "../data_structures/hidden_markov_model.hpp" +#include "../util/matching_debug_info.hpp" #include "../util/simple_logger.hpp" #include "../util/json_util.hpp" #include "../util/json_logger.hpp" @@ -44,7 +46,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -namespace Matching +namespace osrm +{ +namespace matching { struct SubMatching @@ -57,12 +61,15 @@ struct SubMatching using CandidateList = std::vector>; using CandidateLists = std::vector; +using HMM = HiddenMarkovModel; using SubMatchingList = std::vector; -constexpr static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits::infinity(); -constexpr static const double MINIMAL_LOG_PROB = -std::numeric_limits::max(); -constexpr static const unsigned INVALID_STATE = std::numeric_limits::max(); + constexpr static const unsigned MAX_BROKEN_STATES = 6; constexpr static const unsigned MAX_BROKEN_TIME = 30; + +constexpr static const double default_beta = 10.0; +constexpr static const double default_sigma_z = 4.07; +} } // implements a hidden markov model map matching algorithm @@ -73,36 +80,6 @@ class MapMatching final : public BasicRoutingInterface 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; } - }; - double get_network_distance(const PhantomNode &source_phantom, const PhantomNode &target_phantom) const { @@ -188,220 +165,37 @@ class MapMatching final : public BasicRoutingInterface> viterbi; - std::vector>> parents; - std::vector> path_lengths; - std::vector> pruned; - std::vector breakage; - - const Matching::CandidateLists &candidates_list; - const EmissionLogProbability &emission_log_probability; - - HiddenMarkovModel(const Matching::CandidateLists &candidates_list, - const EmissionLogProbability &emission_log_probability) - : breakage(candidates_list.size()), candidates_list(candidates_list), - emission_log_probability(emission_log_probability) - { - for (const auto &l : candidates_list) - { - viterbi.emplace_back(l.size()); - parents.emplace_back(l.size()); - path_lengths.emplace_back(l.size()); - pruned.emplace_back(l.size()); - } - - clear(0); - } - - void clear(unsigned initial_timestamp) - { - BOOST_ASSERT(viterbi.size() == parents.size() && - parents.size() == path_lengths.size() && - path_lengths.size() == pruned.size() && pruned.size() == breakage.size()); - - for (unsigned t = initial_timestamp; t < viterbi.size(); t++) - { - std::fill(viterbi[t].begin(), viterbi[t].end(), Matching::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(pruned[t].begin(), pruned[t].end(), true); - } - std::fill(breakage.begin() + initial_timestamp, breakage.end(), true); - } - - unsigned initialize(unsigned initial_timestamp) - { - BOOST_ASSERT(initial_timestamp < candidates_list.size()); - - do - { - for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) - { - viterbi[initial_timestamp][s] = - emission_log_probability(candidates_list[initial_timestamp][s].second); - parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s); - pruned[initial_timestamp][s] = - viterbi[initial_timestamp][s] < Matching::MINIMAL_LOG_PROB; - - breakage[initial_timestamp] = - breakage[initial_timestamp] && pruned[initial_timestamp][s]; - } - - ++initial_timestamp; - } while (breakage[initial_timestamp - 1]); - - if (initial_timestamp >= viterbi.size()) - { - return Matching::INVALID_STATE; - } - - BOOST_ASSERT(initial_timestamp > 0); - --initial_timestamp; - - BOOST_ASSERT(breakage[initial_timestamp] == false); - - return initial_timestamp; - } - }; - - // Provides the debug interface for introspection tools - struct DebugInfo - { - DebugInfo(const osrm::json::Logger *logger) : logger(logger) - { - if (logger) - { - object = &logger->map->at("matching"); - } - } - - void initialize(const Matching::CandidateLists &candidates_list) - { - // json logger not enabled - if (!logger) - return; - - osrm::json::Array states; - for (unsigned t = 0; t < candidates_list.size(); t++) - { - osrm::json::Array timestamps; - for (unsigned s = 0; s < candidates_list[t].size(); s++) - { - osrm::json::Object state; - state.values["transitions"] = osrm::json::Array(); - state.values["coordinate"] = osrm::json::make_array( - candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, - candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); - state.values["viterbi"] = - osrm::json::clamp_float(Matching::IMPOSSIBLE_LOG_PROB); - state.values["pruned"] = 0u; - timestamps.values.push_back(state); - } - states.values.push_back(timestamps); - } - osrm::json::get(*object, "states") = states; - } - - void add_transition_info(const unsigned prev_t, - const unsigned current_t, - const unsigned prev_state, - const unsigned current_state, - const double prev_viterbi, - const double emission_pr, - const double transition_pr, - const double network_distance, - const double great_circle_distance) - { - // json logger not enabled - if (!logger) - return; - - osrm::json::Object transistion; - transistion.values["to"] = osrm::json::make_array(current_t, current_state); - transistion.values["properties"] = osrm::json::make_array( - osrm::json::clamp_float(prev_viterbi), osrm::json::clamp_float(emission_pr), - osrm::json::clamp_float(transition_pr), network_distance, great_circle_distance); - - osrm::json::get(*object, "states", prev_t, prev_state, "transitions") - .get>() - .get() - .values.push_back(transistion); - } - - void set_viterbi(const std::vector> &viterbi, - const std::vector> &pruned) - { - // json logger not enabled - if (!logger) - return; - - for (auto t = 0u; t < viterbi.size(); t++) - { - for (auto s_prime = 0u; s_prime < viterbi[t].size(); ++s_prime) - { - osrm::json::get(*object, "states", t, s_prime, "viterbi") = - osrm::json::clamp_float(viterbi[t][s_prime]); - osrm::json::get(*object, "states", t, s_prime, "pruned") = - static_cast(pruned[t][s_prime]); - } - } - } - - void add_chosen(const unsigned t, const unsigned s) - { - // json logger not enabled - if (!logger) - return; - - osrm::json::get(*object, "states", t, s, "chosen") = true; - } - - void add_breakage(const std::vector &breakage) - { - // json logger not enabled - if (!logger) - return; - - osrm::json::get(*object, "breakage") = osrm::json::make_array(breakage); - } - - const osrm::json::Logger *logger; - osrm::json::Value *object; - }; - public: MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data) : super(facade), engine_working_data(engine_working_data) { } - void operator()(const Matching::CandidateLists &candidates_list, + void operator()(const osrm::matching::CandidateLists &candidates_list, const std::vector &trace_coordinates, const std::vector &trace_timestamps, const double matching_beta, const double gps_precision, - Matching::SubMatchingList &sub_matchings) const + osrm::matching::SubMatchingList &sub_matchings) const { BOOST_ASSERT(candidates_list.size() > 0); // TODO replace default values with table lookup based on sampling frequency - EmissionLogProbability emission_log_probability(gps_precision > 0 ? gps_precision - : default_sigma_z); - TransitionLogProbability transition_log_probability(matching_beta > 0 ? matching_beta - : default_beta); + EmissionLogProbability emission_log_probability( + gps_precision > 0 ? gps_precision : osrm::matching::default_sigma_z); + TransitionLogProbability transition_log_probability( + matching_beta > 0 ? matching_beta : osrm::matching::default_beta); - HiddenMarkovModel model(candidates_list, emission_log_probability); + osrm::matching::HMM model(candidates_list, emission_log_probability); unsigned initial_timestamp = model.initialize(0); - if (initial_timestamp == Matching::INVALID_STATE) + if (initial_timestamp == osrm::matching::INVALID_STATE) { return; } - DebugInfo debug(osrm::json::Logger::get()); - debug.initialize(candidates_list); + MatchingDebugInfo matching_debug(osrm::json::Logger::get()); + matching_debug.initialize(candidates_list); unsigned breakage_begin = std::numeric_limits::max(); std::vector split_points; @@ -419,12 +213,12 @@ class MapMatching final : public BasicRoutingInterface - Matching::MAX_BROKEN_TIME); + osrm::matching::MAX_BROKEN_TIME); } else { - trace_split = trace_split || - (t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES); + trace_split = trace_split || (t - prev_unbroken_timestamps.back() > + osrm::matching::MAX_BROKEN_STATES); } if (trace_split) @@ -441,7 +235,7 @@ class MapMatching final : public BasicRoutingInterface stop viterbi calculation - if (new_start == Matching::INVALID_STATE) + if (new_start == osrm::matching::INVALID_STATE) { break; } @@ -499,9 +293,9 @@ class MapMatching final : public BasicRoutingInterface current_viterbi[s_prime]) { @@ -533,7 +327,7 @@ class MapMatching final : public BasicRoutingInterface 0) { @@ -543,7 +337,7 @@ class MapMatching final : public BasicRoutingInterface + +// Provides the debug interface for introspection tools +struct MatchingDebugInfo +{ + MatchingDebugInfo(const osrm::json::Logger *logger) : logger(logger) + { + if (logger) + { + object = &logger->map->at("matching"); + } + } + + template void initialize(const CandidateLists &candidates_list) + { + // json logger not enabled + if (!logger) + { + return; + } + + osrm::json::Array states; + for (unsigned t = 0; t < candidates_list.size(); t++) + { + osrm::json::Array timestamps; + for (unsigned s = 0; s < candidates_list[t].size(); s++) + { + osrm::json::Object state; + state.values["transitions"] = osrm::json::Array(); + state.values["coordinate"] = osrm::json::make_array( + candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, + candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); + state.values["viterbi"] = + osrm::json::clamp_float(osrm::matching::IMPOSSIBLE_LOG_PROB); + state.values["pruned"] = 0u; + timestamps.values.push_back(state); + } + states.values.push_back(timestamps); + } + osrm::json::get(*object, "states") = states; + } + + void add_transition_info(const unsigned prev_t, + const unsigned current_t, + const unsigned prev_state, + const unsigned current_state, + const double prev_viterbi, + const double emission_pr, + const double transition_pr, + const double network_distance, + const double great_circle_distance) + { + // json logger not enabled + if (!logger) + { + return; + } + + osrm::json::Object transistion; + transistion.values["to"] = osrm::json::make_array(current_t, current_state); + transistion.values["properties"] = osrm::json::make_array( + osrm::json::clamp_float(prev_viterbi), osrm::json::clamp_float(emission_pr), + osrm::json::clamp_float(transition_pr), network_distance, great_circle_distance); + + osrm::json::get(*object, "states", prev_t, prev_state, "transitions") + .get>() + .get() + .values.push_back(transistion); + } + + void set_viterbi(const std::vector> &viterbi, + const std::vector> &pruned) + { + // json logger not enabled + if (!logger) + { + return; + } + + for (auto t = 0u; t < viterbi.size(); t++) + { + for (auto s_prime = 0u; s_prime < viterbi[t].size(); ++s_prime) + { + osrm::json::get(*object, "states", t, s_prime, "viterbi") = + osrm::json::clamp_float(viterbi[t][s_prime]); + osrm::json::get(*object, "states", t, s_prime, "pruned") = + static_cast(pruned[t][s_prime]); + } + } + } + + void add_chosen(const unsigned t, const unsigned s) + { + // json logger not enabled + if (!logger) + { + return; + } + + osrm::json::get(*object, "states", t, s, "chosen") = true; + } + + void add_breakage(const std::vector &breakage) + { + // json logger not enabled + if (!logger) + { + return; + } + + osrm::json::get(*object, "breakage") = osrm::json::make_array(breakage); + } + + const osrm::json::Logger *logger; + osrm::json::Value *object; +}; + +#endif // MATCHING_DEBUG_INFO_HPP