From 3905074a81c6ea1bc949212eb6dfcc4833e428c8 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Wed, 2 Nov 2016 19:42:48 +0000 Subject: [PATCH] Port fix for gap logic --- .../routing_algorithms/map_matching.hpp | 250 +++++++++--------- 1 file changed, 128 insertions(+), 122 deletions(-) diff --git a/include/engine/routing_algorithms/map_matching.hpp b/include/engine/routing_algorithms/map_matching.hpp index ba43f6d47..a0029fb2f 100644 --- a/include/engine/routing_algorithms/map_matching.hpp +++ b/include/engine/routing_algorithms/map_matching.hpp @@ -176,24 +176,133 @@ class MapMatching final : public BasicRoutingInterface + max_broken_time; + } + else + { + return t - prev_unbroken_timestamps.back() > MAX_BROKEN_STATES; + } + }(); + + if (!gap_in_trace) + { + BOOST_ASSERT(!prev_unbroken_timestamps.empty()); + const std::size_t 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 = + candidates_list[prev_unbroken_timestamp]; + const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; + + auto ¤t_viterbi = model.viterbi[t]; + auto ¤t_pruned = model.pruned[t]; + auto ¤t_parents = model.parents[t]; + auto ¤t_lengths = model.path_distances[t]; + const auto ¤t_timestamps_list = candidates_list[t]; + const auto ¤t_coordinate = trace_coordinates[t]; + + const auto haversine_distance = util::coordinate_calculation::haversineDistance( + prev_coordinate, current_coordinate); + // assumes minumum of 0.1 m/s + const int duration_upper_bound = + ((haversine_distance + max_distance_delta) * 0.25) * 10; + + // compute d_t for this timestamp and the next one + for (const auto s : util::irange(0UL, prev_viterbi.size())) + { + if (prev_pruned[s]) + { + continue; + } + + for (const auto s_prime : + util::irange(0UL, current_viterbi.size())) + { + const double emission_pr = emission_log_probabilities[t][s_prime]; + double new_value = prev_viterbi[s] + emission_pr; + if (current_viterbi[s_prime] > new_value) + { + continue; + } + + forward_heap.Clear(); + reverse_heap.Clear(); + + double network_distance; + if (super::facade->GetCoreSize() > 0) + { + forward_core_heap.Clear(); + reverse_core_heap.Clear(); + network_distance = super::GetNetworkDistanceWithCore( + forward_heap, + reverse_heap, + forward_core_heap, + reverse_core_heap, + prev_unbroken_timestamps_list[s].phantom_node, + current_timestamps_list[s_prime].phantom_node, + duration_upper_bound); + } + else + { + network_distance = super::GetNetworkDistance( + forward_heap, + reverse_heap, + prev_unbroken_timestamps_list[s].phantom_node, + current_timestamps_list[s_prime].phantom_node); + } + + // get distance diff between loc1/2 and locs/s_prime + const auto d_t = std::abs(network_distance - haversine_distance); + + // very low probability transition -> prune + if (d_t >= max_distance_delta) + { + continue; + } + + const double transition_pr = transition_log_probability(d_t); + new_value += transition_pr; + + if (new_value > current_viterbi[s_prime]) + { + current_viterbi[s_prime] = new_value; + current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s); + current_lengths[s_prime] = network_distance; + current_pruned[s_prime] = false; + model.breakage[t] = false; + } + } + } + + if (model.breakage[t]) + { + // save start of breakage -> we need this as split point + if (t < breakage_begin) + { + breakage_begin = t; + } + + BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); + // remove both ends of the breakage + prev_unbroken_timestamps.pop_back(); + } + else + { + prev_unbroken_timestamps.push_back(t); + } + } + // breakage recover has removed all previous good points - bool trace_split = prev_unbroken_timestamps.empty(); + const bool trace_split = prev_unbroken_timestamps.empty(); - // use temporal information if available to determine a split - if (use_timestamps) - { - trace_split = - trace_split || - (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > - max_broken_time); - } - else - { - trace_split = - trace_split || (t - prev_unbroken_timestamps.back() > MAX_BROKEN_STATES); - } - - if (trace_split) + if (trace_split || gap_in_trace) { std::size_t split_index = t; if (breakage_begin != map_matching::INVALID_STATE) @@ -217,112 +326,9 @@ class MapMatching final : public BasicRoutingInterface new_start >= breakge_begin // we can only reset trace_coordindates.size() times. - t = new_start + 1; - } - - BOOST_ASSERT(!prev_unbroken_timestamps.empty()); - const std::size_t 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 = candidates_list[prev_unbroken_timestamp]; - const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; - - auto ¤t_viterbi = model.viterbi[t]; - auto ¤t_pruned = model.pruned[t]; - auto ¤t_parents = model.parents[t]; - auto ¤t_lengths = model.path_distances[t]; - const auto ¤t_timestamps_list = candidates_list[t]; - const auto ¤t_coordinate = trace_coordinates[t]; - - const auto haversine_distance = util::coordinate_calculation::haversineDistance( - prev_coordinate, current_coordinate); - // assumes minumum of 0.1 m/s - const int duration_uppder_bound = - ((haversine_distance + max_distance_delta) * 0.25) * 10; - - // compute d_t for this timestamp and the next one - for (const auto s : util::irange(0UL, prev_viterbi.size())) - { - if (prev_pruned[s]) - { - continue; - } - - for (const auto s_prime : util::irange(0UL, current_viterbi.size())) - { - - const double emission_pr = emission_log_probabilities[t][s_prime]; - double new_value = prev_viterbi[s] + emission_pr; - if (current_viterbi[s_prime] > new_value) - { - continue; - } - - forward_heap.Clear(); - reverse_heap.Clear(); - - double network_distance; - if (super::facade->GetCoreSize() > 0) - { - forward_core_heap.Clear(); - reverse_core_heap.Clear(); - network_distance = super::GetNetworkDistanceWithCore( - forward_heap, - reverse_heap, - forward_core_heap, - reverse_core_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - duration_uppder_bound); - } - else - { - network_distance = super::GetNetworkDistance( - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node); - } - - // get distance diff between loc1/2 and locs/s_prime - const auto d_t = std::abs(network_distance - haversine_distance); - - // very low probability transition -> prune - if (d_t >= max_distance_delta) - { - continue; - } - - const double transition_pr = transition_log_probability(d_t); - new_value += transition_pr; - - if (new_value > current_viterbi[s_prime]) - { - current_viterbi[s_prime] = new_value; - current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s); - current_lengths[s_prime] = network_distance; - current_pruned[s_prime] = false; - model.breakage[t] = false; - } - } - } - - if (model.breakage[t]) - { - // save start of breakage -> we need this as split point - if (t < breakage_begin) - { - breakage_begin = t; - } - - BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); - // remove both ends of the breakage - prev_unbroken_timestamps.pop_back(); - } - else - { - prev_unbroken_timestamps.push_back(t); + t = new_start; + // note: the head of the loop will call ++t, hence the next + // iteration will actually be on new_start+1 } }