diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 7bbac2afd..9cb19402b 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -26,6 +26,9 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "../data_structures/coordinate_calculation.hpp" #include "../util/simple_logger.hpp" +#include +#include + #include #include #include @@ -87,24 +90,23 @@ template class MapMatching final constexpr static const double log_sigma_z = std::log(sigma_z); constexpr static const double log_2_pi = std::log(2 * M_PI); - // TODO: move to a probability util header and implement as normal distribution - constexpr double emission_probability(const double distance) const + constexpr static double emission_probability(const double distance) { return (1. / (std::sqrt(2. * M_PI) * sigma_z)) * std::exp(-0.5 * std::pow((distance / sigma_z), 2.)); } - constexpr double transition_probability(const float d_t, const float beta) const + constexpr static double transition_probability(const float d_t, const float beta) { return (1. / beta) * std::exp(-d_t / beta); } - inline double log_emission_probability(const double distance) const { - const double normed_distance = distance / sigma_z; - return -0.5 * (log_2_pi + normed_distance * normed_distance) - log_sigma_z; + constexpr static double log_emission_probability(const double distance) + { + return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z; } - inline double log_transition_probability(const float d_t, const float beta) const + constexpr static double log_transition_probability(const float d_t, const float beta) { return -std::log(beta) - d_t / beta; } @@ -228,92 +230,139 @@ template class MapMatching final return distance; } + struct HiddenMarkovModel + { + std::vector> viterbi; + std::vector> parents; + std::vector> path_lengths; + std::vector> pruned; + std::vector breakage; + + const Matching::CandidateLists& timestamp_list; + + constexpr static double IMPOSSIBLE_LOG_PROB = -std::numeric_limits::infinity(); + constexpr static double MINIMAL_LOG_PROB = -std::numeric_limits::max(); + + HiddenMarkovModel(const Matching::CandidateLists& timestamp_list) + : breakage(timestamp_list.size()) + , timestamp_list(timestamp_list) + { + for (const auto& l : timestamp_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(), IMPOSSIBLE_LOG_PROB); + std::fill(parents[t].begin(), parents[t].end(), 0); + 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 < timestamp_list.size()); + + do + { + for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) + { + viterbi[initial_timestamp][s] = log_emission_probability(timestamp_list[initial_timestamp][s].second); + parents[initial_timestamp][s] = s; + pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < MINIMAL_LOG_PROB; + + breakage[initial_timestamp] = breakage[initial_timestamp] && pruned[initial_timestamp][s]; + + } + + ++initial_timestamp; + } while (breakage[initial_timestamp - 1]); + + BOOST_ASSERT(initial_timestamp > 0 && initial_timestamp < viterbi.size()); + --initial_timestamp; + + BOOST_ASSERT(breakage[initial_timestamp] == false); + + return initial_timestamp; + } + + }; + public: MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data) : super(facade), engine_working_data(engine_working_data) { } + void operator()(const Matching::CandidateLists ×tamp_list, const std::vector coordinate_list, std::vector& matched_nodes, float& matched_length, JSON::Object& _debug_info) const { - std::vector breakage(timestamp_list.size(), true); - BOOST_ASSERT(timestamp_list.size() > 0); - // TODO for the viterbi values we actually only need the current and last row - std::vector> viterbi; - std::vector> parents; - std::vector> segment_lengths; - std::vector> pruned; - for (const auto& l : timestamp_list) - { - viterbi.emplace_back(l.size(), -std::numeric_limits::infinity()); - parents.emplace_back(l.size(), 0); - segment_lengths.emplace_back(l.size(), 0); - pruned.emplace_back(l.size(), true); + HiddenMarkovModel model(timestamp_list); - } + unsigned initial_timestamp = model.initialize(0); JSON::Array _debug_states; - for (const auto& l : timestamp_list) + for (unsigned t = 0; t < timestamp_list.size(); t++) { JSON::Array _debug_timestamps; - for (unsigned i = 0; i < l.size(); i++) + for (unsigned s = 0; s < timestamp_list[t].size(); s++) { JSON::Object _debug_state; _debug_state.values["transitions"] = JSON::Array(); - _debug_state.values["coordinate"] = makeJSONArray(l[i].first.location.lat / COORDINATE_PRECISION, - l[i].first.location.lon / COORDINATE_PRECISION); + _debug_state.values["coordinate"] = makeJSONArray(timestamp_list[t][s].first.location.lat / COORDINATE_PRECISION, + timestamp_list[t][s].first.location.lon / COORDINATE_PRECISION); + if (t < initial_timestamp) + { + _debug_state.values["viterbi"] = makeJSONSafe(HiddenMarkovModel::IMPOSSIBLE_LOG_PROB); + _debug_state.values["pruned"] = 0u; + } + else if (t == initial_timestamp) + { + _debug_state.values["viterbi"] = makeJSONSafe(model.viterbi[t][s]); + _debug_state.values["pruned"] = static_cast(model.pruned[t][s]); + } _debug_timestamps.values.push_back(_debug_state); } _debug_states.values.push_back(_debug_timestamps); } - unsigned initial_timestamp = 0; - do - { - for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) - { - // this might need to be squared as pi_s is also defined as the emission - // probability in the paper. - viterbi[initial_timestamp][s] = log_emission_probability(timestamp_list[initial_timestamp][s].second); - parents[initial_timestamp][s] = s; - pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < -std::numeric_limits::max(); - - breakage[initial_timestamp] = breakage[initial_timestamp] && pruned[initial_timestamp][s]; - - _debug_states.values[initial_timestamp] - .get().get().values[s] - .get().get().values["viterbi"] = makeJSONSafe(viterbi[initial_timestamp][s]); - _debug_states.values[initial_timestamp] - .get().get().values[s] - .get().get().values["pruned"] = static_cast(pruned[initial_timestamp][s]); - } - - ++initial_timestamp; - } while (breakage[initial_timestamp - 1]); - - BOOST_ASSERT(initial_timestamp > 0 && initial_timestamp < viterbi.size()); - --initial_timestamp; - - BOOST_ASSERT(breakage[initial_timestamp] == false); - - unsigned prev_unbroken_timestamp = initial_timestamp; + std::vector prev_unbroken_timestamps; + prev_unbroken_timestamps.reserve(timestamp_list.size()); + prev_unbroken_timestamps.push_back(initial_timestamp); for (auto t = initial_timestamp + 1; t < timestamp_list.size(); ++t) { - const auto& prev_viterbi = viterbi[prev_unbroken_timestamp]; - const auto& prev_pruned = pruned[prev_unbroken_timestamp]; + unsigned prev_unbroken_timestamp = prev_unbroken_timestamps.back(); + const auto& prev_viterbi = model.viterbi[prev_unbroken_timestamp]; + const auto& prev_pruned = model.pruned[prev_unbroken_timestamp]; const auto& prev_unbroken_timestamps_list = timestamp_list[prev_unbroken_timestamp]; const auto& prev_coordinate = coordinate_list[prev_unbroken_timestamp]; - auto& current_viterbi = viterbi[t]; - auto& current_pruned = pruned[t]; - auto& current_parents = parents[t]; - auto& current_lengths = segment_lengths[t]; + auto& current_viterbi = model.viterbi[t]; + auto& current_pruned = model.pruned[t]; + auto& current_parents = model.parents[t]; + auto& current_lengths = model.path_lengths[t]; const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; @@ -361,14 +410,13 @@ template class MapMatching final .get().get().values["transitions"] .get().get().values.push_back(_debug_transistion); - if (new_value > current_viterbi[s_prime]) { current_viterbi[s_prime] = new_value; current_parents[s_prime] = s; current_lengths[s_prime] = network_distance; current_pruned[s_prime] = false; - breakage[t] = false; + model.breakage[t] = false; } } } @@ -383,23 +431,45 @@ template class MapMatching final .get().get().values["pruned"] = static_cast(current_pruned[s_prime]); } - if (!breakage[t]) + if (model.breakage[t]) { - prev_unbroken_timestamp = t; + if (prev_unbroken_timestamps.size() > 1) + { + // remove both ends of the breakge + prev_unbroken_timestamps.pop_back(); + } + // we reached the beginning of the trace, discard the whole beginning + else + { + model.clear(t); + model.initialize(t); + } + } + else + { + prev_unbroken_timestamps.push_back(t); } } + if (prev_unbroken_timestamps.size() < 1) + { + return; + } + + unsigned last_unbroken_timestamp = prev_unbroken_timestamps.back(); + // loop through the columns, and only compare the last entry - auto max_element_iter = std::max_element(viterbi[prev_unbroken_timestamp].begin(), viterbi[prev_unbroken_timestamp].end()); - auto parent_index = std::distance(viterbi[prev_unbroken_timestamp].begin(), max_element_iter); + auto max_element_iter = std::max_element(model.viterbi[last_unbroken_timestamp].begin(), + model.viterbi[last_unbroken_timestamp].end()); + auto parent_index = std::distance(model.viterbi[last_unbroken_timestamp].begin(), max_element_iter); std::deque> reconstructed_indices; - for (auto i = prev_unbroken_timestamp; i > initial_timestamp; --i) + for (auto i = last_unbroken_timestamp; i > initial_timestamp; --i) { - if (breakage[i]) + if (model.breakage[i]) continue; reconstructed_indices.emplace_front(i, parent_index); - parent_index = parents[i][parent_index]; + parent_index = model.parents[i][parent_index]; } reconstructed_indices.emplace_front(initial_timestamp, parent_index); @@ -411,7 +481,7 @@ template class MapMatching final auto location_index = reconstructed_indices[i].second; matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; - matched_length += segment_lengths[timestamp_index][location_index]; + matched_length += model.path_lengths[timestamp_index][location_index]; _debug_states.values[timestamp_index] .get().get().values[location_index] @@ -419,7 +489,7 @@ template class MapMatching final } JSON::Array _debug_breakage; - for (auto b : breakage) { + for (auto b : model.breakage) { _debug_breakage.values.push_back(static_cast(b)); }