From 2259bce05f03758136afa0276f37858372398b1b Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 23 Sep 2014 18:46:14 +0200 Subject: [PATCH] Add skeleton code for matching --- data_structures/search_engine.hpp | 9 +- data_structures/static_rtree.hpp | 172 +++++----- library/osrm_impl.cpp | 2 + plugins/map_matching.hpp | 107 ++++++ routing_algorithms/map_matching.hpp | 317 ++++++++++++++++++ server/data_structures/datafacade_base.hpp | 5 + .../data_structures/internal_datafacade.hpp | 15 + server/data_structures/shared_datafacade.hpp | 15 + 8 files changed, 552 insertions(+), 90 deletions(-) create mode 100644 plugins/map_matching.hpp create mode 100644 routing_algorithms/map_matching.hpp diff --git a/data_structures/search_engine.hpp b/data_structures/search_engine.hpp index 24e550767..7e47b1f5d 100644 --- a/data_structures/search_engine.hpp +++ b/data_structures/search_engine.hpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "search_engine_data.hpp" #include "../routing_algorithms/alternative_path.hpp" #include "../routing_algorithms/many_to_many.hpp" +#include "../routing_algorithms/map_matching.hpp" #include "../routing_algorithms/shortest_path.hpp" #include @@ -45,10 +46,14 @@ template class SearchEngine ShortestPathRouting shortest_path; AlternativeRouting alternative_path; ManyToManyRouting distance_table; + MapMatching map_matching; explicit SearchEngine(DataFacadeT *facade) - : facade(facade), shortest_path(facade, engine_working_data), - alternative_path(facade, engine_working_data), distance_table(facade, engine_working_data) + : facade(facade), + shortest_path(facade, engine_working_data), + alternative_path(facade, engine_working_data), + distance_table(facade, engine_working_data), + map_matching(facade, engine_working_data) { static_assert(!std::is_pointer::value, "don't instantiate with ptr type"); static_assert(std::is_object::value, diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index a0ce38a1e..45109df73 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -745,6 +745,7 @@ class StaticRTree // check if it is smaller than what we had before float current_ratio = 0.f; FixedPointCoordinate foot_point_coordinate_on_segment; + // const float current_perpendicular_distance = coordinate_calculation::perpendicular_distance_from_projected_coordinate( m_coordinate_list->at(current_segment.u), @@ -796,20 +797,24 @@ class StaticRTree return !result_phantom_node_vector.empty(); } - // implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree bool IncrementalFindPhantomNodeForCoordinateWithDistance( const FixedPointCoordinate &input_coordinate, std::vector> &result_phantom_node_vector, - const unsigned number_of_results, - const unsigned max_checked_segments = 4 * LEAF_NODE_SIZE) + const unsigned max_number_of_phantom_nodes, + const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE) { - std::vector min_found_distances(number_of_results, - std::numeric_limits::max()); + unsigned inspected_elements = 0; + unsigned number_of_elements_from_big_cc = 0; + unsigned number_of_elements_from_tiny_cc = 0; - unsigned number_of_results_found_in_big_cc = 0; - unsigned number_of_results_found_in_tiny_cc = 0; + unsigned pruned_elements = 0; - unsigned inspected_segments = 0; + std::pair projected_coordinate = { + mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION), + input_coordinate.lon / COORDINATE_PRECISION}; + + // upper bound pruning technique + upper_bound pruning_bound(max_number_of_phantom_nodes); // initialize queue with root element std::priority_queue traversal_queue; @@ -820,42 +825,43 @@ class StaticRTree const IncrementalQueryCandidate current_query_node = traversal_queue.top(); traversal_queue.pop(); - const float current_min_dist = min_found_distances[number_of_results - 1]; - - if (current_query_node.min_dist > current_min_dist) - { - continue; - } - - if (current_query_node.RepresentsTreeNode()) - { + if (current_query_node.node.template is()) + { // current object is a tree node const TreeNode ¤t_tree_node = current_query_node.node.template get(); if (current_tree_node.child_is_on_disk) { LeafNode current_leaf_node; LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node); - // Add all objects from leaf into queue - for (uint32_t i = 0; i < current_leaf_node.object_count; ++i) + + // current object represents a block on disk + for (const auto i : osrm::irange(0u, current_leaf_node.object_count)) { const auto ¤t_edge = current_leaf_node.objects[i]; - const float current_perpendicular_distance = - coordinate_calculation::perpendicular_distance( + const float current_perpendicular_distance = coordinate_calculation:: + perpendicular_distance_from_projected_coordinate( m_coordinate_list->at(current_edge.u), - m_coordinate_list->at(current_edge.v), input_coordinate); + m_coordinate_list->at(current_edge.v), input_coordinate, + projected_coordinate); // distance must be non-negative - BOOST_ASSERT(0. <= current_perpendicular_distance); + BOOST_ASSERT(0.f <= current_perpendicular_distance); - if (current_perpendicular_distance < current_min_dist) + if (pruning_bound.get() >= current_perpendicular_distance || + current_edge.is_in_tiny_cc()) { + pruning_bound.insert(current_perpendicular_distance); traversal_queue.emplace(current_perpendicular_distance, current_edge); } + else + { + ++pruned_elements; + } } } else { - // for each child mbr - for (uint32_t i = 0; i < current_tree_node.child_count; ++i) + // for each child mbr get a lower bound and enqueue it + for (const auto i : osrm::irange(0u, current_tree_node.child_count)) { const int32_t child_id = current_tree_node.children[i]; const TreeNode &child_tree_node = m_search_tree[child_id]; @@ -863,99 +869,89 @@ class StaticRTree child_tree_node.minimum_bounding_rectangle; const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate); + BOOST_ASSERT(0.f <= lower_bound_to_element); - // TODO - enough elements found, i.e. nearest distance > maximum distance? - // ie. some measure of 'confidence of accuracy' - - // check if it needs to be explored by mindist - if (lower_bound_to_element < current_min_dist) - { - traversal_queue.emplace(lower_bound_to_element, child_tree_node); - } + traversal_queue.emplace(lower_bound_to_element, child_tree_node); } - // SimpleLogger().Write(logDEBUG) << "added " << current_tree_node.child_count - // << " mbrs into queue of " << traversal_queue.size(); } } else - { - ++inspected_segments; + { // current object is a leaf node + ++inspected_elements; // inspecting an actual road segment const EdgeDataT ¤t_segment = current_query_node.node.template get(); - // don't collect too many results from small components - if (number_of_results_found_in_big_cc == number_of_results && - !current_segment.is_in_tiny_cc) - { - continue; - } - - // don't collect too many results from big components - if (number_of_results_found_in_tiny_cc == number_of_results && - current_segment.is_in_tiny_cc) + // continue searching for the first segment from a big component + if (number_of_elements_from_big_cc == 0 && + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes && + current_segment.is_in_tiny_cc()) { continue; } // check if it is smaller than what we had before - float current_ratio = 0.; + float current_ratio = 0.f; FixedPointCoordinate foot_point_coordinate_on_segment; + const float current_perpendicular_distance = - coordinate_calculation::perpendicular_distance( - m_coordinate_list->at(current_segment.u), - m_coordinate_list->at(current_segment.v), input_coordinate, - foot_point_coordinate_on_segment, current_ratio); + coordinate_calculation::perpendicular_distance_from_projected_coordinate( + m_coordinate_list->at(current_segment.u), + m_coordinate_list->at(current_segment.v), input_coordinate, + projected_coordinate, foot_point_coordinate_on_segment, current_ratio); - BOOST_ASSERT(0. <= current_perpendicular_distance); + // store phantom node in result vector + result_phantom_node_vector.emplace_back( + PhantomNode( current_segment.forward_edge_based_node_id, + current_segment.reverse_edge_based_node_id, current_segment.name_id, + current_segment.forward_weight, current_segment.reverse_weight, + current_segment.forward_offset, current_segment.reverse_offset, + current_segment.packed_geometry_id, current_segment.component_id, + foot_point_coordinate_on_segment, current_segment.fwd_segment_position, + current_segment.forward_travel_mode, current_segment.backward_travel_mode), + current_perpendicular_distance); - if ((current_perpendicular_distance < current_min_dist) && - !osrm::epsilon_compare(current_perpendicular_distance, current_min_dist)) - { - // store phantom node in result vector - result_phantom_node_vector.emplace_back(current_segment, - foot_point_coordinate_on_segment, - current_perpendicular_distance); + // Hack to fix rounding errors and wandering via nodes. + FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first); - // Hack to fix rounding errors and wandering via nodes. - FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back()); + // set forward and reverse weights on the phantom node + SetForwardAndReverseWeightsOnPhantomNode(current_segment, + result_phantom_node_vector.back().first); - // set forward and reverse weights on the phantom node - SetForwardAndReverseWeightsOnPhantomNode(current_segment, - result_phantom_node_vector.back()); - - // do we have results only in a small scc - if (current_segment.is_in_tiny_cc) - { - ++number_of_results_found_in_tiny_cc; - } - else - { - // found an element in a large component - min_found_distances[number_of_results_found_in_big_cc] = - current_perpendicular_distance; - ++number_of_results_found_in_big_cc; - // SimpleLogger().Write(logDEBUG) << std::setprecision(8) << - // foot_point_coordinate_on_segment << " at " << - // current_perpendicular_distance; - } + // update counts on what we found from which result class + if (current_segment.is_in_tiny_cc()) + { // found an element in tiny component + ++number_of_elements_from_tiny_cc; + } + else + { // found an element in a big component + ++number_of_elements_from_big_cc; } } - // TODO add indicator to prune if maxdist > threshold - if (number_of_results == number_of_results_found_in_big_cc || - inspected_segments >= max_checked_segments) + // stop the search by flushing the queue + if ((result_phantom_node_vector.size() >= max_number_of_phantom_nodes && + number_of_elements_from_big_cc > 0) || + inspected_elements >= max_checked_elements) { - // SimpleLogger().Write(logDEBUG) << "flushing queue of " << traversal_queue.size() - // << " elements"; - // work-around for traversal_queue.clear(); traversal_queue = std::priority_queue{}; } } + // SimpleLogger().Write() << "result_phantom_node_vector.size(): " << + // result_phantom_node_vector.size(); + // SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes; + // SimpleLogger().Write() << "number_of_elements_from_big_cc: " << + // number_of_elements_from_big_cc; + // SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " << + // number_of_elements_from_tiny_cc; + // SimpleLogger().Write() << "inspected_elements: " << inspected_elements; + // SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements; + // SimpleLogger().Write() << "pruned_elements: " << pruned_elements; return !result_phantom_node_vector.empty(); } + bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, PhantomNode &result_phantom_node, const unsigned zoom_level) diff --git a/library/osrm_impl.cpp b/library/osrm_impl.cpp index 71ea1fb1d..9ea009912 100644 --- a/library/osrm_impl.cpp +++ b/library/osrm_impl.cpp @@ -42,6 +42,7 @@ class named_mutex; #include "../plugins/nearest.hpp" #include "../plugins/timestamp.hpp" #include "../plugins/viaroute.hpp" +#include "../plugins/map_matching.hpp" #include "../server/data_structures/datafacade_base.hpp" #include "../server/data_structures/internal_datafacade.hpp" #include "../server/data_structures/shared_barriers.hpp" @@ -81,6 +82,7 @@ OSRM_impl::OSRM_impl(libosrm_config &lib_config) RegisterPlugin(new HelloWorldPlugin()); RegisterPlugin(new LocatePlugin>(query_data_facade)); RegisterPlugin(new NearestPlugin>(query_data_facade)); + RegisterPlugin(new MapMatchingPlugin>(query_data_facade)); RegisterPlugin(new TimestampPlugin>(query_data_facade)); RegisterPlugin(new ViaRoutePlugin>(query_data_facade)); } diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp new file mode 100644 index 000000000..8b997d881 --- /dev/null +++ b/plugins/map_matching.hpp @@ -0,0 +1,107 @@ +/* + open source routing machine + Copyright (C) Dennis Luxen, others 2010 + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU AFFERO General Public License as published by +the Free Software Foundation; either version 3 of the License, or +any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Affero General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +or see http://www.gnu.org/licenses/agpl.txt. + */ + +#ifndef MAP_MATCHING_PLUGIN_H +#define MAP_MATCHING_PLUGIN_H + +#include "plugin_base.hpp" + +#include "../algorithms/object_encoder.hpp" +#include "../util/integer_range.hpp" +#include "../data_structures/search_engine.hpp" +#include "../routing_algorithms/map_matching.hpp" +#include "../util/simple_logger.hpp" +#include "../util/string_util.hpp" + +#include + +#include +#include +#include +#include + +template class MapMatchingPlugin : public BasePlugin +{ + private: + std::shared_ptr> search_engine_ptr; + + public: + MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade) + { + search_engine_ptr = std::make_shared>(facade); + } + + virtual ~MapMatchingPlugin() { search_engine_ptr.reset(); } + + const std::string GetDescriptor() const final { return descriptor_string; } + + int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final + { + // check number of parameters + + SimpleLogger().Write() << "1"; + if (!check_all_coordinates(route_parameters.coordinates)) + { + return 400; + } + + SimpleLogger().Write() << "2"; + + InternalRouteResult raw_route; + Matching::CandidateLists candidate_lists; + candidate_lists.resize(route_parameters.coordinates.size()); + + SimpleLogger().Write() << "3"; + // fetch 10 candidates for each given coordinate + for (const auto current_coordinate : osrm::irange(0, candidate_lists.size())) + { + if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance( + route_parameters.coordinates[current_coordinate], + candidate_lists[current_coordinate], + 10)) + { + return 400; + } + + while (candidate_lists[current_coordinate].size() < 10) + { + // TODO: add dummy candidates, if any are missing + // TODO: add factory method to get an invalid PhantomNode/Distance pair + } + } + SimpleLogger().Write() << "4"; + + // call the actual map matching + search_engine_ptr->map_matching(10, candidate_lists, route_parameters.coordinates, raw_route); + + if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length) + { + SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found"; + } + + return 200; + } + + private: + std::string descriptor_string; + DataFacadeT *facade; +}; + +#endif /* MAP_MATCHING_PLUGIN_H */ diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp new file mode 100644 index 000000000..80152f1ef --- /dev/null +++ b/routing_algorithms/map_matching.hpp @@ -0,0 +1,317 @@ +/* + open source routing machine + Copyright (C) Dennis Luxen, others 2010 + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU AFFERO General Public License as published by +the Free Software Foundation; either version 3 of the License, or +any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Affero General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +or see http://www.gnu.org/licenses/agpl.txt. + */ + +#ifndef MAP_MATCHING_H +#define MAP_MATCHING_H + +#include "routing_base.hpp" + +#include "../data_structures/coordinate_calculation.hpp" +#include "../util/simple_logger.hpp" +#include "../util/container.hpp" + +#include +#include +#include + +namespace Matching +{ +typedef std::vector> CandidateList; +typedef std::vector CandidateLists; +typedef std::pair PhantomNodesWithProbability; +} + +// implements a hidden markov model map matching algorithm +template class MapMatching final + : public BasicRoutingInterface> +{ + using super = BasicRoutingInterface>; + using QueryHeap = SearchEngineData::QueryHeap; + SearchEngineData &engine_working_data; + + constexpr static const double sigma_z = 4.07; + + constexpr double emission_probability(const double distance) const + { + return (1. / (std::sqrt(2. * M_PI) * sigma_z)) * + std::exp(-0.5 * std::pow((distance / sigma_z), 2.)); + } + + constexpr double log_probability(const double probability) const + { + return std::log2(probability); + } + + // TODO: needs to be estimated from the input locations + //constexpr static const double beta = 1.; + // samples/min and beta + // 1 0.49037673 + // 2 0.82918373 + // 3 1.24364564 + // 4 1.67079581 + // 5 2.00719298 + // 6 2.42513007 + // 7 2.81248831 + // 8 3.15745473 + // 9 3.52645392 + // 10 4.09511775 + // 11 4.67319795 + // 21 12.55107715 + // 12 5.41088180 + // 13 6.47666590 + // 14 6.29010734 + // 15 7.80752112 + // 16 8.09074504 + // 17 8.08550528 + // 18 9.09405065 + // 19 11.09090603 + // 20 11.87752824 + // 21 12.55107715 + // 22 15.82820829 + // 23 17.69496773 + // 24 18.07655652 + // 25 19.63438911 + // 26 25.40832185 + // 27 23.76001877 + // 28 28.43289797 + // 29 32.21683062 + // 30 34.56991141 + + constexpr double transition_probability(const float d_t, const float beta) const + { + return (1. / beta) * std::exp(-d_t / beta); + } + + // deprecated + // translates a distance into how likely it is an input + // double DistanceToProbability(const double distance) const + // { + // if (0. > distance) + // { + // return 0.; + // } + // return 1. - 1. / (1. + exp((-distance + 35.) / 6.)); + // } + + double get_beta(const unsigned state_size, + const Matching::CandidateLists ×tamp_list, + const std::vector coordinate_list) const + { + std::vector d_t_list, median_select_d_t_list; + for (auto t = 1; t < timestamp_list.size(); ++t) + { + for (auto s = 0; s < state_size; ++s) + { + d_t_list.push_back(get_distance_difference(coordinate_list[t - 1], + coordinate_list[t], + timestamp_list[t - 1][s].first, + timestamp_list[t][s].first)); + median_select_d_t_list.push_back(d_t_list.back()); + } + } + + std::nth_element(median_select_d_t_list.begin(), + median_select_d_t_list.begin() + median_select_d_t_list.size() / 2, + median_select_d_t_list.end()); + const auto median_d_t = median_select_d_t_list[median_select_d_t_list.size() / 2]; + + return (1. / std::log(2)) * median_d_t; + } + + + double get_distance_difference(const FixedPointCoordinate &location1, + const FixedPointCoordinate &location2, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom) const + { + // great circle distance of two locations - median/avg dist table(candidate list1/2) + const EdgeWeight network_distance = get_network_distance(source_phantom, target_phantom); + const auto great_circle_distance = + coordinate_calculation::great_circle_distance(location1, location2); + + if (great_circle_distance > network_distance) + { + return great_circle_distance - network_distance; + } + return network_distance - great_circle_distance; + } + + EdgeWeight get_network_distance(const PhantomNode &source_phantom, + const PhantomNode &target_phantom) const + { + EdgeWeight upper_bound = INVALID_EDGE_WEIGHT; + NodeID middle_node = SPECIAL_NODEID; + EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset()); + edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset()); + + engine_working_data.InitializeOrClearFirstThreadLocalStorage( + super::facade->GetNumberOfNodes()); + engine_working_data.InitializeOrClearSecondThreadLocalStorage( + super::facade->GetNumberOfNodes()); + + QueryHeap &forward_heap = *(engine_working_data.forward_heap_1); + QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1); + + if (source_phantom.forward_node_id != SPECIAL_NODEID) + { + forward_heap.Insert(source_phantom.forward_node_id, + -source_phantom.GetForwardWeightPlusOffset(), + source_phantom.forward_node_id); + } + if (source_phantom.reverse_node_id != SPECIAL_NODEID) + { + forward_heap.Insert(source_phantom.reverse_node_id, + -source_phantom.GetReverseWeightPlusOffset(), + source_phantom.reverse_node_id); + } + + if (target_phantom.forward_node_id != SPECIAL_NODEID) + { + reverse_heap.Insert(target_phantom.forward_node_id, + target_phantom.GetForwardWeightPlusOffset(), + target_phantom.forward_node_id); + } + if (target_phantom.reverse_node_id != SPECIAL_NODEID) + { + reverse_heap.Insert(target_phantom.reverse_node_id, + target_phantom.GetReverseWeightPlusOffset(), + target_phantom.reverse_node_id); + } + + // search from s and t till new_min/(1+epsilon) > length_of_shortest_path + while (0 < (forward_heap.Size() + reverse_heap.Size())) + { + if (0 < forward_heap.Size()) + { + super::RoutingStep( + forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, true); + } + if (0 < reverse_heap.Size()) + { + super::RoutingStep( + reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, false); + } + } + return upper_bound; + } + + public: + MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data) + : super(facade), engine_working_data(engine_working_data) + { + } + + void operator()(const unsigned state_size, + const Matching::CandidateLists ×tamp_list, + const std::vector coordinate_list, + InternalRouteResult &raw_route_data) const + { + BOOST_ASSERT(state_size != std::numeric_limits::max()); + BOOST_ASSERT(state_size != 0); + SimpleLogger().Write() << "matching starts with " << timestamp_list.size() << " locations"; + + SimpleLogger().Write() << "state_size: " << state_size; + + std::vector> viterbi(state_size, + std::vector(timestamp_list.size() + 1, 0)); + std::vector> parent( + state_size, std::vector(timestamp_list.size() + 1, 0)); + + SimpleLogger().Write() << "a"; + + for (auto s = 0; s < state_size; ++s) + { + SimpleLogger().Write() << "initializing s: " << s << "/" << state_size; + SimpleLogger().Write() + << " distance: " << timestamp_list[0][s].second << " at " + << timestamp_list[0][s].first.location << " prob " << std::setprecision(10) + << emission_probability(timestamp_list[0][s].second) << " logprob " + << log_probability(emission_probability(timestamp_list[0][s].second)); + // TODO: implement + const double emission_pr = 0.; + viterbi[s][0] = emission_pr; + parent[s][0] = s; + } + SimpleLogger().Write() << "b"; + + // attention, this call is relatively expensive + const auto beta = get_beta(state_size, timestamp_list, coordinate_list); + + for (auto t = 1; t < timestamp_list.size(); ++t) + { + // compute d_t for this timestamp and the next one + for (auto s = 0; s < state_size; ++s) + { + for (auto s_prime = 0; s_prime < state_size; ++s_prime) + { + // how likely is candidate s_prime at time t to be emitted? + const double emission_pr = emission_probability(timestamp_list[t][s_prime].second); + + // get distance diff between loc1/2 and locs/s_prime + const auto d_t = get_distance_difference(coordinate_list[t-1], + coordinate_list[t], + timestamp_list[t-1][s].first, + timestamp_list[t][s_prime].first); + + // plug probabilities together. TODO: change to addition for logprobs + const double transition_pr = transition_probability(beta, d_t); + const double new_value = viterbi[s][t] * emission_pr * transition_pr; + if (new_value > viterbi[s_prime][t]) + { + viterbi[s_prime][t] = new_value; + parent[s_prime][t] = s; + } + } + } + } + SimpleLogger().Write() << "c"; + SimpleLogger().Write() << "timestamps: " << timestamp_list.size(); + const auto number_of_timestamps = timestamp_list.size(); + const auto max_element_iter = std::max_element(viterbi[number_of_timestamps].begin(), + viterbi[number_of_timestamps].end()); + auto parent_index = std::distance(max_element_iter, viterbi[number_of_timestamps].begin()); + std::deque reconstructed_indices; + + SimpleLogger().Write() << "d"; + + for (auto i = number_of_timestamps - 1; i > 0; --i) + { + SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ; + reconstructed_indices.push_front(parent_index); + parent_index = parent[parent_index][i]; + } + SimpleLogger().Write() << "[0] parent: " << parent_index; + reconstructed_indices.push_front(parent_index); + + SimpleLogger().Write() << "e"; + + for (auto i = 0; i < reconstructed_indices.size(); ++i) + { + auto location_index = reconstructed_indices[i]; + SimpleLogger().Write() << std::setprecision(8) << "location " << coordinate_list[i] << " to " << timestamp_list[i][location_index].first.location; + } + + SimpleLogger().Write() << "f, done"; + } +}; + +//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM GIS + +#endif /* MAP_MATCHING_H */ diff --git a/server/data_structures/datafacade_base.hpp b/server/data_structures/datafacade_base.hpp index 0a2d22025..0c9edb2eb 100644 --- a/server/data_structures/datafacade_base.hpp +++ b/server/data_structures/datafacade_base.hpp @@ -105,6 +105,11 @@ template class BaseDataFacade IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, PhantomNode &resulting_phantom_node) = 0; + virtual bool + IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + std::vector> &resulting_phantom_node_vector, + const unsigned number_of_results) = 0; + virtual unsigned GetCheckSum() const = 0; virtual unsigned GetNameIndexFromEdgeID(const unsigned id) const = 0; diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp index 746402418..5ab769bb0 100644 --- a/server/data_structures/internal_datafacade.hpp +++ b/server/data_structures/internal_datafacade.hpp @@ -398,6 +398,7 @@ template class InternalDataFacade final : public BaseDataFacad BOOST_ASSERT(!resulting_phantom_node_vector.empty()); resulting_phantom_node = resulting_phantom_node_vector.front(); } + return result; } @@ -415,6 +416,20 @@ template class InternalDataFacade final : public BaseDataFacad input_coordinate, resulting_phantom_node_vector, number_of_results); } + bool + IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + std::vector> &resulting_phantom_node_vector, + const unsigned number_of_results) final + { + if (!m_static_rtree.get()) + { + LoadRTree(); + } + + return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance( + input_coordinate, resulting_phantom_node_vector, number_of_results); + } + unsigned GetCheckSum() const override final { return m_check_sum; } unsigned GetNameIndexFromEdgeID(const unsigned id) const override final diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp index fcec478b2..eaf9386f7 100644 --- a/server/data_structures/shared_datafacade.hpp +++ b/server/data_structures/shared_datafacade.hpp @@ -386,6 +386,7 @@ template class SharedDataFacade final : public BaseDataFacade< BOOST_ASSERT(!resulting_phantom_node_vector.empty()); resulting_phantom_node = resulting_phantom_node_vector.front(); } + return result; } @@ -403,6 +404,20 @@ template class SharedDataFacade final : public BaseDataFacade< input_coordinate, resulting_phantom_node_vector, number_of_results); } + bool + IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + std::vector> &resulting_phantom_node_vector, + const unsigned number_of_results) final + { + if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first) + { + LoadRTree(); + } + + return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance( + input_coordinate, resulting_phantom_node_vector, number_of_results); + } + unsigned GetCheckSum() const override final { return m_check_sum; } unsigned GetNameIndexFromEdgeID(const unsigned id) const override final