From 2259bce05f03758136afa0276f37858372398b1b Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 23 Sep 2014 18:46:14 +0200 Subject: [PATCH 01/67] 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 From 3a5e41ed91beecd7fbfda0dfe1c37de338b709e8 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 8 Dec 2014 14:46:31 -0800 Subject: [PATCH 02/67] Implement missing matching pieces --- data_structures/static_rtree.hpp | 23 +- plugins/map_matching.hpp | 103 +++++-- routing_algorithms/map_matching.hpp | 287 +++++++++++++----- server/data_structures/datafacade_base.hpp | 4 +- .../data_structures/internal_datafacade.hpp | 6 +- server/data_structures/shared_datafacade.hpp | 6 +- 6 files changed, 317 insertions(+), 112 deletions(-) diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index 45109df73..e2a93c8e3 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -642,10 +642,6 @@ class StaticRTree return result_coordinate.is_valid(); } - // implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree - // - searches for k elements nearest elements - // - continues to find the k+1st element from a big component if k elements - // come from tiny components bool IncrementalFindPhantomNodeForCoordinate( const FixedPointCoordinate &input_coordinate, std::vector &result_phantom_node_vector, @@ -797,9 +793,17 @@ class StaticRTree return !result_phantom_node_vector.empty(); } + /** + * Returns elements within max_distance. + * If the minium of elements could not be found in the search radius, widen + * it until the minimum can be satisfied. + * At the number of returned nodes is capped at the given maximum. + */ bool IncrementalFindPhantomNodeForCoordinateWithDistance( const FixedPointCoordinate &input_coordinate, std::vector> &result_phantom_node_vector, + const double max_distance, + const unsigned min_number_of_phantom_nodes, const unsigned max_number_of_phantom_nodes, const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE) { @@ -884,7 +888,7 @@ class StaticRTree // 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 && + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes-1 && current_segment.is_in_tiny_cc()) { continue; @@ -900,6 +904,14 @@ class StaticRTree m_coordinate_list->at(current_segment.v), input_coordinate, projected_coordinate, foot_point_coordinate_on_segment, current_ratio); + if (number_of_elements_from_big_cc > 0 + && (number_of_elements_from_tiny_cc + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes + || current_perpendicular_distance >= max_distance)) + { + traversal_queue = std::priority_queue{}; + continue; + } + // store phantom node in result vector result_phantom_node_vector.emplace_back( PhantomNode( current_segment.forward_edge_based_node_id, @@ -951,7 +963,6 @@ class StaticRTree return !result_phantom_node_vector.empty(); } - bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, PhantomNode &result_phantom_node, const unsigned zoom_level) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 8b997d881..7be46ed49 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -1,5 +1,5 @@ -/* - open source routing machine + /* + open source routing machine Copyright (C) Dennis Luxen, others 2010 This program is free software; you can redistribute it and/or modify @@ -29,6 +29,9 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "../routing_algorithms/map_matching.hpp" #include "../util/simple_logger.hpp" #include "../util/string_util.hpp" +#include "../descriptors/descriptor_base.hpp" +#include "../descriptors/gpx_descriptor.hpp" +#include "../descriptors/json_descriptor.hpp" #include @@ -40,11 +43,16 @@ or see http://www.gnu.org/licenses/agpl.txt. template class MapMatchingPlugin : public BasePlugin { private: + std::unordered_map descriptor_table; std::shared_ptr> search_engine_ptr; public: MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade) { + descriptor_table.emplace("json", 0); + descriptor_table.emplace("gpx", 1); + // descriptor_table.emplace("geojson", 2); + // search_engine_ptr = std::make_shared>(facade); } @@ -55,47 +63,98 @@ template class MapMatchingPlugin : public BasePlugin 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())) + double last_distance = coordinate_calculation::great_circle_distance( + route_parameters.coordinates[0], + route_parameters.coordinates[1]); + for (const auto current_coordinate : osrm::irange(0, route_parameters.coordinates.size())) { + if (0 < current_coordinate) + last_distance = coordinate_calculation::great_circle_distance( + route_parameters.coordinates[current_coordinate - 1], + route_parameters.coordinates[current_coordinate]); + + std::cout << "Searching: " << current_coordinate << std::endl; + std::vector> candidates; if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance( route_parameters.coordinates[current_coordinate], - candidate_lists[current_coordinate], - 10)) + candidates, + last_distance, + 5, + 20)) { - return 400; + std::cout << "Nothing found for " << current_coordinate << std::endl; + continue; } - 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 - } + candidate_lists.push_back(candidates); + + std::cout << current_coordinate << " (" << (last_distance / 2.0) << ") : " + << candidates.size() << std::endl; + + BOOST_ASSERT(candidate_lists[current_coordinate].size() == 10); + } + + if (2 > candidate_lists.size()) + { + return 400; } - SimpleLogger().Write() << "4"; // call the actual map matching - search_engine_ptr->map_matching(10, candidate_lists, route_parameters.coordinates, raw_route); + std::vector matched_nodes; + JSON::Object debug_info; + search_engine_ptr->map_matching(candidate_lists, route_parameters.coordinates, matched_nodes, debug_info); - if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length) + PhantomNodes current_phantom_node_pair; + for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) { - SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found"; + current_phantom_node_pair.source_phantom = matched_nodes[i]; + current_phantom_node_pair.target_phantom = matched_nodes[i + 1]; + raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); } + search_engine_ptr->shortest_path( + raw_route.segment_end_coordinates, route_parameters.uturns, raw_route); + + DescriptorConfig descriptor_config; + + auto iter = descriptor_table.find(route_parameters.output_format); + unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); + + descriptor_config.zoom_level = route_parameters.zoom_level; + descriptor_config.instructions = route_parameters.print_instructions; + descriptor_config.geometry = route_parameters.geometry; + descriptor_config.encode_geometry = route_parameters.compression; + + std::shared_ptr> descriptor; + switch (descriptor_type) + { + // case 0: + // descriptor = std::make_shared>(); + // break; + case 1: + descriptor = std::make_shared>(facade); + break; + // case 2: + // descriptor = std::make_shared>(); + // break; + default: + descriptor = std::make_shared>(facade); + break; + } + + descriptor->SetConfig(descriptor_config); + descriptor->Run(raw_route, json_result); + + json_result.values["debug"] = debug_info; + return 200; } diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 80152f1ef..1b28f8a30 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -31,6 +31,38 @@ or see http://www.gnu.org/licenses/agpl.txt. #include #include +#include + +template +T makeJSONSave(T d) +{ + if (std::isnan(d) || std::numeric_limits::infinity() == d) { + return std::numeric_limits::max(); + } + if (-std::numeric_limits::infinity() == d) { + return -std::numeric_limits::max(); + } + + return d; +} + +void appendToJSONArray(JSON::Array& a) { } + +template +void appendToJSONArray(JSON::Array& a, T value, Args... args) +{ + a.values.emplace_back(value); + appendToJSONArray(a, args...); +} + +template +JSON::Array makeJSONArray(Args... args) +{ + JSON::Array a; + appendToJSONArray(a, args...); + return a; +} + namespace Matching { typedef std::vector> CandidateList; @@ -115,9 +147,9 @@ template class MapMatching final 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 t = 1u; t < timestamp_list.size(); ++t) { - for (auto s = 0; s < state_size; ++s) + for (auto s = 0u; s < state_size; ++s) { d_t_list.push_back(get_distance_difference(coordinate_list[t - 1], coordinate_list[t], @@ -142,7 +174,7 @@ template class MapMatching final 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 network_distance = get_network_distance(source_phantom, target_phantom); const auto great_circle_distance = coordinate_calculation::great_circle_distance(location1, location2); @@ -153,7 +185,7 @@ template class MapMatching final return network_distance - great_circle_distance; } - EdgeWeight get_network_distance(const PhantomNode &source_phantom, + double get_network_distance(const PhantomNode &source_phantom, const PhantomNode &target_phantom) const { EdgeWeight upper_bound = INVALID_EDGE_WEIGHT; @@ -209,7 +241,31 @@ template class MapMatching final reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, false); } } - return upper_bound; + + double distance = std::numeric_limits::max(); + if (upper_bound != INVALID_EDGE_WEIGHT) + { + std::vector packed_leg; + super::RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg); + std::vector unpacked_path; + PhantomNodes nodes; + nodes.source_phantom = source_phantom; + nodes.target_phantom = target_phantom; + super::UnpackPath(packed_leg, nodes, unpacked_path); + + FixedPointCoordinate previous_coordinate = source_phantom.location; + FixedPointCoordinate current_coordinate; + distance = 0; + for (const auto& p : unpacked_path) + { + current_coordinate = super::facade->GetCoordinateOfNode(p.node); + distance += coordinate_calculation::great_circle_distance(previous_coordinate, current_coordinate); + previous_coordinate = current_coordinate; + } + distance += coordinate_calculation::great_circle_distance(previous_coordinate, target_phantom.location); + } + + return distance; } public: @@ -218,97 +274,170 @@ template class MapMatching final { } - void operator()(const unsigned state_size, - const Matching::CandidateLists ×tamp_list, - const std::vector coordinate_list, - InternalRouteResult &raw_route_data) const + // TODO optimize: a lot of copying that could probably be avoided + void expandCandidates(const Matching::CandidateLists &candidates_lists, + Matching::CandidateLists &expanded_lists) 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) + // expand list of PhantomNodes to be single-directional + expanded_lists.resize(candidates_lists.size()); + for (const auto i : osrm::irange(0lu, candidates_lists.size())) { - 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 (const auto& candidate : candidates_lists[i]) { - for (auto s_prime = 0; s_prime < state_size; ++s_prime) + // bi-directional edge, split phantom node + if (candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID) { - // 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; - } + PhantomNode forward_node(candidate.first); + PhantomNode reverse_node(candidate.first); + forward_node.reverse_node_id = SPECIAL_NODEID; + reverse_node.forward_node_id = SPECIAL_NODEID; + expanded_lists[i].emplace_back(forward_node, candidate.second); + expanded_lists[i].emplace_back(reverse_node, candidate.second); + } + else + { + expanded_lists[i].push_back(candidate); } } } - 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()); + } + + void operator()(const Matching::CandidateLists &candidates_lists, + const std::vector coordinate_list, + std::vector& matched_nodes, + JSON::Object& _debug_info) const + { + BOOST_ASSERT(candidates_lists.size() == coordinate_list.size()); + + Matching::CandidateLists timestamp_list; + expandCandidates(candidates_lists, timestamp_list); + + 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; + for (const auto& l : timestamp_list) + { + viterbi.emplace_back(l.size(), -std::numeric_limits::infinity()); + parents.emplace_back(l.size(), 0); + } + + JSON::Array _debug_viterbi; + JSON::Array _debug_initial_viterbi; + for (auto s = 0u; s < viterbi[0].size(); ++s) + { + // this might need to be squared as pi_s is also defined as the emission + // probability in the paper. + viterbi[0][s] = log_probability(emission_probability(timestamp_list[0][s].second)); + parents[0][s] = s; + + _debug_initial_viterbi.values.push_back(makeJSONSave(viterbi[0][s])); + } + _debug_viterbi.values.push_back(_debug_initial_viterbi); + + // attention, this call is relatively expensive + //const auto beta = get_beta(state_size, timestamp_list, coordinate_list); + const auto beta = 10.0; + + JSON::Array _debug_timestamps; + for (auto t = 1u; t < timestamp_list.size(); ++t) + { + const auto& prev_viterbi = viterbi[t-1]; + const auto& prev_timestamps_list = timestamp_list[t-1]; + const auto& prev_coordinate = coordinate_list[t-1]; + + auto& current_viterbi = viterbi[t]; + auto& current_parents = parents[t]; + const auto& current_timestamps_list = timestamp_list[t]; + const auto& current_coordinate = coordinate_list[t]; + + JSON::Array _debug_transition_rows; + // compute d_t for this timestamp and the next one + for (auto s = 0u; s < prev_viterbi.size(); ++s) + { + + JSON::Array _debug_row; + for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) + { + + // how likely is candidate s_prime at time t to be emitted? + const double emission_pr = log_probability(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(prev_coordinate, + current_coordinate, + prev_timestamps_list[s].first, + current_timestamps_list[s_prime].first); + + // plug probabilities together + const double transition_pr = log_probability(transition_probability(d_t, beta)); + const double new_value = prev_viterbi[s] + emission_pr + transition_pr; + + JSON::Array _debug_element = makeJSONArray( + makeJSONSave(prev_viterbi[s]), + makeJSONSave(emission_pr), + makeJSONSave(transition_pr), + get_network_distance(prev_timestamps_list[s].first, current_timestamps_list[s_prime].first), + coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate) + ); + + _debug_row.values.push_back(_debug_element); + + if (new_value > current_viterbi[s_prime]) + { + current_viterbi[s_prime] = new_value; + current_parents[s_prime] = s; + } + } + _debug_transition_rows.values.push_back(_debug_row); + } + _debug_timestamps.values.push_back(_debug_transition_rows); + + JSON::Array _debug_viterbi_col; + for (auto s_prime = 0u; s_prime < current_timestamps_list.size(); ++s_prime) + { + _debug_viterbi_col.values.push_back(makeJSONSave(current_viterbi[s_prime])); + } + _debug_viterbi.values.push_back(_debug_viterbi_col); + } + + _debug_info.values["transitions"] = _debug_timestamps; + _debug_info.values["viterbi"] = _debug_viterbi; + _debug_info.values["beta"] = beta; + + // loop through the columns, and only compare the last entry + auto max_element_iter = std::max_element(viterbi.back().begin(), viterbi.back().end()); + auto parent_index = std::distance(viterbi.back().begin(), max_element_iter); std::deque reconstructed_indices; - SimpleLogger().Write() << "d"; - - for (auto i = number_of_timestamps - 1; i > 0; --i) + for (auto i = timestamp_list.size() - 1u; i > 0u; --i) { - SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ; reconstructed_indices.push_front(parent_index); - parent_index = parent[parent_index][i]; + parent_index = parents[i][parent_index]; } - SimpleLogger().Write() << "[0] parent: " << parent_index; reconstructed_indices.push_front(parent_index); - SimpleLogger().Write() << "e"; - - for (auto i = 0; i < reconstructed_indices.size(); ++i) + JSON::Array _debug_chosen_candidates; + matched_nodes.resize(reconstructed_indices.size()); + for (auto i = 0u; 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; + matched_nodes[i] = timestamp_list[i][location_index].first; + _debug_chosen_candidates.values.push_back(location_index); } - - SimpleLogger().Write() << "f, done"; + _debug_info.values["chosen_candidates"] = _debug_chosen_candidates; + JSON::Array _debug_expanded_candidates; + for (const auto& l : timestamp_list) { + JSON::Array _debug_expanded_candidates_col; + for (const auto& pair : l) { + const auto& coord = pair.first.location; + _debug_expanded_candidates_col.values.push_back(makeJSONArray(coord.lat / COORDINATE_PRECISION, + coord.lon / COORDINATE_PRECISION)); + } + _debug_expanded_candidates.values.push_back(_debug_expanded_candidates_col); + } + _debug_info.values["expanded_candidates"] = _debug_expanded_candidates; } }; diff --git a/server/data_structures/datafacade_base.hpp b/server/data_structures/datafacade_base.hpp index 0c9edb2eb..9ec01e16e 100644 --- a/server/data_structures/datafacade_base.hpp +++ b/server/data_structures/datafacade_base.hpp @@ -108,7 +108,9 @@ template class BaseDataFacade virtual bool IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const unsigned number_of_results) = 0; + const double max_distance, + const unsigned min_number_of_phantom_nodes, + const unsigned max_number_of_phantom_nodes) = 0; virtual unsigned GetCheckSum() const = 0; diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp index 5ab769bb0..da974886b 100644 --- a/server/data_structures/internal_datafacade.hpp +++ b/server/data_structures/internal_datafacade.hpp @@ -419,7 +419,9 @@ template class InternalDataFacade final : public BaseDataFacad bool IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const unsigned number_of_results) final + const double max_distance, + const unsigned min_number_of_phantom_nodes, + const unsigned max_number_of_phantom_nodes) final { if (!m_static_rtree.get()) { @@ -427,7 +429,7 @@ template class InternalDataFacade final : public BaseDataFacad } return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, number_of_results); + input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes); } unsigned GetCheckSum() const override final { return m_check_sum; } diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp index eaf9386f7..8451775bb 100644 --- a/server/data_structures/shared_datafacade.hpp +++ b/server/data_structures/shared_datafacade.hpp @@ -407,7 +407,9 @@ template class SharedDataFacade final : public BaseDataFacade< bool IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, - const unsigned number_of_results) final + const double max_distance, + const unsigned min_number_of_phantom_nodes, + const unsigned max_number_of_phantom_nodes) final { if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first) { @@ -415,7 +417,7 @@ template class SharedDataFacade final : public BaseDataFacade< } return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, number_of_results); + input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes); } unsigned GetCheckSum() const override final { return m_check_sum; } From 59727a69676ca931f01f5df0cd827e4e138d5296 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 16 Jan 2015 00:17:40 +0100 Subject: [PATCH 03/67] Get all nodes in dense areas but make sure we don't underflow in sparse ones --- plugins/map_matching.hpp | 18 +++--------------- server/data_structures/datafacade_base.hpp | 3 +-- server/data_structures/internal_datafacade.hpp | 2 +- server/data_structures/shared_datafacade.hpp | 2 +- 4 files changed, 6 insertions(+), 19 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 7be46ed49..3a589ed25 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -81,30 +81,18 @@ template class MapMatchingPlugin : public BasePlugin route_parameters.coordinates[current_coordinate - 1], route_parameters.coordinates[current_coordinate]); - std::cout << "Searching: " << current_coordinate << std::endl; std::vector> candidates; - if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance( + if (!facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance( route_parameters.coordinates[current_coordinate], candidates, - last_distance, + last_distance/2.0, 5, 20)) { - std::cout << "Nothing found for " << current_coordinate << std::endl; - continue; + return 400; } candidate_lists.push_back(candidates); - - std::cout << current_coordinate << " (" << (last_distance / 2.0) << ") : " - << candidates.size() << std::endl; - - BOOST_ASSERT(candidate_lists[current_coordinate].size() == 10); - } - - if (2 > candidate_lists.size()) - { - return 400; } // call the actual map matching diff --git a/server/data_structures/datafacade_base.hpp b/server/data_structures/datafacade_base.hpp index 9ec01e16e..04486532d 100644 --- a/server/data_structures/datafacade_base.hpp +++ b/server/data_structures/datafacade_base.hpp @@ -104,9 +104,8 @@ template class BaseDataFacade virtual bool IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate, PhantomNode &resulting_phantom_node) = 0; - virtual bool - IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + IncrementalFindPhantomNodeForCoordinateWithMaxDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, const double max_distance, const unsigned min_number_of_phantom_nodes, diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp index da974886b..a85a78e30 100644 --- a/server/data_structures/internal_datafacade.hpp +++ b/server/data_structures/internal_datafacade.hpp @@ -417,7 +417,7 @@ template class InternalDataFacade final : public BaseDataFacad } bool - IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + IncrementalFindPhantomNodeForCoordinateWithMaxDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, const double max_distance, const unsigned min_number_of_phantom_nodes, diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp index 8451775bb..87af81929 100644 --- a/server/data_structures/shared_datafacade.hpp +++ b/server/data_structures/shared_datafacade.hpp @@ -405,7 +405,7 @@ template class SharedDataFacade final : public BaseDataFacade< } bool - IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate, + IncrementalFindPhantomNodeForCoordinateWithMaxDistance(const FixedPointCoordinate &input_coordinate, std::vector> &resulting_phantom_node_vector, const double max_distance, const unsigned min_number_of_phantom_nodes, From 496338d84d13c415cfa6a3d56eaa61ff1f51f937 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 17 Jan 2015 01:26:43 +0100 Subject: [PATCH 04/67] Implemented pruning and breakage detection --- routing_algorithms/map_matching.hpp | 138 ++++++++++++++++++++++------ 1 file changed, 109 insertions(+), 29 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 1b28f8a30..baa225fb6 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -312,42 +312,82 @@ template class MapMatching final Matching::CandidateLists timestamp_list; expandCandidates(candidates_lists, timestamp_list); + 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> pruned; for (const auto& l : timestamp_list) { viterbi.emplace_back(l.size(), -std::numeric_limits::infinity()); parents.emplace_back(l.size(), 0); + pruned.emplace_back(l.size(), true); } + JSON::Array _debug_timestamps; JSON::Array _debug_viterbi; - JSON::Array _debug_initial_viterbi; - for (auto s = 0u; s < viterbi[0].size(); ++s) - { - // this might need to be squared as pi_s is also defined as the emission - // probability in the paper. - viterbi[0][s] = log_probability(emission_probability(timestamp_list[0][s].second)); - parents[0][s] = s; + JSON::Array _debug_pruned; - _debug_initial_viterbi.values.push_back(makeJSONSave(viterbi[0][s])); - } - _debug_viterbi.values.push_back(_debug_initial_viterbi); + unsigned initial_timestamp = 0; + do + { + JSON::Array _debug_initial_viterbi; + JSON::Array _debug_initial_pruned; + + 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_probability(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]; + } + + for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) + { + _debug_initial_viterbi.values.push_back(makeJSONSave(viterbi[initial_timestamp][s])); + _debug_initial_pruned.values.push_back(static_cast(pruned[initial_timestamp][s])); + } + + _debug_viterbi.values.push_back(_debug_initial_viterbi); + _debug_pruned.values.push_back(_debug_initial_pruned); + + + if (initial_timestamp > 0) { + JSON::Array _debug_transition_rows; + for (auto s = 0u; s < viterbi[initial_timestamp-1].size(); ++s) { + _debug_transition_rows.values.push_back(JSON::Array()); + } + _debug_timestamps.values.push_back(_debug_transition_rows); + } + + ++initial_timestamp; + } while (breakage[initial_timestamp - 1]); + + BOOST_ASSERT(initial_timestamp > 0 && initial_timestamp < viterbi.size()); + --initial_timestamp; + + BOOST_ASSERT(breakage[initial_timestamp] == false); // attention, this call is relatively expensive //const auto beta = get_beta(state_size, timestamp_list, coordinate_list); const auto beta = 10.0; - JSON::Array _debug_timestamps; - for (auto t = 1u; t < timestamp_list.size(); ++t) + unsigned prev_unbroken_timestamp = initial_timestamp; + for (auto t = initial_timestamp + 1; t < timestamp_list.size(); ++t) { - const auto& prev_viterbi = viterbi[t-1]; - const auto& prev_timestamps_list = timestamp_list[t-1]; - const auto& prev_coordinate = coordinate_list[t-1]; + const auto& prev_viterbi = viterbi[prev_unbroken_timestamp]; + const auto& prev_pruned = 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]; const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; @@ -356,8 +396,13 @@ template class MapMatching final // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { - JSON::Array _debug_row; + if (prev_pruned[s]) + { + _debug_transition_rows.values.push_back(_debug_row); + continue; + } + for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { @@ -367,7 +412,7 @@ template class MapMatching final // get distance diff between loc1/2 and locs/s_prime const auto d_t = get_distance_difference(prev_coordinate, current_coordinate, - prev_timestamps_list[s].first, + prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first); // plug probabilities together @@ -378,7 +423,7 @@ template class MapMatching final makeJSONSave(prev_viterbi[s]), makeJSONSave(emission_pr), makeJSONSave(transition_pr), - get_network_distance(prev_timestamps_list[s].first, current_timestamps_list[s_prime].first), + get_network_distance(prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first), coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate) ); @@ -388,6 +433,8 @@ template class MapMatching final { current_viterbi[s_prime] = new_value; current_parents[s_prime] = s; + current_pruned[s_prime] = false; + breakage[t] = false; } } _debug_transition_rows.values.push_back(_debug_row); @@ -395,38 +442,71 @@ template class MapMatching final _debug_timestamps.values.push_back(_debug_transition_rows); JSON::Array _debug_viterbi_col; - for (auto s_prime = 0u; s_prime < current_timestamps_list.size(); ++s_prime) + JSON::Array _debug_pruned_col; + for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { _debug_viterbi_col.values.push_back(makeJSONSave(current_viterbi[s_prime])); + _debug_pruned_col.values.push_back(static_cast(current_pruned[s_prime])); } _debug_viterbi.values.push_back(_debug_viterbi_col); + _debug_pruned.values.push_back(_debug_pruned_col); + + if (!breakage[t]) + { + prev_unbroken_timestamp = t; + } } _debug_info.values["transitions"] = _debug_timestamps; _debug_info.values["viterbi"] = _debug_viterbi; + _debug_info.values["pruned"] = _debug_pruned; _debug_info.values["beta"] = beta; // loop through the columns, and only compare the last entry - auto max_element_iter = std::max_element(viterbi.back().begin(), viterbi.back().end()); - auto parent_index = std::distance(viterbi.back().begin(), max_element_iter); - std::deque reconstructed_indices; + 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); + std::deque> reconstructed_indices; - for (auto i = timestamp_list.size() - 1u; i > 0u; --i) + for (auto i = prev_unbroken_timestamp; i > initial_timestamp; --i) { - reconstructed_indices.push_front(parent_index); + if (breakage[i]) + continue; + reconstructed_indices.emplace_front(i, parent_index); parent_index = parents[i][parent_index]; } - reconstructed_indices.push_front(parent_index); + reconstructed_indices.emplace_front(initial_timestamp, parent_index); - JSON::Array _debug_chosen_candidates; matched_nodes.resize(reconstructed_indices.size()); for (auto i = 0u; i < reconstructed_indices.size(); ++i) { - auto location_index = reconstructed_indices[i]; - matched_nodes[i] = timestamp_list[i][location_index].first; - _debug_chosen_candidates.values.push_back(location_index); + auto timestamp_index = reconstructed_indices[i].first; + auto location_index = reconstructed_indices[i].second; + + matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; + } + + JSON::Array _debug_chosen_candidates; + auto _debug_candidate_iter = reconstructed_indices.begin(); + for (auto i = 0u; i < timestamp_list.size(); ++i) + { + if (_debug_candidate_iter != reconstructed_indices.end() && _debug_candidate_iter->first == i) + { + _debug_chosen_candidates.values.push_back(_debug_candidate_iter->second); + _debug_candidate_iter = std::next(_debug_candidate_iter); + } + else + { + _debug_chosen_candidates.values.push_back(JSON::Null()); + } } _debug_info.values["chosen_candidates"] = _debug_chosen_candidates; + + JSON::Array _debug_breakage; + for (auto b : breakage) { + _debug_breakage.values.push_back(static_cast(b)); + } + _debug_info.values["breakage"] = _debug_breakage; + JSON::Array _debug_expanded_candidates; for (const auto& l : timestamp_list) { JSON::Array _debug_expanded_candidates_col; From 173fad2362a4b2fe50353470a25a4606a288fb78 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 19 Jan 2015 23:02:07 +0100 Subject: [PATCH 05/67] Return error when less than 2 points left. --- plugins/map_matching.hpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 3a589ed25..641d198ca 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -108,8 +108,16 @@ template class MapMatchingPlugin : public BasePlugin raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); } + if (2 > matched_nodes.size()) + { + reply = http::Reply::StockReply(http::Reply::badRequest); + return; + } + search_engine_ptr->shortest_path( - raw_route.segment_end_coordinates, route_parameters.uturns, raw_route); + raw_route.segment_end_coordinates, + std::vector(raw_route.segment_end_coordinates.size(), true), + raw_route); DescriptorConfig descriptor_config; From a5db3ea25ba9f744867d21838d70e79b2f4d40dc Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 19 Jan 2015 23:03:26 +0100 Subject: [PATCH 06/67] Print warning when more than 10 points are removed --- routing_algorithms/map_matching.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index baa225fb6..8432c548c 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -485,6 +485,10 @@ template class MapMatching final matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; } + unsigned removed = candidates_lists.size() - matched_nodes.size(); + if (removed > 10) + SimpleLogger().Write(logWARNING) << "Warning: removed " << removed << " candiates."; + JSON::Array _debug_chosen_candidates; auto _debug_candidate_iter = reconstructed_indices.begin(); for (auto i = 0u; i < timestamp_list.size(); ++i) From b5228dcda01359ead1c84f1980a03110b566029e Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Thu, 22 Jan 2015 23:53:35 +0100 Subject: [PATCH 07/67] Detect possible uturns in the data. To make them work, we have to disable PhantomNode splitting for this coordinates. --- CMakeLists.txt | 2 +- plugins/map_matching.hpp | 39 +++++++++++++++++++++-------- routing_algorithms/map_matching.hpp | 11 +++++--- 3 files changed, 37 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b957d5dce..e3dac8ba9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,7 +69,7 @@ file(GLOB PrepareGlob contractor/*.cpp data_structures/hilbert_value.cpp util/co set(PrepareSources prepare.cpp ${PrepareGlob}) add_executable(osrm-prepare ${PrepareSources} $ $ $ $ $ $ $ $) -file(GLOB ServerGlob server/*.cpp) +file(GLOB ServerGlob server/*.cpp util/compute_angle.cpp) file(GLOB DescriptorGlob descriptors/*.cpp) file(GLOB DatastructureGlob data_structures/search_engine_data.cpp data_structures/route_parameters.cpp util/bearing.cpp) list(REMOVE_ITEM DatastructureGlob data_structures/Coordinate.cpp) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 641d198ca..3fad436be 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -27,6 +27,7 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "../util/integer_range.hpp" #include "../data_structures/search_engine.hpp" #include "../routing_algorithms/map_matching.hpp" +#include "../util/compute_angle.hpp" #include "../util/simple_logger.hpp" #include "../util/string_util.hpp" #include "../descriptors/descriptor_base.hpp" @@ -68,22 +69,39 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - InternalRouteResult raw_route; + const auto& input_coords = route_parameters.coordinates; Matching::CandidateLists candidate_lists; + std::vector uturn_indicators(false, input_coords.size()); double last_distance = coordinate_calculation::great_circle_distance( - route_parameters.coordinates[0], - route_parameters.coordinates[1]); - for (const auto current_coordinate : osrm::irange(0, route_parameters.coordinates.size())) + input_coords[0], + input_coords[1]); + for (const auto current_coordinate : osrm::irange(0, input_coords.size())) { if (0 < current_coordinate) + { last_distance = coordinate_calculation::great_circle_distance( - route_parameters.coordinates[current_coordinate - 1], - route_parameters.coordinates[current_coordinate]); + input_coords[current_coordinate - 1], + input_coords[current_coordinate]); + } + + if (input_coords.size()-1 > current_coordinate && 0 < current_coordinate) + { + double turn_angle = ComputeAngle::OfThreeFixedPointCoordinates( + input_coords[current_coordinate-1], + input_coords[current_coordinate], + input_coords[current_coordinate+1]); + + // sharp turns indicate a possible uturn + if (turn_angle < 100.0 || turn_angle > 260.0) + { + uturn_indicators[current_coordinate] = true; + } + } std::vector> candidates; if (!facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance( - route_parameters.coordinates[current_coordinate], + input_coords[current_coordinate], candidates, last_distance/2.0, 5, @@ -98,8 +116,10 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching std::vector matched_nodes; JSON::Object debug_info; - search_engine_ptr->map_matching(candidate_lists, route_parameters.coordinates, matched_nodes, debug_info); + search_engine_ptr->map_matching(candidate_lists, input_coords, uturn_indicators, matched_nodes, debug_info); + + InternalRouteResult raw_route; PhantomNodes current_phantom_node_pair; for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) { @@ -110,8 +130,7 @@ template class MapMatchingPlugin : public BasePlugin if (2 > matched_nodes.size()) { - reply = http::Reply::StockReply(http::Reply::badRequest); - return; + return 400; } search_engine_ptr->shortest_path( diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 8432c548c..8ebd8778b 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -276,7 +276,8 @@ template class MapMatching final // TODO optimize: a lot of copying that could probably be avoided void expandCandidates(const Matching::CandidateLists &candidates_lists, - Matching::CandidateLists &expanded_lists) const + Matching::CandidateLists &expanded_lists, + const std::vector& uturn_indicators) const { // expand list of PhantomNodes to be single-directional expanded_lists.resize(candidates_lists.size()); @@ -284,8 +285,8 @@ template class MapMatching final { for (const auto& candidate : candidates_lists[i]) { - // bi-directional edge, split phantom node - if (candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID) + // bi-directional edge, split phantom node if we don't expect a uturn + if (!uturn_indicators[i] && candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID) { PhantomNode forward_node(candidate.first); PhantomNode reverse_node(candidate.first); @@ -304,13 +305,15 @@ template class MapMatching final void operator()(const Matching::CandidateLists &candidates_lists, const std::vector coordinate_list, + const std::vector& uturn_indicators, std::vector& matched_nodes, JSON::Object& _debug_info) const { BOOST_ASSERT(candidates_lists.size() == coordinate_list.size()); + BOOST_ASSERT(candidates_lists.size() == uturn_indicators.size()); Matching::CandidateLists timestamp_list; - expandCandidates(candidates_lists, timestamp_list); + expandCandidates(candidates_lists, timestamp_list, uturn_indicators); std::vector breakage(timestamp_list.size(), true); From f092fc3fc668fe0b1a58bfee5f60675db8d9d716 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Thu, 29 Jan 2015 23:02:48 +0100 Subject: [PATCH 08/67] Fix minimum number of candidates --- data_structures/static_rtree.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index e2a93c8e3..f9cfbfca0 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -905,8 +905,8 @@ class StaticRTree projected_coordinate, foot_point_coordinate_on_segment, current_ratio); if (number_of_elements_from_big_cc > 0 - && (number_of_elements_from_tiny_cc + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes - || current_perpendicular_distance >= max_distance)) + && result_phantom_node_vector.size() >= min_number_of_phantom_nodes + && current_perpendicular_distance >= max_distance) { traversal_queue = std::priority_queue{}; continue; From fe07f9208cdf39e43ce3779ede9d64d2ca1ee4d3 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 6 Feb 2015 02:15:51 +0100 Subject: [PATCH 09/67] Add bayes classifier --- algorithms/bayes_classifier.hpp | 113 ++++++++++++++++++++++++++++++++ plugins/map_matching.hpp | 12 +++- 2 files changed, 123 insertions(+), 2 deletions(-) create mode 100644 algorithms/bayes_classifier.hpp diff --git a/algorithms/bayes_classifier.hpp b/algorithms/bayes_classifier.hpp new file mode 100644 index 000000000..1fc53937a --- /dev/null +++ b/algorithms/bayes_classifier.hpp @@ -0,0 +1,113 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +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 BAYES_CLASSIFIER_HPP +#define BAYES_CLASSIFIER_HPP + +#include +#include + +struct NormalDistribution +{ + NormalDistribution(const double mean, const double standard_deviation) + : mean(mean) + , standard_deviation(standard_deviation) + { + } + + // FIXME implement log-probability version since its faster + double probabilityDensityFunction(const double val) + { + const double x = val - mean; + return 1.0 / (std::sqrt(2*M_PI) * standard_deviation) * std::exp(-x*x / (standard_deviation * standard_deviation)); + } + + double mean; + double standard_deviation; +}; + + +struct LaplaceDistribution +{ + LaplaceDistribution(const double location, const double scale) + : location(location) + , scale(scale) + { + } + + // FIXME implement log-probability version since its faster + double probabilityDensityFunction(const double val) + { + const double x = std::abs(val - location); + return 1.0 / (2*scale) * std::exp(-x / scale); + } + + double location; + double scale; +}; + +template +class BayesClassifier +{ +public: + enum class ClassLabel : unsigned {NEGATIVE = 0, POSITIVE}; + + BayesClassifier(const PositiveDistributionT& positive_distribution, + const NegativeDistributionT& negative_distribution, + const double positive_apriori_probability) + : positive_distribution(positive_distribution) + , negative_distribution(negative_distribution) + , positive_apriori_probability(positive_apriori_probability) + , negative_apriori_probability(1 - positive_apriori_probability) + { + } + + /* + * Returns label and the probability of the label. + */ + std::pair classify(const ValueT& v) + { + const double positive_postpriori = positive_apriori_probability * positive_distribution.probabilityDensityFunction(v); + const double negative_postpriori = negative_apriori_probability * negative_distribution.probabilityDensityFunction(v); + const double norm = positive_postpriori + negative_postpriori; + + if (positive_postpriori > negative_postpriori) + { + return std::make_pair(ClassLabel::POSITIVE, positive_postpriori / norm); + } + + return std::make_pair(ClassLabel::NEGATIVE, negative_postpriori / norm); + } + +private: + PositiveDistributionT positive_distribution; + NegativeDistributionT negative_distribution; + double positive_apriori_probability; + double negative_apriori_probability; +}; + +#endif /* BAYES_CLASSIFIER_HPP */ diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 3fad436be..1838ab0b3 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -23,6 +23,7 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "plugin_base.hpp" +#include "../algorithms/bayes_classifier.hpp" #include "../algorithms/object_encoder.hpp" #include "../util/integer_range.hpp" #include "../data_structures/search_engine.hpp" @@ -48,7 +49,14 @@ template class MapMatchingPlugin : public BasePlugin std::shared_ptr> search_engine_ptr; public: - MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade) + MapMatchingPlugin(DataFacadeT *facade) + : descriptor_string("match") + , facade(facade) + // the values where derived from fitting a laplace distribution + // to the values of manually classified traces + , classifier(LaplaceDistribution(0.0057154021891018675, 0.020294704891166186), + LaplaceDistribution(0.11467696742821254, 0.49918444000368756), + 0.7977883096366508) // valid apriori probability { descriptor_table.emplace("json", 0); descriptor_table.emplace("gpx", 1); @@ -118,7 +126,6 @@ template class MapMatchingPlugin : public BasePlugin JSON::Object debug_info; search_engine_ptr->map_matching(candidate_lists, input_coords, uturn_indicators, matched_nodes, debug_info); - InternalRouteResult raw_route; PhantomNodes current_phantom_node_pair; for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) @@ -176,6 +183,7 @@ template class MapMatchingPlugin : public BasePlugin private: std::string descriptor_string; DataFacadeT *facade; + BayesClassifier classifier; }; #endif /* MAP_MATCHING_PLUGIN_H */ From 1b16dd126bcbbde1b949712ba65164df529f3c50 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 7 Feb 2015 15:39:07 +0100 Subject: [PATCH 10/67] Actually compute and transmit confidence in the response --- algorithms/bayes_classifier.hpp | 5 +++-- plugins/map_matching.hpp | 40 ++++++++++++++++++++++++++++++++- 2 files changed, 42 insertions(+), 3 deletions(-) diff --git a/algorithms/bayes_classifier.hpp b/algorithms/bayes_classifier.hpp index 1fc53937a..042259111 100644 --- a/algorithms/bayes_classifier.hpp +++ b/algorithms/bayes_classifier.hpp @@ -60,7 +60,7 @@ struct LaplaceDistribution } // FIXME implement log-probability version since its faster - double probabilityDensityFunction(const double val) + double probabilityDensityFunction(const double val) const { const double x = std::abs(val - location); return 1.0 / (2*scale) * std::exp(-x / scale); @@ -75,6 +75,7 @@ class BayesClassifier { public: enum class ClassLabel : unsigned {NEGATIVE = 0, POSITIVE}; + using ClassificationT = std::pair; BayesClassifier(const PositiveDistributionT& positive_distribution, const NegativeDistributionT& negative_distribution, @@ -89,7 +90,7 @@ public: /* * Returns label and the probability of the label. */ - std::pair classify(const ValueT& v) + ClassificationT classify(const ValueT& v) const { const double positive_postpriori = positive_apriori_probability * positive_distribution.probabilityDensityFunction(v); const double negative_postpriori = negative_apriori_probability * negative_distribution.probabilityDensityFunction(v); diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 1838ab0b3..8c3c64a12 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -48,6 +48,9 @@ template class MapMatchingPlugin : public BasePlugin std::unordered_map descriptor_table; std::shared_ptr> search_engine_ptr; + using ClassifierT = BayesClassifier; + using TraceClassification = ClassifierT::ClassificationT; + public: MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match") @@ -69,6 +72,29 @@ template class MapMatchingPlugin : public BasePlugin const std::string GetDescriptor() const final { return descriptor_string; } + TraceClassification classify(double trace_length, const std::vector& matched_nodes, int removed_points) const + { + double matching_length = 0; + for (const auto current_node : osrm::irange(0, matched_nodes.size()-1)) + { + matching_length += coordinate_calculation::great_circle_distance( + matched_nodes[current_node].location, + matched_nodes[current_node + 1].location); + } + double distance_feature = -std::log(trace_length / matching_length); + + auto label_with_confidence = classifier.classify(distance_feature); + + // "second stage classifier": if we need to remove points there is something fishy + if (removed_points > 0) + { + label_with_confidence.first = ClassifierT::ClassLabel::NEGATIVE; + label_with_confidence.second = 1.0; + } + + return label_with_confidence; + } + int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final { // check number of parameters @@ -84,6 +110,7 @@ template class MapMatchingPlugin : public BasePlugin double last_distance = coordinate_calculation::great_circle_distance( input_coords[0], input_coords[1]); + double trace_length = 0; for (const auto current_coordinate : osrm::irange(0, input_coords.size())) { if (0 < current_coordinate) @@ -91,6 +118,7 @@ template class MapMatchingPlugin : public BasePlugin last_distance = coordinate_calculation::great_circle_distance( input_coords[current_coordinate - 1], input_coords[current_coordinate]); + trace_length += last_distance; } if (input_coords.size()-1 > current_coordinate && 0 < current_coordinate) @@ -126,6 +154,16 @@ template class MapMatchingPlugin : public BasePlugin JSON::Object debug_info; search_engine_ptr->map_matching(candidate_lists, input_coords, uturn_indicators, matched_nodes, debug_info); + TraceClassification classification = classify(trace_length, matched_nodes, input_coords.size() - matched_nodes.size()); + if (classification.first == ClassifierT::ClassLabel::POSITIVE) + { + json_result.values["confidence"] = classification.second; + } + else + { + json_result.values["confidence"] = 1-classification.second; + } + InternalRouteResult raw_route; PhantomNodes current_phantom_node_pair; for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) @@ -183,7 +221,7 @@ template class MapMatchingPlugin : public BasePlugin private: std::string descriptor_string; DataFacadeT *facade; - BayesClassifier classifier; + ClassifierT classifier; }; #endif /* MAP_MATCHING_PLUGIN_H */ From 0fce20c5033f976e026f2bd90d1a649affc961f3 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 7 Feb 2015 17:26:38 +0100 Subject: [PATCH 11/67] Directly compute log probabilities --- routing_algorithms/map_matching.hpp | 75 +++++++++-------------------- 1 file changed, 22 insertions(+), 53 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 8ebd8778b..7ffe80fd7 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -25,7 +25,6 @@ or see http://www.gnu.org/licenses/agpl.txt. #include "../data_structures/coordinate_calculation.hpp" #include "../util/simple_logger.hpp" -#include "../util/container.hpp" #include #include @@ -78,20 +77,36 @@ template class MapMatching final using QueryHeap = SearchEngineData::QueryHeap; SearchEngineData &engine_working_data; + // FIXME this value should be a table based on samples/meter (or samples/min) + constexpr static const double beta = 10.0; constexpr static const double sigma_z = 4.07; + 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 { 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 + constexpr double transition_probability(const float d_t, const float beta) const { - return std::log2(probability); + 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; + } + + inline double log_transition_probability(const float d_t, const float beta) const + { + return -std::log(beta) - d_t / beta; } // TODO: needs to be estimated from the input locations + // FIXME These values seem wrong. Higher beta for more samples/minute? Should be inverse proportional. //constexpr static const double beta = 1.; // samples/min and beta // 1 0.49037673 @@ -126,48 +141,6 @@ template class MapMatching final // 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 = 1u; t < timestamp_list.size(); ++t) - { - for (auto s = 0u; 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, @@ -186,7 +159,7 @@ template class MapMatching final } double get_network_distance(const PhantomNode &source_phantom, - const PhantomNode &target_phantom) const + const PhantomNode &target_phantom) const { EdgeWeight upper_bound = INVALID_EDGE_WEIGHT; NodeID middle_node = SPECIAL_NODEID; @@ -344,7 +317,7 @@ template class MapMatching final { // this might need to be squared as pi_s is also defined as the emission // probability in the paper. - viterbi[initial_timestamp][s] = log_probability(emission_probability(timestamp_list[initial_timestamp][s].second)); + 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(); @@ -377,10 +350,6 @@ template class MapMatching final BOOST_ASSERT(breakage[initial_timestamp] == false); - // attention, this call is relatively expensive - //const auto beta = get_beta(state_size, timestamp_list, coordinate_list); - const auto beta = 10.0; - unsigned prev_unbroken_timestamp = initial_timestamp; for (auto t = initial_timestamp + 1; t < timestamp_list.size(); ++t) { @@ -410,7 +379,7 @@ template class MapMatching final { // how likely is candidate s_prime at time t to be emitted? - const double emission_pr = log_probability(emission_probability(timestamp_list[t][s_prime].second)); + const double emission_pr = log_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(prev_coordinate, @@ -419,7 +388,7 @@ template class MapMatching final current_timestamps_list[s_prime].first); // plug probabilities together - const double transition_pr = log_probability(transition_probability(d_t, beta)); + const double transition_pr = log_transition_probability(d_t, beta); const double new_value = prev_viterbi[s] + emission_pr + transition_pr; JSON::Array _debug_element = makeJSONArray( From dc1405ffa822c72a958512a928902a7ce14583d9 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 7 Feb 2015 17:29:09 +0100 Subject: [PATCH 12/67] Fix typo in debugging code --- routing_algorithms/map_matching.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 7ffe80fd7..e52fd225a 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -33,7 +33,7 @@ or see http://www.gnu.org/licenses/agpl.txt. #include template -T makeJSONSave(T d) +T makeJSONSafe(T d) { if (std::isnan(d) || std::numeric_limits::infinity() == d) { return std::numeric_limits::max(); @@ -326,7 +326,7 @@ template class MapMatching final for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) { - _debug_initial_viterbi.values.push_back(makeJSONSave(viterbi[initial_timestamp][s])); + _debug_initial_viterbi.values.push_back(makeJSONSafe(viterbi[initial_timestamp][s])); _debug_initial_pruned.values.push_back(static_cast(pruned[initial_timestamp][s])); } @@ -392,9 +392,9 @@ template class MapMatching final const double new_value = prev_viterbi[s] + emission_pr + transition_pr; JSON::Array _debug_element = makeJSONArray( - makeJSONSave(prev_viterbi[s]), - makeJSONSave(emission_pr), - makeJSONSave(transition_pr), + makeJSONSafe(prev_viterbi[s]), + makeJSONSafe(emission_pr), + makeJSONSafe(transition_pr), get_network_distance(prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first), coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate) ); @@ -417,7 +417,7 @@ template class MapMatching final JSON::Array _debug_pruned_col; for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { - _debug_viterbi_col.values.push_back(makeJSONSave(current_viterbi[s_prime])); + _debug_viterbi_col.values.push_back(makeJSONSafe(current_viterbi[s_prime])); _debug_pruned_col.values.push_back(static_cast(current_pruned[s_prime])); } _debug_viterbi.values.push_back(_debug_viterbi_col); From 0637215b85a16ef431f7d90ad11ace0c5150b702 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 9 Feb 2015 00:19:42 +0100 Subject: [PATCH 13/67] Skip computing viterbi if viterbi of previous state is lower than lower bound This causes a speedup of 300%. --- routing_algorithms/map_matching.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index e52fd225a..a512e307a 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -377,9 +377,15 @@ template class MapMatching final for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { + double new_value = prev_viterbi[s]; + if (current_viterbi[s_prime] > new_value) + continue; // how likely is candidate s_prime at time t to be emitted? const double emission_pr = log_emission_probability(timestamp_list[t][s_prime].second); + new_value += emission_pr; + if (current_viterbi[s_prime] > new_value) + continue; // get distance diff between loc1/2 and locs/s_prime const auto d_t = get_distance_difference(prev_coordinate, @@ -387,9 +393,8 @@ template class MapMatching final prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first); - // plug probabilities together const double transition_pr = log_transition_probability(d_t, beta); - const double new_value = prev_viterbi[s] + emission_pr + transition_pr; + new_value += transition_pr; JSON::Array _debug_element = makeJSONArray( makeJSONSafe(prev_viterbi[s]), From 66d7a073d34e7a4d837203a03ba73ff95b891835 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 9 Feb 2015 00:55:12 +0100 Subject: [PATCH 14/67] Move splitting candidates to plugin --- plugins/map_matching.hpp | 69 ++++++++++++++++++++++------- routing_algorithms/map_matching.hpp | 42 +----------------- 2 files changed, 53 insertions(+), 58 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 8c3c64a12..ae81e5b48 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -95,24 +95,15 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final + bool get_candiates(const std::vector& input_coords, double& trace_length, Matching::CandidateLists& candidates_lists) { - // check number of parameters - if (!check_all_coordinates(route_parameters.coordinates)) - { - return 400; - } - - const auto& input_coords = route_parameters.coordinates; - Matching::CandidateLists candidate_lists; - std::vector uturn_indicators(false, input_coords.size()); - double last_distance = coordinate_calculation::great_circle_distance( input_coords[0], input_coords[1]); - double trace_length = 0; + trace_length = 0; for (const auto current_coordinate : osrm::irange(0, input_coords.size())) { + bool allow_uturn = false; if (0 < current_coordinate) { last_distance = coordinate_calculation::great_circle_distance( @@ -131,7 +122,7 @@ template class MapMatchingPlugin : public BasePlugin // sharp turns indicate a possible uturn if (turn_angle < 100.0 || turn_angle > 260.0) { - uturn_indicators[current_coordinate] = true; + allow_uturn = true; } } @@ -143,17 +134,60 @@ template class MapMatchingPlugin : public BasePlugin 5, 20)) { - return 400; + return false; } - candidate_lists.push_back(candidates); + if (allow_uturn) + { + candidates_lists.push_back(candidates); + } + else + { + unsigned compact_size = candidates.size(); + for (const auto i : osrm::irange(0u, compact_size)) + { + // Split edge if it is bidirectional and append reverse direction to end of list + if (candidates[i].first.forward_node_id != SPECIAL_NODEID + && candidates[i].first.reverse_node_id != SPECIAL_NODEID) + { + PhantomNode reverse_node(candidates[i].first); + reverse_node.forward_node_id = SPECIAL_NODEID; + candidates.push_back(std::make_pair(reverse_node, candidates[i].second)); + + candidates[i].first.reverse_node_id = SPECIAL_NODEID; + } + } + candidates_lists.push_back(candidates); + } + + } + + return true; + } + + int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final + { + // check number of parameters + if (!check_all_coordinates(route_parameters.coordinates)) + { + return 400; + } + + double trace_length; + Matching::CandidateLists candidates_lists; + const auto& input_coords = route_parameters.coordinates; + bool found_candidates = get_candiates(input_coords, trace_length, candidates_lists); + if (!found_candidates) + { + return 400; } // call the actual map matching - std::vector matched_nodes; JSON::Object debug_info; - search_engine_ptr->map_matching(candidate_lists, input_coords, uturn_indicators, matched_nodes, debug_info); + std::vector matched_nodes; + search_engine_ptr->map_matching(candidates_lists, input_coords, matched_nodes, debug_info); + // classify result TraceClassification classification = classify(trace_length, matched_nodes, input_coords.size() - matched_nodes.size()); if (classification.first == ClassifierT::ClassLabel::POSITIVE) { @@ -164,6 +198,7 @@ template class MapMatchingPlugin : public BasePlugin json_result.values["confidence"] = 1-classification.second; } + // run shortest path routing to obtain geometry InternalRouteResult raw_route; PhantomNodes current_phantom_node_pair; for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index a512e307a..049017e80 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -247,47 +247,11 @@ template class MapMatching final { } - // TODO optimize: a lot of copying that could probably be avoided - void expandCandidates(const Matching::CandidateLists &candidates_lists, - Matching::CandidateLists &expanded_lists, - const std::vector& uturn_indicators) const - { - // expand list of PhantomNodes to be single-directional - expanded_lists.resize(candidates_lists.size()); - for (const auto i : osrm::irange(0lu, candidates_lists.size())) - { - for (const auto& candidate : candidates_lists[i]) - { - // bi-directional edge, split phantom node if we don't expect a uturn - if (!uturn_indicators[i] && candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID) - { - PhantomNode forward_node(candidate.first); - PhantomNode reverse_node(candidate.first); - forward_node.reverse_node_id = SPECIAL_NODEID; - reverse_node.forward_node_id = SPECIAL_NODEID; - expanded_lists[i].emplace_back(forward_node, candidate.second); - expanded_lists[i].emplace_back(reverse_node, candidate.second); - } - else - { - expanded_lists[i].push_back(candidate); - } - } - } - } - - void operator()(const Matching::CandidateLists &candidates_lists, + void operator()(const Matching::CandidateLists ×tamp_list, const std::vector coordinate_list, - const std::vector& uturn_indicators, std::vector& matched_nodes, JSON::Object& _debug_info) const { - BOOST_ASSERT(candidates_lists.size() == coordinate_list.size()); - BOOST_ASSERT(candidates_lists.size() == uturn_indicators.size()); - - Matching::CandidateLists timestamp_list; - expandCandidates(candidates_lists, timestamp_list, uturn_indicators); - std::vector breakage(timestamp_list.size(), true); BOOST_ASSERT(timestamp_list.size() > 0); @@ -462,10 +426,6 @@ template class MapMatching final matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; } - unsigned removed = candidates_lists.size() - matched_nodes.size(); - if (removed > 10) - SimpleLogger().Write(logWARNING) << "Warning: removed " << removed << " candiates."; - JSON::Array _debug_chosen_candidates; auto _debug_candidate_iter = reconstructed_indices.begin(); for (auto i = 0u; i < timestamp_list.size(); ++i) From e381566494d91395d32e9333a68c07f92d2b8d08 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 9 Feb 2015 10:14:26 +0100 Subject: [PATCH 15/67] Eliminate branch --- routing_algorithms/map_matching.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 049017e80..ffade6fd8 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -341,13 +341,9 @@ template class MapMatching final for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { - double new_value = prev_viterbi[s]; - if (current_viterbi[s_prime] > new_value) - continue; - // how likely is candidate s_prime at time t to be emitted? const double emission_pr = log_emission_probability(timestamp_list[t][s_prime].second); - new_value += emission_pr; + double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) continue; From 1bcf41d382aa5cd4a8a275e06c7bd9479c41e714 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 9 Feb 2015 11:06:58 +0100 Subject: [PATCH 16/67] Prune low probability transitions --- routing_algorithms/map_matching.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index ffade6fd8..55376d224 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -352,6 +352,9 @@ template class MapMatching final current_coordinate, prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first); + // very low probability transition -> prune + if (d_t > 2000) + continue; const double transition_pr = log_transition_probability(d_t, beta); new_value += transition_pr; From 4838ffb82dd42abda98263792af59b3b89c4663a Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 9 Feb 2015 12:41:30 +0100 Subject: [PATCH 17/67] Fix nan values if matched to single point --- plugins/map_matching.hpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index ae81e5b48..f4dec3676 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -81,15 +81,20 @@ template class MapMatchingPlugin : public BasePlugin matched_nodes[current_node].location, matched_nodes[current_node + 1].location); } - double distance_feature = -std::log(trace_length / matching_length); + double distance_feature = -std::log(trace_length) + std::log(matching_length); + + // matched to the same point + if (!std::isfinite(distance_feature)) + { + return std::make_pair(ClassifierT::ClassLabel::NEGATIVE, 1.0); + } auto label_with_confidence = classifier.classify(distance_feature); // "second stage classifier": if we need to remove points there is something fishy if (removed_points > 0) { - label_with_confidence.first = ClassifierT::ClassLabel::NEGATIVE; - label_with_confidence.second = 1.0; + return std::make_pair(ClassifierT::ClassLabel::NEGATIVE, 1.0); } return label_with_confidence; From 6e54f8cfa6c51ad37a5f937c00c9c4c0ff7f2d46 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Tue, 10 Feb 2015 10:54:47 +0100 Subject: [PATCH 18/67] Tighter threshold on low probability transistions --- routing_algorithms/map_matching.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 55376d224..8d22ae81e 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -345,7 +345,10 @@ template class MapMatching final const double emission_pr = log_emission_probability(timestamp_list[t][s_prime].second); double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) + { + _debug_row.values.push_back(JSON::Array()); continue; + } // get distance diff between loc1/2 and locs/s_prime const auto d_t = get_distance_difference(prev_coordinate, @@ -353,8 +356,11 @@ template class MapMatching final prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first); // very low probability transition -> prune - if (d_t > 2000) + if (d_t > 500) + { + _debug_row.values.push_back(JSON::Array()); continue; + } const double transition_pr = log_transition_probability(d_t, beta); new_value += transition_pr; From 1a8c83203937d2d91c47c46d7267f57ac89452bd Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Tue, 10 Feb 2015 13:00:00 +0100 Subject: [PATCH 19/67] Update format of debug json ouput --- routing_algorithms/map_matching.hpp | 116 ++++++++++------------------ 1 file changed, 40 insertions(+), 76 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 8d22ae81e..9ea4fd763 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -32,6 +32,9 @@ or see http://www.gnu.org/licenses/agpl.txt. #include +using JSONVariantArray = mapbox::util::recursive_wrapper; +using JSONVariantObject = mapbox::util::recursive_wrapper; + template T makeJSONSafe(T d) { @@ -265,18 +268,25 @@ template class MapMatching final viterbi.emplace_back(l.size(), -std::numeric_limits::infinity()); parents.emplace_back(l.size(), 0); pruned.emplace_back(l.size(), true); + } - JSON::Array _debug_timestamps; - JSON::Array _debug_viterbi; - JSON::Array _debug_pruned; + JSON::Array _debug_states; + for (const auto& l : timestamp_list) + { + JSON::Array _debug_timestamps; + for (unsigned i = 0; i < l.size(); i++) + { + JSON::Object _debug_state; + _debug_state.values["transitions"] = JSON::Array(); + _debug_timestamps.values.push_back(_debug_state); + } + _debug_states.values.push_back(_debug_timestamps); + } unsigned initial_timestamp = 0; do { - JSON::Array _debug_initial_viterbi; - JSON::Array _debug_initial_pruned; - 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 @@ -286,24 +296,13 @@ template class MapMatching final pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < -std::numeric_limits::max(); breakage[initial_timestamp] = breakage[initial_timestamp] && pruned[initial_timestamp][s]; - } - for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) - { - _debug_initial_viterbi.values.push_back(makeJSONSafe(viterbi[initial_timestamp][s])); - _debug_initial_pruned.values.push_back(static_cast(pruned[initial_timestamp][s])); - } - - _debug_viterbi.values.push_back(_debug_initial_viterbi); - _debug_pruned.values.push_back(_debug_initial_pruned); - - - if (initial_timestamp > 0) { - JSON::Array _debug_transition_rows; - for (auto s = 0u; s < viterbi[initial_timestamp-1].size(); ++s) { - _debug_transition_rows.values.push_back(JSON::Array()); - } - _debug_timestamps.values.push_back(_debug_transition_rows); + _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; @@ -328,16 +327,11 @@ template class MapMatching final const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; - JSON::Array _debug_transition_rows; // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { - JSON::Array _debug_row; if (prev_pruned[s]) - { - _debug_transition_rows.values.push_back(_debug_row); continue; - } for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { @@ -345,10 +339,7 @@ template class MapMatching final const double emission_pr = log_emission_probability(timestamp_list[t][s_prime].second); double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) - { - _debug_row.values.push_back(JSON::Array()); continue; - } // get distance diff between loc1/2 and locs/s_prime const auto d_t = get_distance_difference(prev_coordinate, @@ -357,23 +348,25 @@ template class MapMatching final current_timestamps_list[s_prime].first); // very low probability transition -> prune if (d_t > 500) - { - _debug_row.values.push_back(JSON::Array()); continue; - } const double transition_pr = log_transition_probability(d_t, beta); new_value += transition_pr; - JSON::Array _debug_element = makeJSONArray( + JSON::Object _debug_transistion; + _debug_transistion.values["to"] = makeJSONArray(t, s_prime); + _debug_transistion.values["properties"] = makeJSONArray( makeJSONSafe(prev_viterbi[s]), makeJSONSafe(emission_pr), makeJSONSafe(transition_pr), get_network_distance(prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first), coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate) ); + _debug_states.values[prev_unbroken_timestamp] + .get().get().values[s] + .get().get().values["transitions"] + .get().get().values.push_back(_debug_transistion); - _debug_row.values.push_back(_debug_element); if (new_value > current_viterbi[s_prime]) { @@ -383,19 +376,17 @@ template class MapMatching final breakage[t] = false; } } - _debug_transition_rows.values.push_back(_debug_row); } - _debug_timestamps.values.push_back(_debug_transition_rows); - JSON::Array _debug_viterbi_col; - JSON::Array _debug_pruned_col; for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { - _debug_viterbi_col.values.push_back(makeJSONSafe(current_viterbi[s_prime])); - _debug_pruned_col.values.push_back(static_cast(current_pruned[s_prime])); + _debug_states.values[t] + .get().get().values[s_prime] + .get().get().values["viterbi"] = makeJSONSafe(current_viterbi[s_prime]); + _debug_states.values[t] + .get().get().values[s_prime] + .get().get().values["pruned"] = static_cast(current_pruned[s_prime]); } - _debug_viterbi.values.push_back(_debug_viterbi_col); - _debug_pruned.values.push_back(_debug_pruned_col); if (!breakage[t]) { @@ -403,11 +394,6 @@ template class MapMatching final } } - _debug_info.values["transitions"] = _debug_timestamps; - _debug_info.values["viterbi"] = _debug_viterbi; - _debug_info.values["pruned"] = _debug_pruned; - _debug_info.values["beta"] = beta; - // 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); @@ -429,41 +415,19 @@ template class MapMatching final auto location_index = reconstructed_indices[i].second; matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; - } - JSON::Array _debug_chosen_candidates; - auto _debug_candidate_iter = reconstructed_indices.begin(); - for (auto i = 0u; i < timestamp_list.size(); ++i) - { - if (_debug_candidate_iter != reconstructed_indices.end() && _debug_candidate_iter->first == i) - { - _debug_chosen_candidates.values.push_back(_debug_candidate_iter->second); - _debug_candidate_iter = std::next(_debug_candidate_iter); - } - else - { - _debug_chosen_candidates.values.push_back(JSON::Null()); - } + _debug_states.values[timestamp_index] + .get().get().values[location_index] + .get().get().values["chosen"] = true; } - _debug_info.values["chosen_candidates"] = _debug_chosen_candidates; JSON::Array _debug_breakage; for (auto b : breakage) { _debug_breakage.values.push_back(static_cast(b)); } - _debug_info.values["breakage"] = _debug_breakage; - JSON::Array _debug_expanded_candidates; - for (const auto& l : timestamp_list) { - JSON::Array _debug_expanded_candidates_col; - for (const auto& pair : l) { - const auto& coord = pair.first.location; - _debug_expanded_candidates_col.values.push_back(makeJSONArray(coord.lat / COORDINATE_PRECISION, - coord.lon / COORDINATE_PRECISION)); - } - _debug_expanded_candidates.values.push_back(_debug_expanded_candidates_col); - } - _debug_info.values["expanded_candidates"] = _debug_expanded_candidates; + _debug_info.values["breakage"] = _debug_breakage; + _debug_info.values["states"] = _debug_states; } }; From 38d7db49c88f5dd447bdd3b0a6269e88eae9dc5b Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Tue, 10 Feb 2015 23:24:27 +0100 Subject: [PATCH 20/67] Add state position to debug output --- routing_algorithms/map_matching.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 9ea4fd763..0f2cc0c0c 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -279,6 +279,8 @@ template class MapMatching final { 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_timestamps.values.push_back(_debug_state); } _debug_states.values.push_back(_debug_timestamps); From b3fa03043d604ca443bbf7a10f3500da0ba8f11e Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Thu, 12 Feb 2015 11:44:52 +0100 Subject: [PATCH 21/67] Calculate the real route length for classification --- plugins/map_matching.hpp | 16 ++++-------- routing_algorithms/map_matching.hpp | 40 ++++++++++++----------------- 2 files changed, 22 insertions(+), 34 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index f4dec3676..0f9a94c80 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -72,16 +72,9 @@ template class MapMatchingPlugin : public BasePlugin const std::string GetDescriptor() const final { return descriptor_string; } - TraceClassification classify(double trace_length, const std::vector& matched_nodes, int removed_points) const + TraceClassification classify(float trace_length, float matched_length, int removed_points) const { - double matching_length = 0; - for (const auto current_node : osrm::irange(0, matched_nodes.size()-1)) - { - matching_length += coordinate_calculation::great_circle_distance( - matched_nodes[current_node].location, - matched_nodes[current_node + 1].location); - } - double distance_feature = -std::log(trace_length) + std::log(matching_length); + double distance_feature = -std::log(trace_length) + std::log(matched_length); // matched to the same point if (!std::isfinite(distance_feature)) @@ -189,11 +182,12 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching JSON::Object debug_info; + float matched_length; std::vector matched_nodes; - search_engine_ptr->map_matching(candidates_lists, input_coords, matched_nodes, debug_info); + search_engine_ptr->map_matching(candidates_lists, input_coords, matched_nodes, matched_length, debug_info); // classify result - TraceClassification classification = classify(trace_length, matched_nodes, input_coords.size() - matched_nodes.size()); + TraceClassification classification = classify(trace_length, matched_length, input_coords.size() - matched_nodes.size()); if (classification.first == ClassifierT::ClassLabel::POSITIVE) { json_result.values["confidence"] = classification.second; diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 0f2cc0c0c..a87a739eb 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -144,23 +144,6 @@ template class MapMatching final // 29 32.21683062 // 30 34.56991141 - 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 auto 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; - } - double get_network_distance(const PhantomNode &source_phantom, const PhantomNode &target_phantom) const { @@ -253,6 +236,7 @@ template class MapMatching final 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); @@ -262,11 +246,13 @@ template class MapMatching final // 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); } @@ -326,6 +312,7 @@ template class MapMatching final auto& current_viterbi = viterbi[t]; auto& current_pruned = pruned[t]; auto& current_parents = parents[t]; + auto& current_lengths = segment_lengths[t]; const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; @@ -344,10 +331,14 @@ template class MapMatching final continue; // get distance diff between loc1/2 and locs/s_prime - const auto d_t = get_distance_difference(prev_coordinate, - current_coordinate, - prev_unbroken_timestamps_list[s].first, - current_timestamps_list[s_prime].first); + const auto network_distance = get_network_distance(prev_unbroken_timestamps_list[s].first, + current_timestamps_list[s_prime].first); + const auto great_circle_distance = + coordinate_calculation::great_circle_distance(prev_coordinate, + current_coordinate); + + const auto d_t = std::abs(network_distance - great_circle_distance); + // very low probability transition -> prune if (d_t > 500) continue; @@ -361,8 +352,8 @@ template class MapMatching final makeJSONSafe(prev_viterbi[s]), makeJSONSafe(emission_pr), makeJSONSafe(transition_pr), - get_network_distance(prev_unbroken_timestamps_list[s].first, current_timestamps_list[s_prime].first), - coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate) + network_distance, + great_circle_distance ); _debug_states.values[prev_unbroken_timestamp] .get().get().values[s] @@ -374,6 +365,7 @@ template class MapMatching final { 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; } @@ -410,6 +402,7 @@ template class MapMatching final } reconstructed_indices.emplace_front(initial_timestamp, parent_index); + matched_length = 0.0f; matched_nodes.resize(reconstructed_indices.size()); for (auto i = 0u; i < reconstructed_indices.size(); ++i) { @@ -417,6 +410,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]; _debug_states.values[timestamp_index] .get().get().values[location_index] From 0c3102721c07e04f492a32e2e34e9c56738c3acf Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 13 Feb 2015 11:01:03 +0100 Subject: [PATCH 22/67] Make number of candidates a parameter --- plugins/map_matching.hpp | 2 +- routing_algorithms/map_matching.hpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 0f9a94c80..8a9988277 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -130,7 +130,7 @@ template class MapMatchingPlugin : public BasePlugin candidates, last_distance/2.0, 5, - 20)) + Matching::max_number_of_candidates)) { return false; } diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index a87a739eb..7bbac2afd 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -70,6 +70,7 @@ namespace Matching typedef std::vector> CandidateList; typedef std::vector CandidateLists; typedef std::pair PhantomNodesWithProbability; +constexpr static const unsigned max_number_of_candidates = 20; } // implements a hidden markov model map matching algorithm From d620da365e8419f56db87b240095bcbae478eeb9 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 13 Feb 2015 14:34:44 +0100 Subject: [PATCH 23/67] Implement more general pruning --- routing_algorithms/map_matching.hpp | 214 ++++++++++++++++++---------- 1 file changed, 142 insertions(+), 72 deletions(-) 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)); } From cb4a81008c60cbbd1be010bb963e090d12f88af1 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Tue, 17 Feb 2015 12:22:11 +0100 Subject: [PATCH 24/67] Split traces into subtraces --- plugins/map_matching.hpp | 147 ++++++++++++++++------------ routing_algorithms/map_matching.hpp | 106 +++++++++++++------- 2 files changed, 155 insertions(+), 98 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 8a9988277..2dd9545b5 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -62,9 +62,6 @@ template class MapMatchingPlugin : public BasePlugin 0.7977883096366508) // valid apriori probability { descriptor_table.emplace("json", 0); - descriptor_table.emplace("gpx", 1); - // descriptor_table.emplace("geojson", 2); - // search_engine_ptr = std::make_shared>(facade); } @@ -93,12 +90,13 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - bool get_candiates(const std::vector& input_coords, double& trace_length, Matching::CandidateLists& candidates_lists) + bool get_candiates(const std::vector& input_coords, std::vector& sub_trace_lengths, Matching::CandidateLists& candidates_lists) { double last_distance = coordinate_calculation::great_circle_distance( input_coords[0], input_coords[1]); - trace_length = 0; + sub_trace_lengths.resize(input_coords.size()); + sub_trace_lengths[0] = 0; for (const auto current_coordinate : osrm::irange(0, input_coords.size())) { bool allow_uturn = false; @@ -107,7 +105,7 @@ template class MapMatchingPlugin : public BasePlugin last_distance = coordinate_calculation::great_circle_distance( input_coords[current_coordinate - 1], input_coords[current_coordinate]); - trace_length += last_distance; + sub_trace_lengths[current_coordinate] += sub_trace_lengths[current_coordinate-1] + last_distance; } if (input_coords.size()-1 > current_coordinate && 0 < current_coordinate) @@ -171,10 +169,10 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - double trace_length; + std::vector sub_trace_lengths; Matching::CandidateLists candidates_lists; const auto& input_coords = route_parameters.coordinates; - bool found_candidates = get_candiates(input_coords, trace_length, candidates_lists); + bool found_candidates = get_candiates(input_coords, sub_trace_lengths, candidates_lists); if (!found_candidates) { return 400; @@ -182,72 +180,91 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching JSON::Object debug_info; - float matched_length; - std::vector matched_nodes; - search_engine_ptr->map_matching(candidates_lists, input_coords, matched_nodes, matched_length, debug_info); + Matching::SubMatchingList sub_matchings; + search_engine_ptr->map_matching(candidates_lists, input_coords, sub_matchings, debug_info); - // classify result - TraceClassification classification = classify(trace_length, matched_length, input_coords.size() - matched_nodes.size()); - if (classification.first == ClassifierT::ClassLabel::POSITIVE) - { - json_result.values["confidence"] = classification.second; - } - else - { - json_result.values["confidence"] = 1-classification.second; - } - - // run shortest path routing to obtain geometry - InternalRouteResult raw_route; - PhantomNodes current_phantom_node_pair; - for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) - { - current_phantom_node_pair.source_phantom = matched_nodes[i]; - current_phantom_node_pair.target_phantom = matched_nodes[i + 1]; - raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); - } - - if (2 > matched_nodes.size()) + if (1 > sub_matchings.size()) { return 400; } - search_engine_ptr->shortest_path( - raw_route.segment_end_coordinates, - std::vector(raw_route.segment_end_coordinates.size(), true), - raw_route); - - DescriptorConfig descriptor_config; - - auto iter = descriptor_table.find(route_parameters.output_format); - unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); - - descriptor_config.zoom_level = route_parameters.zoom_level; - descriptor_config.instructions = route_parameters.print_instructions; - descriptor_config.geometry = route_parameters.geometry; - descriptor_config.encode_geometry = route_parameters.compression; - - std::shared_ptr> descriptor; - switch (descriptor_type) + JSON::Array traces; + for (auto& sub : sub_matchings) { - // case 0: - // descriptor = std::make_shared>(); - // break; - case 1: - descriptor = std::make_shared>(facade); - break; - // case 2: - // descriptor = std::make_shared>(); - // break; - default: - descriptor = std::make_shared>(facade); - break; + // classify result + double trace_length = sub_trace_lengths[sub.end_idx-1] - sub_trace_lengths[sub.begin_idx]; + TraceClassification classification = classify(trace_length, + sub.length, + (sub.end_idx - sub.begin_idx) - sub.nodes.size()); + if (classification.first == ClassifierT::ClassLabel::POSITIVE) + { + sub.confidence = classification.second; + } + else + { + sub.confidence = 1-classification.second; + } + + // FIXME this is a pretty bad hack. Geometries should obtained directly + // from map_matching. + // run shortest path routing to obtain geometry + InternalRouteResult raw_route; + PhantomNodes current_phantom_node_pair; + for (unsigned i = 0; i < sub.nodes.size() - 1; ++i) + { + current_phantom_node_pair.source_phantom = sub.nodes[i]; + current_phantom_node_pair.target_phantom = sub.nodes[i + 1]; + raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); + } + search_engine_ptr->shortest_path( + raw_route.segment_end_coordinates, + std::vector(raw_route.segment_end_coordinates.size(), true), + raw_route); + + + DescriptorConfig descriptor_config; + + auto iter = descriptor_table.find(route_parameters.output_format); + unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); + + descriptor_config.zoom_level = route_parameters.zoom_level; + descriptor_config.instructions = false; + descriptor_config.geometry = route_parameters.geometry; + descriptor_config.encode_geometry = route_parameters.compression; + + std::shared_ptr> descriptor; + switch (descriptor_type) + { + // case 0: + // descriptor = std::make_shared>(); + // break; + case 1: + descriptor = std::make_shared>(facade); + break; + // case 2: + // descriptor = std::make_shared>(); + // break; + default: + descriptor = std::make_shared>(facade); + break; + } + + JSON::Object temp_result; + descriptor->SetConfig(descriptor_config); + descriptor->Run(raw_route, temp_result); + + JSON::Object subtrace; + // via_route compability + subtrace.values["route_geometry"] = temp_result.values["route_geometry"]; + subtrace.values["confidence"] = sub.confidence; + subtrace.values["via_indicies"] = temp_result.values["via_indicies"]; + subtrace.values["via_points"] = temp_result.values["via_points"]; + + traces.values.push_back(subtrace); } - descriptor->SetConfig(descriptor_config); - descriptor->Run(raw_route, json_result); - json_result.values["debug"] = debug_info; + json_result.values["traces"] = traces; return 200; } diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 9cb19402b..92c05bc67 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -70,9 +70,19 @@ JSON::Array makeJSONArray(Args... args) namespace Matching { -typedef std::vector> CandidateList; -typedef std::vector CandidateLists; -typedef std::pair PhantomNodesWithProbability; + +struct SubMatching +{ + std::vector nodes; + unsigned begin_idx; + unsigned end_idx; + double length; + double confidence; +}; + +using CandidateList = std::vector>; +using CandidateLists = std::vector; +using SubMatchingList = std::vector; constexpr static const unsigned max_number_of_candidates = 20; } @@ -313,8 +323,7 @@ template class MapMatching final void operator()(const Matching::CandidateLists ×tamp_list, const std::vector coordinate_list, - std::vector& matched_nodes, - float& matched_length, + Matching::SubMatchingList& sub_matchings, JSON::Object& _debug_info) const { BOOST_ASSERT(timestamp_list.size() > 0); @@ -348,6 +357,7 @@ template class MapMatching final _debug_states.values.push_back(_debug_timestamps); } + std::vector split_points; std::vector prev_unbroken_timestamps; prev_unbroken_timestamps.reserve(timestamp_list.size()); prev_unbroken_timestamps.push_back(initial_timestamp); @@ -366,6 +376,8 @@ template class MapMatching final const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; + std::cout << " # " << prev_unbroken_timestamp << " -> " << t << std::endl; + // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { @@ -433,16 +445,24 @@ template class MapMatching final if (model.breakage[t]) { + std::cout << "Broken!" << std::endl; + // TODO we actually don't need to go to the beginning. + // with temporal information we can split after _n_ + // skipped states 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 + // we reached the beginning of the trace and it is still broken + // -> split the trace here else { + split_points.push_back(t); + // note this preserves everything before t model.clear(t); model.initialize(t); + prev_unbroken_timestamps.push_back(t); } } else @@ -451,41 +471,61 @@ template class MapMatching final } } - if (prev_unbroken_timestamps.size() < 1) + if (prev_unbroken_timestamps.size() > 1) { - return; + split_points.push_back(prev_unbroken_timestamps.back()+1); } - 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(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 = last_unbroken_timestamp; i > initial_timestamp; --i) + unsigned sub_matching_begin = initial_timestamp; + for (const unsigned sub_matching_end : split_points) { - if (model.breakage[i]) + Matching::SubMatching matching; + + // matchings that only consist of one candidate are invalid + if (sub_matching_end - sub_matching_begin < 2) + { + sub_matching_begin = sub_matching_end; continue; - reconstructed_indices.emplace_front(i, parent_index); - parent_index = model.parents[i][parent_index]; - } - reconstructed_indices.emplace_front(initial_timestamp, parent_index); + } - matched_length = 0.0f; - matched_nodes.resize(reconstructed_indices.size()); - for (auto i = 0u; i < reconstructed_indices.size(); ++i) - { - auto timestamp_index = reconstructed_indices[i].first; - auto location_index = reconstructed_indices[i].second; + std::cout << sub_matching_begin << " -> " << sub_matching_end << std::endl; - matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; - matched_length += model.path_lengths[timestamp_index][location_index]; + matching.begin_idx = sub_matching_begin; + matching.end_idx = sub_matching_end; - _debug_states.values[timestamp_index] - .get().get().values[location_index] - .get().get().values["chosen"] = true; + // loop through the columns, and only compare the last entry + auto max_element_iter = std::max_element(model.viterbi[sub_matching_end-1].begin(), + model.viterbi[sub_matching_end-1].end()); + + auto parent_index = std::distance(model.viterbi[sub_matching_end-1].begin(), max_element_iter); + std::deque> reconstructed_indices; + for (auto i = sub_matching_end-1; i > sub_matching_begin; --i) + { + if (model.breakage[i]) + continue; + reconstructed_indices.emplace_front(i, parent_index); + parent_index = model.parents[i][parent_index]; + } + reconstructed_indices.emplace_front(initial_timestamp, parent_index); + + matching.length = 0.0f; + matching.nodes.resize(reconstructed_indices.size()); + for (auto i = 0u; i < reconstructed_indices.size(); ++i) + { + auto timestamp_index = reconstructed_indices[i].first; + auto location_index = reconstructed_indices[i].second; + + matching.nodes[i] = timestamp_list[timestamp_index][location_index].first; + matching.length += model.path_lengths[timestamp_index][location_index]; + + _debug_states.values[timestamp_index] + .get().get().values[location_index] + .get().get().values["chosen"] = true; + } + + sub_matchings.push_back(matching); + + sub_matching_begin = sub_matching_end; } JSON::Array _debug_breakage; From fb0ce48f2f14c6eebff6a7f0911f44ce65a96e74 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Thu, 19 Feb 2015 01:05:24 +0100 Subject: [PATCH 25/67] Simplify matching response --- plugins/map_matching.hpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 2dd9545b5..a1947fe91 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -253,12 +253,19 @@ template class MapMatchingPlugin : public BasePlugin descriptor->SetConfig(descriptor_config); descriptor->Run(raw_route, temp_result); + JSON::Array input_points; + for(const auto& n: sub.nodes) { + JSON::Array coord; + coord.values.emplace_back(n.location.lat / COORDINATE_PRECISION); + coord.values.emplace_back(n.location.lon / COORDINATE_PRECISION); + input_points.values.push_back(coord); + } + JSON::Object subtrace; - // via_route compability - subtrace.values["route_geometry"] = temp_result.values["route_geometry"]; + subtrace.values["geometry"] = temp_result.values["route_geometry"]; subtrace.values["confidence"] = sub.confidence; - subtrace.values["via_indicies"] = temp_result.values["via_indicies"]; - subtrace.values["via_points"] = temp_result.values["via_points"]; + subtrace.values["input_points"] = input_points; + subtrace.values["matched_points"] = temp_result.values["via_points"]; traces.values.push_back(subtrace); } From f46b259384477b810204a241d3e74c697a3af1a8 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 20 Feb 2015 00:35:51 +0100 Subject: [PATCH 26/67] Fix splitting traces --- plugins/map_matching.hpp | 2 + routing_algorithms/map_matching.hpp | 102 +++++++++++++++++----------- 2 files changed, 65 insertions(+), 39 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index a1947fe91..a58b95836 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -205,6 +205,8 @@ template class MapMatchingPlugin : public BasePlugin sub.confidence = 1-classification.second; } + BOOST_ASSERT(sub.nodes.size() > 1); + // FIXME this is a pretty bad hack. Geometries should obtained directly // from map_matching. // run shortest path routing to obtain geometry diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 92c05bc67..3eb375efd 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -83,7 +83,12 @@ struct SubMatching using CandidateList = std::vector>; using CandidateLists = std::vector; using SubMatchingList = std::vector; -constexpr static const unsigned max_number_of_candidates = 20; +constexpr static const unsigned max_number_of_candidates = 10; +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(); +// FIXME that should be a time threshold. +constexpr static const unsigned MAX_BROKEN_STATES = 6; } // implements a hidden markov model map matching algorithm @@ -243,15 +248,13 @@ template class MapMatching final struct HiddenMarkovModel { std::vector> viterbi; - std::vector> parents; + 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()) @@ -277,8 +280,8 @@ template class MapMatching final 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(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); } @@ -294,8 +297,8 @@ template class MapMatching final 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; + 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]; @@ -304,7 +307,12 @@ template class MapMatching final ++initial_timestamp; } while (breakage[initial_timestamp - 1]); - BOOST_ASSERT(initial_timestamp > 0 && initial_timestamp < viterbi.size()); + if (initial_timestamp >= viterbi.size()) + { + return Matching::INVALID_STATE; + } + + BOOST_ASSERT(initial_timestamp > 0); --initial_timestamp; BOOST_ASSERT(breakage[initial_timestamp] == false); @@ -331,6 +339,10 @@ template class MapMatching final HiddenMarkovModel model(timestamp_list); unsigned initial_timestamp = model.initialize(0); + if (initial_timestamp == Matching::INVALID_STATE) + { + return; + } JSON::Array _debug_states; for (unsigned t = 0; t < timestamp_list.size(); t++) @@ -344,7 +356,7 @@ template class MapMatching final 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["viterbi"] = makeJSONSafe(Matching::IMPOSSIBLE_LOG_PROB); _debug_state.values["pruned"] = 0u; } else if (t == initial_timestamp) @@ -376,8 +388,6 @@ template class MapMatching final const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; - std::cout << " # " << prev_unbroken_timestamp << " -> " << t << std::endl; - // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { @@ -425,7 +435,7 @@ template class MapMatching final if (new_value > current_viterbi[s_prime]) { current_viterbi[s_prime] = new_value; - current_parents[s_prime] = s; + 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; @@ -445,24 +455,25 @@ template class MapMatching final if (model.breakage[t]) { - std::cout << "Broken!" << std::endl; - // TODO we actually don't need to go to the beginning. - // with temporal information we can split after _n_ - // skipped states - if (prev_unbroken_timestamps.size() > 1) - { - // remove both ends of the breakge - prev_unbroken_timestamps.pop_back(); - } + BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); + + // remove both ends of the breakage + prev_unbroken_timestamps.pop_back(); + // we reached the beginning of the trace and it is still broken // -> split the trace here - else + if (prev_unbroken_timestamps.size() < 1 || t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES) { split_points.push_back(t); // note this preserves everything before t model.clear(t); - model.initialize(t); - prev_unbroken_timestamps.push_back(t); + unsigned new_start = model.initialize(t); + // no new start was found -> stop viterbi calculation + if (new_start == Matching::INVALID_STATE) + { + break; + } + prev_unbroken_timestamps.push_back(new_start); } } else @@ -471,7 +482,7 @@ template class MapMatching final } } - if (prev_unbroken_timestamps.size() > 1) + if (prev_unbroken_timestamps.size() > 0) { split_points.push_back(prev_unbroken_timestamps.back()+1); } @@ -481,32 +492,45 @@ template class MapMatching final { Matching::SubMatching matching; + // find real end of trace + // not sure if this is really needed + unsigned parent_timestamp_index = sub_matching_end-1; + while (parent_timestamp_index >= sub_matching_begin && model.breakage[parent_timestamp_index]) + { + parent_timestamp_index--; + } + // matchings that only consist of one candidate are invalid - if (sub_matching_end - sub_matching_begin < 2) + if (parent_timestamp_index - sub_matching_begin < 2) { sub_matching_begin = sub_matching_end; continue; } - std::cout << sub_matching_begin << " -> " << sub_matching_end << std::endl; - matching.begin_idx = sub_matching_begin; - matching.end_idx = sub_matching_end; + matching.end_idx = parent_timestamp_index; // loop through the columns, and only compare the last entry - auto max_element_iter = std::max_element(model.viterbi[sub_matching_end-1].begin(), - model.viterbi[sub_matching_end-1].end()); + auto max_element_iter = std::max_element(model.viterbi[parent_timestamp_index].begin(), + model.viterbi[parent_timestamp_index].end()); - auto parent_index = std::distance(model.viterbi[sub_matching_end-1].begin(), max_element_iter); - std::deque> reconstructed_indices; - for (auto i = sub_matching_end-1; i > sub_matching_begin; --i) + unsigned parent_candidate_index = std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter); + std::deque> reconstructed_indices; + while (parent_timestamp_index > sub_matching_begin) { - if (model.breakage[i]) + if (model.breakage[parent_timestamp_index]) + { continue; - reconstructed_indices.emplace_front(i, parent_index); - parent_index = model.parents[i][parent_index]; + } + reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); + parent_timestamp_index = model.parents[parent_timestamp_index][parent_candidate_index].first; + parent_candidate_index = model.parents[parent_timestamp_index][parent_candidate_index].second; + } + reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); + if (reconstructed_indices.size() < 2) + { + continue; } - reconstructed_indices.emplace_front(initial_timestamp, parent_index); matching.length = 0.0f; matching.nodes.resize(reconstructed_indices.size()); From 89460dd39c64ca6e43625505e0e6021debf6b76a Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 20 Feb 2015 01:13:44 +0100 Subject: [PATCH 27/67] Return indices instead of points --- plugins/map_matching.hpp | 16 +++++++--------- routing_algorithms/map_matching.hpp | 8 +++----- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index a58b95836..57717581c 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -192,10 +192,10 @@ template class MapMatchingPlugin : public BasePlugin for (auto& sub : sub_matchings) { // classify result - double trace_length = sub_trace_lengths[sub.end_idx-1] - sub_trace_lengths[sub.begin_idx]; + double trace_length = sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()]; TraceClassification classification = classify(trace_length, sub.length, - (sub.end_idx - sub.begin_idx) - sub.nodes.size()); + (sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size()); if (classification.first == ClassifierT::ClassLabel::POSITIVE) { sub.confidence = classification.second; @@ -255,18 +255,16 @@ template class MapMatchingPlugin : public BasePlugin descriptor->SetConfig(descriptor_config); descriptor->Run(raw_route, temp_result); - JSON::Array input_points; - for(const auto& n: sub.nodes) { - JSON::Array coord; - coord.values.emplace_back(n.location.lat / COORDINATE_PRECISION); - coord.values.emplace_back(n.location.lon / COORDINATE_PRECISION); - input_points.values.push_back(coord); + JSON::Array indices; + for (const auto& i : sub.indices) + { + indices.values.emplace_back(i); } JSON::Object subtrace; subtrace.values["geometry"] = temp_result.values["route_geometry"]; subtrace.values["confidence"] = sub.confidence; - subtrace.values["input_points"] = input_points; + subtrace.values["indices"] = indices; subtrace.values["matched_points"] = temp_result.values["via_points"]; traces.values.push_back(subtrace); diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 3eb375efd..6f766aefb 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -74,8 +74,7 @@ namespace Matching struct SubMatching { std::vector nodes; - unsigned begin_idx; - unsigned end_idx; + std::vector indices; double length; double confidence; }; @@ -507,9 +506,6 @@ template class MapMatching final continue; } - matching.begin_idx = sub_matching_begin; - matching.end_idx = parent_timestamp_index; - // loop through the columns, and only compare the last entry auto max_element_iter = std::max_element(model.viterbi[parent_timestamp_index].begin(), model.viterbi[parent_timestamp_index].end()); @@ -534,11 +530,13 @@ template class MapMatching final matching.length = 0.0f; matching.nodes.resize(reconstructed_indices.size()); + matching.indices.resize(reconstructed_indices.size()); for (auto i = 0u; i < reconstructed_indices.size(); ++i) { auto timestamp_index = reconstructed_indices[i].first; auto location_index = reconstructed_indices[i].second; + matching.indices[i] = timestamp_index; matching.nodes[i] = timestamp_list[timestamp_index][location_index].first; matching.length += model.path_lengths[timestamp_index][location_index]; From d429485f0c3e5ff53e6878492898aa4be3882c65 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 20 Feb 2015 02:31:48 +0100 Subject: [PATCH 28/67] Fix stupid error in backtracking --- routing_algorithms/map_matching.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 6f766aefb..ceca02214 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -511,6 +511,7 @@ template class MapMatching final model.viterbi[parent_timestamp_index].end()); unsigned parent_candidate_index = std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter); + std::deque> reconstructed_indices; while (parent_timestamp_index > sub_matching_begin) { @@ -518,9 +519,11 @@ template class MapMatching final { continue; } + reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); - parent_timestamp_index = model.parents[parent_timestamp_index][parent_candidate_index].first; - parent_candidate_index = model.parents[parent_timestamp_index][parent_candidate_index].second; + const auto& next = model.parents[parent_timestamp_index][parent_candidate_index]; + parent_timestamp_index = next.first; + parent_candidate_index = next.second; } reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); if (reconstructed_indices.size() < 2) From 70703c39f3424574cb686d29683e8b09ac12201c Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 20 Feb 2015 15:21:50 +0100 Subject: [PATCH 29/67] Add timestamp parameters and reset to beginning of breakage --- data_structures/route_parameters.cpp | 9 +++ include/osrm/route_parameters.hpp | 3 + plugins/map_matching.hpp | 7 ++- routing_algorithms/map_matching.hpp | 82 ++++++++++++++++++---------- server/api_grammar.hpp | 6 +- 5 files changed, 76 insertions(+), 31 deletions(-) diff --git a/data_structures/route_parameters.cpp b/data_structures/route_parameters.cpp index 9416a6d69..7429e4902 100644 --- a/data_structures/route_parameters.cpp +++ b/data_structures/route_parameters.cpp @@ -99,6 +99,15 @@ void RouteParameters::addHint(const std::string &hint) } } +void RouteParameters::addTimestamp(const unsigned timestamp) +{ + timestamps.resize(coordinates.size()); + if (!timestamps.empty()) + { + timestamps.back() = timestamp; + } +} + void RouteParameters::setLanguage(const std::string &language_string) { language = language_string; diff --git a/include/osrm/route_parameters.hpp b/include/osrm/route_parameters.hpp index 04538986e..5fe454cee 100644 --- a/include/osrm/route_parameters.hpp +++ b/include/osrm/route_parameters.hpp @@ -63,6 +63,8 @@ struct RouteParameters void addHint(const std::string &hint); + void addTimestamp(const unsigned timestamp); + void setLanguage(const std::string &language); void setGeometryFlag(const bool flag); @@ -85,6 +87,7 @@ struct RouteParameters std::string jsonp_parameter; std::string language; std::vector hints; + std::vector timestamps; std::vector uturns; std::vector coordinates; }; diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 57717581c..03ada65bc 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -172,6 +172,11 @@ template class MapMatchingPlugin : public BasePlugin std::vector sub_trace_lengths; 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()) + { + return 400; + } bool found_candidates = get_candiates(input_coords, sub_trace_lengths, candidates_lists); if (!found_candidates) { @@ -181,7 +186,7 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching JSON::Object debug_info; Matching::SubMatchingList sub_matchings; - search_engine_ptr->map_matching(candidates_lists, input_coords, sub_matchings, debug_info); + search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, sub_matchings, debug_info); if (1 > sub_matchings.size()) { diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index ceca02214..03ec5d097 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -86,8 +86,8 @@ constexpr static const unsigned max_number_of_candidates = 10; 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(); -// FIXME that should be a time threshold. constexpr static const unsigned MAX_BROKEN_STATES = 6; +constexpr static const unsigned MAX_BROKEN_TIME = 180; } // implements a hidden markov model map matching algorithm @@ -252,14 +252,14 @@ template class MapMatching final std::vector> pruned; std::vector breakage; - const Matching::CandidateLists& timestamp_list; + const Matching::CandidateLists& candidates_list; - HiddenMarkovModel(const Matching::CandidateLists& timestamp_list) - : breakage(timestamp_list.size()) - , timestamp_list(timestamp_list) + HiddenMarkovModel(const Matching::CandidateLists& candidates_list) + : breakage(candidates_list.size()) + , candidates_list(candidates_list) { - for (const auto& l : timestamp_list) + for (const auto& l : candidates_list) { viterbi.emplace_back(l.size()); parents.emplace_back(l.size()); @@ -289,13 +289,13 @@ template class MapMatching final unsigned initialize(unsigned initial_timestamp) { - BOOST_ASSERT(initial_timestamp < timestamp_list.size()); + BOOST_ASSERT(initial_timestamp < candidates_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); + viterbi[initial_timestamp][s] = log_emission_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; @@ -328,14 +328,15 @@ template class MapMatching final } - void operator()(const Matching::CandidateLists ×tamp_list, - const std::vector coordinate_list, + void operator()(const Matching::CandidateLists &candidates_list, + const std::vector& trace_coordinates, + const std::vector& trace_timestamps, Matching::SubMatchingList& sub_matchings, JSON::Object& _debug_info) const { - BOOST_ASSERT(timestamp_list.size() > 0); + BOOST_ASSERT(candidates_list.size() > 0); - HiddenMarkovModel model(timestamp_list); + HiddenMarkovModel model(candidates_list); unsigned initial_timestamp = model.initialize(0); if (initial_timestamp == Matching::INVALID_STATE) @@ -344,15 +345,15 @@ template class MapMatching final } JSON::Array _debug_states; - for (unsigned t = 0; t < timestamp_list.size(); t++) + for (unsigned t = 0; t < candidates_list.size(); t++) { JSON::Array _debug_timestamps; - for (unsigned s = 0; s < timestamp_list[t].size(); s++) + for (unsigned s = 0; s < candidates_list[t].size(); s++) { JSON::Object _debug_state; _debug_state.values["transitions"] = JSON::Array(); - _debug_state.values["coordinate"] = makeJSONArray(timestamp_list[t][s].first.location.lat / COORDINATE_PRECISION, - timestamp_list[t][s].first.location.lon / COORDINATE_PRECISION); + _debug_state.values["coordinate"] = makeJSONArray(candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, + candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); if (t < initial_timestamp) { _debug_state.values["viterbi"] = makeJSONSafe(Matching::IMPOSSIBLE_LOG_PROB); @@ -368,24 +369,25 @@ template class MapMatching final _debug_states.values.push_back(_debug_timestamps); } + unsigned breakage_begin = std::numeric_limits::max(); std::vector split_points; std::vector prev_unbroken_timestamps; - prev_unbroken_timestamps.reserve(timestamp_list.size()); + prev_unbroken_timestamps.reserve(candidates_list.size()); prev_unbroken_timestamps.push_back(initial_timestamp); - for (auto t = initial_timestamp + 1; t < timestamp_list.size(); ++t) + for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t) { 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]; + const auto& prev_unbroken_timestamps_list = candidates_list[prev_unbroken_timestamp]; + const auto& prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; 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]; + const auto& current_timestamps_list = candidates_list[t]; + const auto& current_coordinate = trace_coordinates[t]; // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) @@ -396,7 +398,7 @@ template class MapMatching final for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { // how likely is candidate s_prime at time t to be emitted? - const double emission_pr = log_emission_probability(timestamp_list[t][s_prime].second); + const double emission_pr = log_emission_probability(candidates_list[t][s_prime].second); double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) continue; @@ -456,23 +458,47 @@ template class MapMatching final { BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); + // save start of breakage -> we need this as split point + if (t < breakage_begin) + { + breakage_begin = t; + } + // remove both ends of the breakage prev_unbroken_timestamps.pop_back(); + bool trace_split = prev_unbroken_timestamps.size() < 1; + + // use temporal information to determine a split if available + if (trace_timestamps.size() > 0) + { + trace_split = trace_split || (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > Matching::MAX_BROKEN_TIME); + } + else + { + trace_split = trace_split || (t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES); + } + // we reached the beginning of the trace and it is still broken // -> split the trace here - if (prev_unbroken_timestamps.size() < 1 || t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES) + if (trace_split) { - split_points.push_back(t); + split_points.push_back(breakage_begin); // note this preserves everything before t - model.clear(t); - unsigned new_start = model.initialize(t); + model.clear(breakage_begin); + unsigned new_start = model.initialize(breakage_begin); // no new start was found -> stop viterbi calculation if (new_start == Matching::INVALID_STATE) { break; } + prev_unbroken_timestamps.clear(); prev_unbroken_timestamps.push_back(new_start); + // Important: We potentially go back here! + // However since t+1 > new_start >= breakge_begin + // we can only reset trace_coordindates.size() times. + t = new_start; + breakage_begin = std::numeric_limits::max(); } } else @@ -540,7 +566,7 @@ template class MapMatching final auto location_index = reconstructed_indices[i].second; matching.indices[i] = timestamp_index; - matching.nodes[i] = timestamp_list[timestamp_index][location_index].first; + matching.nodes[i] = candidates_list[timestamp_index][location_index].first; matching.length += model.path_lengths[timestamp_index][location_index]; _debug_states.values[timestamp_index] diff --git a/server/api_grammar.hpp b/server/api_grammar.hpp index fecbb92cc..3d7f4ea4b 100644 --- a/server/api_grammar.hpp +++ b/server/api_grammar.hpp @@ -40,7 +40,7 @@ template struct APIGrammar : qi::grammar> string[boost::bind(&HandlerT::setService, handler, ::_1)] >> *(query) >> -(uturns); - query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | u | cmp | + query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | u | cmp | language | instruction | geometry | alt_route | old_API | num_results)); zoom = (-qi::lit('&')) >> qi::lit('z') >> '=' >> @@ -62,6 +62,8 @@ template struct APIGrammar : qi::grammar> qi::lit("hint") >> '=' >> stringwithDot[boost::bind(&HandlerT::addHint, handler, ::_1)]; + timestamp = (-qi::lit('&')) >> qi::lit("t") >> '=' >> + qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)]; u = (-qi::lit('&')) >> qi::lit("u") >> '=' >> qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)]; uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >> @@ -83,7 +85,7 @@ template struct APIGrammar : qi::grammar api_call, query; qi::rule service, zoom, output, string, jsonp, checksum, location, - hint, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, + hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, uturns, old_API, num_results; HandlerT *handler; From c4f193b13ee451c572cfb6e6ace3000b6d4123c9 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 20 Feb 2015 15:24:51 +0100 Subject: [PATCH 30/67] Fix comment --- routing_algorithms/map_matching.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 03ec5d097..2f1e618c1 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -484,7 +484,7 @@ template class MapMatching final if (trace_split) { split_points.push_back(breakage_begin); - // note this preserves everything before t + // note: this preserves everything before breakage_begin model.clear(breakage_begin); unsigned new_start = model.initialize(breakage_begin); // no new start was found -> stop viterbi calculation From fd6c70afe14aa13cbb606f97cfe00a843a8439a6 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 21 Feb 2015 14:54:29 +0100 Subject: [PATCH 31/67] Fix regression in sub-matching length check --- routing_algorithms/map_matching.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 2f1e618c1..71d8686dd 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -526,7 +526,7 @@ template class MapMatching final } // matchings that only consist of one candidate are invalid - if (parent_timestamp_index - sub_matching_begin < 2) + if (parent_timestamp_index - sub_matching_begin + 1 < 2) { sub_matching_begin = sub_matching_end; continue; @@ -554,6 +554,7 @@ template class MapMatching final reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); if (reconstructed_indices.size() < 2) { + sub_matching_begin = sub_matching_end; continue; } From 0e6ed53cee4ebfe1a749f6047eda9b80ab268f1b Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 22 Feb 2015 17:13:08 +0100 Subject: [PATCH 32/67] Adapt to JSON container rename --- plugins/map_matching.hpp | 12 ++++++------ routing_algorithms/map_matching.hpp | 26 +++++++++++++------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 03ada65bc..1aa362120 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -161,7 +161,7 @@ template class MapMatchingPlugin : public BasePlugin return true; } - int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final + int HandleRequest(const RouteParameters &route_parameters, osrm::json::Object &json_result) final { // check number of parameters if (!check_all_coordinates(route_parameters.coordinates)) @@ -184,7 +184,7 @@ template class MapMatchingPlugin : public BasePlugin } // call the actual map matching - JSON::Object debug_info; + osrm::json::Object debug_info; Matching::SubMatchingList sub_matchings; search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, sub_matchings, debug_info); @@ -193,7 +193,7 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - JSON::Array traces; + osrm::json::Array traces; for (auto& sub : sub_matchings) { // classify result @@ -256,17 +256,17 @@ template class MapMatchingPlugin : public BasePlugin break; } - JSON::Object temp_result; + osrm::json::Object temp_result; descriptor->SetConfig(descriptor_config); descriptor->Run(raw_route, temp_result); - JSON::Array indices; + osrm::json::Array indices; for (const auto& i : sub.indices) { indices.values.emplace_back(i); } - JSON::Object subtrace; + osrm::json::Object subtrace; subtrace.values["geometry"] = temp_result.values["route_geometry"]; subtrace.values["confidence"] = sub.confidence; subtrace.values["indices"] = indices; diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 71d8686dd..a9fdd1676 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -35,8 +35,8 @@ or see http://www.gnu.org/licenses/agpl.txt. #include -using JSONVariantArray = mapbox::util::recursive_wrapper; -using JSONVariantObject = mapbox::util::recursive_wrapper; +using JSONVariantArray = mapbox::util::recursive_wrapper; +using JSONVariantObject = mapbox::util::recursive_wrapper; template T makeJSONSafe(T d) @@ -51,19 +51,19 @@ T makeJSONSafe(T d) return d; } -void appendToJSONArray(JSON::Array& a) { } +void appendToJSONArray(osrm::json::Array& a) { } template -void appendToJSONArray(JSON::Array& a, T value, Args... args) +void appendToJSONArray(osrm::json::Array& a, T value, Args... args) { a.values.emplace_back(value); appendToJSONArray(a, args...); } template -JSON::Array makeJSONArray(Args... args) +osrm::json::Array makeJSONArray(Args... args) { - JSON::Array a; + osrm::json::Array a; appendToJSONArray(a, args...); return a; } @@ -332,7 +332,7 @@ template class MapMatching final const std::vector& trace_coordinates, const std::vector& trace_timestamps, Matching::SubMatchingList& sub_matchings, - JSON::Object& _debug_info) const + osrm::json::Object& _debug_info) const { BOOST_ASSERT(candidates_list.size() > 0); @@ -344,14 +344,14 @@ template class MapMatching final return; } - JSON::Array _debug_states; + osrm::json::Array _debug_states; for (unsigned t = 0; t < candidates_list.size(); t++) { - JSON::Array _debug_timestamps; + osrm::json::Array _debug_timestamps; for (unsigned s = 0; s < candidates_list[t].size(); s++) { - JSON::Object _debug_state; - _debug_state.values["transitions"] = JSON::Array(); + osrm::json::Object _debug_state; + _debug_state.values["transitions"] = osrm::json::Array(); _debug_state.values["coordinate"] = makeJSONArray(candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); if (t < initial_timestamp) @@ -419,7 +419,7 @@ template class MapMatching final const double transition_pr = log_transition_probability(d_t, beta); new_value += transition_pr; - JSON::Object _debug_transistion; + osrm::json::Object _debug_transistion; _debug_transistion.values["to"] = makeJSONArray(t, s_prime); _debug_transistion.values["properties"] = makeJSONArray( makeJSONSafe(prev_viterbi[s]), @@ -580,7 +580,7 @@ template class MapMatching final sub_matching_begin = sub_matching_end; } - JSON::Array _debug_breakage; + osrm::json::Array _debug_breakage; for (auto b : model.breakage) { _debug_breakage.values.push_back(static_cast(b)); } From dec73b02e926154e385cd82e59ff196b7cacfd62 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 22 Feb 2015 23:21:37 +0100 Subject: [PATCH 33/67] Rename traces to matchings in response --- plugins/map_matching.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 1aa362120..d4fa22dd9 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -193,7 +193,7 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - osrm::json::Array traces; + osrm::json::Array matchings; for (auto& sub : sub_matchings) { // classify result @@ -272,11 +272,11 @@ template class MapMatchingPlugin : public BasePlugin subtrace.values["indices"] = indices; subtrace.values["matched_points"] = temp_result.values["via_points"]; - traces.values.push_back(subtrace); + matchings.values.push_back(subtrace); } json_result.values["debug"] = debug_info; - json_result.values["traces"] = traces; + json_result.values["matchings"] = matchings; return 200; } From 2115a67d24cf9008a5056bcf9c0b47ad2f896836 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 23 Feb 2015 11:59:27 +0100 Subject: [PATCH 34/67] Link libOSRM with compute_angle --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3dac8ba9..bbebb485a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,7 @@ list(REMOVE_ITEM DatastructureGlob data_structures/Coordinate.cpp) file(GLOB CoordinateGlob data_structures/coordinate*.cpp) file(GLOB AlgorithmGlob algorithms/*.cpp) file(GLOB HttpGlob server/http/*.cpp) -file(GLOB LibOSRMGlob library/*.cpp) +file(GLOB LibOSRMGlob library/*.cpp util/compute_angle.cpp) file(GLOB DataStructureTestsGlob unit_tests/data_structures/*.cpp data_structures/hilbert_value.cpp) file(GLOB AlgorithmTestsGlob unit_tests/algorithms/*.cpp) From a2c88b607fd47c12d38ff39800529a0192e8025c Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Thu, 26 Feb 2015 18:50:18 +0100 Subject: [PATCH 35/67] lint corrections - fix license header - let shared_ptr autodestruct as it's shared - rename fences to resemble file names - reorder includes into lexicographic order --- plugins/map_matching.hpp | 57 ++++++++++++++++------------- routing_algorithms/map_matching.hpp | 49 ++++++++++++++----------- 2 files changed, 60 insertions(+), 46 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index d4fa22dd9..b4f65ad1e 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -1,39 +1,46 @@ - /* - 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. +Copyright (c) 2015, Project OSRM contributors +All rights reserved. -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. +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: -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. - */ +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. -#ifndef MAP_MATCHING_PLUGIN_H -#define MAP_MATCHING_PLUGIN_H +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 MAP_MATCHING_HPP +#define MAP_MATCHING_HPP #include "plugin_base.hpp" #include "../algorithms/bayes_classifier.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/compute_angle.hpp" -#include "../util/simple_logger.hpp" -#include "../util/string_util.hpp" #include "../descriptors/descriptor_base.hpp" #include "../descriptors/gpx_descriptor.hpp" #include "../descriptors/json_descriptor.hpp" +#include "../routing_algorithms/map_matching.hpp" +#include "../util/compute_angle.hpp" +#include "../util/integer_range.hpp" +#include "../util/simple_logger.hpp" +#include "../util/string_util.hpp" #include @@ -65,7 +72,7 @@ template class MapMatchingPlugin : public BasePlugin search_engine_ptr = std::make_shared>(facade); } - virtual ~MapMatchingPlugin() { search_engine_ptr.reset(); } + virtual ~MapMatchingPlugin() { } const std::string GetDescriptor() const final { return descriptor_string; } @@ -287,4 +294,4 @@ template class MapMatchingPlugin : public BasePlugin ClassifierT classifier; }; -#endif /* MAP_MATCHING_PLUGIN_H */ +#endif /* MAP_MATCHING_HPP */ diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index a9fdd1676..f9691d1b2 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -1,40 +1,47 @@ /* - 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. +Copyright (c) 2015, Project OSRM contributors +All rights reserved. -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. +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: -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. - */ +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. -#ifndef MAP_MATCHING_H -#define MAP_MATCHING_H +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 MAP_MATCHING_HPP +#define MAP_MATCHING_HPP #include "routing_base.hpp" #include "../data_structures/coordinate_calculation.hpp" #include "../util/simple_logger.hpp" -#include #include +#include + +#include #include #include #include -#include - using JSONVariantArray = mapbox::util::recursive_wrapper; using JSONVariantObject = mapbox::util::recursive_wrapper; @@ -592,4 +599,4 @@ template class MapMatching final //[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM GIS -#endif /* MAP_MATCHING_H */ +#endif /* MAP_MATCHING_HPP */ From adbca39fef3402e5ce3b3b906c844fcd5a1d0472 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 27 Feb 2015 10:59:06 +0100 Subject: [PATCH 36/67] Fix include guard --- plugins/map_matching.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index b4f65ad1e..d3fcccac3 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -25,8 +25,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MAP_MATCHING_HPP -#define MAP_MATCHING_HPP +#ifndef MAP_MATCHING_PLUGIN_HPP +#define MAP_MATCHING_PLUGIN_HPP #include "plugin_base.hpp" From e8e637c4f2bb7247800808a32b8600d01c1cb537 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 27 Feb 2015 10:59:36 +0100 Subject: [PATCH 37/67] Replace descriptor code with code that generates only geometry --- plugins/map_matching.hpp | 105 ++++++++++++++++++++------------------- 1 file changed, 53 insertions(+), 52 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index d3fcccac3..3868572ff 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -52,7 +52,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. template class MapMatchingPlugin : public BasePlugin { private: - std::unordered_map descriptor_table; std::shared_ptr> search_engine_ptr; using ClassifierT = BayesClassifier; @@ -68,7 +67,6 @@ template class MapMatchingPlugin : public BasePlugin LaplaceDistribution(0.11467696742821254, 0.49918444000368756), 0.7977883096366508) // valid apriori probability { - descriptor_table.emplace("json", 0); search_engine_ptr = std::make_shared>(facade); } @@ -97,7 +95,7 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - bool get_candiates(const std::vector& input_coords, std::vector& sub_trace_lengths, Matching::CandidateLists& candidates_lists) + bool getCandiates(const std::vector& input_coords, std::vector& sub_trace_lengths, Matching::CandidateLists& candidates_lists) { double last_distance = coordinate_calculation::great_circle_distance( input_coords[0], @@ -168,6 +166,54 @@ template class MapMatchingPlugin : public BasePlugin return true; } + osrm::json::Object submatchingToJSON(const Matching::SubMatching& sub, const RouteParameters& route_parameters, const InternalRouteResult& raw_route) + { + osrm::json::Object subtrace; + + subtrace.values["confidence"] = sub.confidence; + + if (route_parameters.geometry) + { + DescriptionFactory factory; + FixedPointCoordinate current_coordinate; + factory.SetStartSegment( + raw_route.segment_end_coordinates.front().source_phantom, + raw_route.source_traversed_in_reverse.front()); + for (const auto i : osrm::irange(0, raw_route.unpacked_path_segments.size())) + { + for (const PathData &path_data : raw_route.unpacked_path_segments[i]) + { + current_coordinate = facade->GetCoordinateOfNode(path_data.node); + factory.AppendSegment(current_coordinate, path_data); + } + factory.SetEndSegment(raw_route.segment_end_coordinates[i].target_phantom, + raw_route.target_traversed_in_reverse[i], + raw_route.is_via_leg(i)); + } + subtrace.values["geometry"] = factory.AppendGeometryString(route_parameters.compression); + } + + osrm::json::Array indices; + for (const auto& i : sub.indices) + { + indices.values.emplace_back(i); + } + subtrace.values["indices"] = indices; + + + osrm::json::Array points; + for (const auto& node : sub.nodes) + { + osrm::json::Array coordinate; + coordinate.values.emplace_back(node.location.lat / COORDINATE_PRECISION); + coordinate.values.emplace_back(node.location.lon / COORDINATE_PRECISION); + points.values.emplace_back(coordinate); + } + subtrace.values["matched_points"] = points; + + return subtrace; + } + int HandleRequest(const RouteParameters &route_parameters, osrm::json::Object &json_result) final { // check number of parameters @@ -184,7 +230,7 @@ template class MapMatchingPlugin : public BasePlugin { return 400; } - bool found_candidates = get_candiates(input_coords, sub_trace_lengths, candidates_lists); + bool found_candidates = getCandiates(input_coords, sub_trace_lengths, candidates_lists); if (!found_candidates) { return 400; @@ -219,9 +265,8 @@ template class MapMatchingPlugin : public BasePlugin BOOST_ASSERT(sub.nodes.size() > 1); - // FIXME this is a pretty bad hack. Geometries should obtained directly - // from map_matching. - // run shortest path routing to obtain geometry + // FIXME we only run this to obtain the geometry + // The clean way would be to get this directly from the map matching plugin InternalRouteResult raw_route; PhantomNodes current_phantom_node_pair; for (unsigned i = 0; i < sub.nodes.size() - 1; ++i) @@ -235,51 +280,7 @@ template class MapMatchingPlugin : public BasePlugin std::vector(raw_route.segment_end_coordinates.size(), true), raw_route); - - DescriptorConfig descriptor_config; - - auto iter = descriptor_table.find(route_parameters.output_format); - unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); - - descriptor_config.zoom_level = route_parameters.zoom_level; - descriptor_config.instructions = false; - descriptor_config.geometry = route_parameters.geometry; - descriptor_config.encode_geometry = route_parameters.compression; - - std::shared_ptr> descriptor; - switch (descriptor_type) - { - // case 0: - // descriptor = std::make_shared>(); - // break; - case 1: - descriptor = std::make_shared>(facade); - break; - // case 2: - // descriptor = std::make_shared>(); - // break; - default: - descriptor = std::make_shared>(facade); - break; - } - - osrm::json::Object temp_result; - descriptor->SetConfig(descriptor_config); - descriptor->Run(raw_route, temp_result); - - osrm::json::Array indices; - for (const auto& i : sub.indices) - { - indices.values.emplace_back(i); - } - - osrm::json::Object subtrace; - subtrace.values["geometry"] = temp_result.values["route_geometry"]; - subtrace.values["confidence"] = sub.confidence; - subtrace.values["indices"] = indices; - subtrace.values["matched_points"] = temp_result.values["via_points"]; - - matchings.values.push_back(subtrace); + matchings.values.emplace_back(submatchingToJSON(sub, route_parameters, raw_route)); } json_result.values["debug"] = debug_info; From 34d5d353af71918e474672b926fd2ad3fd827f93 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Fri, 27 Feb 2015 11:17:19 +0100 Subject: [PATCH 38/67] Apply clang-format and split out json_util --- routing_algorithms/map_matching.hpp | 199 ++++++++++++++-------------- util/json_util.hpp | 75 +++++++++++ 2 files changed, 174 insertions(+), 100 deletions(-) create mode 100644 util/json_util.hpp diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index f9691d1b2..afa1b2811 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "../data_structures/coordinate_calculation.hpp" #include "../util/simple_logger.hpp" +#include "../util/json_util.hpp" #include #include @@ -45,36 +46,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using JSONVariantArray = mapbox::util::recursive_wrapper; using JSONVariantObject = mapbox::util::recursive_wrapper; -template -T makeJSONSafe(T d) -{ - if (std::isnan(d) || std::numeric_limits::infinity() == d) { - return std::numeric_limits::max(); - } - if (-std::numeric_limits::infinity() == d) { - return -std::numeric_limits::max(); - } - - return d; -} - -void appendToJSONArray(osrm::json::Array& a) { } - -template -void appendToJSONArray(osrm::json::Array& a, T value, Args... args) -{ - a.values.emplace_back(value); - appendToJSONArray(a, args...); -} - -template -osrm::json::Array makeJSONArray(Args... args) -{ - osrm::json::Array a; - appendToJSONArray(a, args...); - return a; -} - namespace Matching { @@ -86,7 +57,7 @@ struct SubMatching double confidence; }; -using CandidateList = std::vector>; +using CandidateList = std::vector>; using CandidateLists = std::vector; using SubMatchingList = std::vector; constexpr static const unsigned max_number_of_candidates = 10; @@ -133,8 +104,8 @@ template class MapMatching final } // TODO: needs to be estimated from the input locations - // FIXME These values seem wrong. Higher beta for more samples/minute? Should be inverse proportional. - //constexpr static const double beta = 1.; + // FIXME These values seem wrong. Higher beta for more samples/minute? Should be inverse + // proportional. // samples/min and beta // 1 0.49037673 // 2 0.82918373 @@ -239,13 +210,15 @@ template class MapMatching final FixedPointCoordinate previous_coordinate = source_phantom.location; FixedPointCoordinate current_coordinate; distance = 0; - for (const auto& p : unpacked_path) + for (const auto &p : unpacked_path) { current_coordinate = super::facade->GetCoordinateOfNode(p.node); - distance += coordinate_calculation::great_circle_distance(previous_coordinate, current_coordinate); + distance += coordinate_calculation::great_circle_distance(previous_coordinate, + current_coordinate); previous_coordinate = current_coordinate; } - distance += coordinate_calculation::great_circle_distance(previous_coordinate, target_phantom.location); + distance += coordinate_calculation::great_circle_distance(previous_coordinate, + target_phantom.location); } return distance; @@ -259,14 +232,12 @@ template class MapMatching final std::vector> pruned; std::vector breakage; - const Matching::CandidateLists& candidates_list; + const Matching::CandidateLists &candidates_list; - - HiddenMarkovModel(const Matching::CandidateLists& candidates_list) - : breakage(candidates_list.size()) - , candidates_list(candidates_list) + HiddenMarkovModel(const Matching::CandidateLists &candidates_list) + : breakage(candidates_list.size()), candidates_list(candidates_list) { - for (const auto& l : candidates_list) + for (const auto &l : candidates_list) { viterbi.emplace_back(l.size()); parents.emplace_back(l.size()); @@ -279,10 +250,9 @@ template class MapMatching final 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()); + 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++) { @@ -291,7 +261,7 @@ template class MapMatching final 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); + std::fill(breakage.begin() + initial_timestamp, breakage.end(), true); } unsigned initialize(unsigned initial_timestamp) @@ -302,12 +272,14 @@ template class MapMatching final { for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) { - viterbi[initial_timestamp][s] = log_emission_probability(candidates_list[initial_timestamp][s].second); + viterbi[initial_timestamp][s] = + log_emission_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]; + pruned[initial_timestamp][s] = + viterbi[initial_timestamp][s] < Matching::MINIMAL_LOG_PROB; + breakage[initial_timestamp] = + breakage[initial_timestamp] && pruned[initial_timestamp][s]; } ++initial_timestamp; @@ -325,7 +297,6 @@ template class MapMatching final return initial_timestamp; } - }; public: @@ -334,12 +305,11 @@ template class MapMatching final { } - void operator()(const Matching::CandidateLists &candidates_list, - const std::vector& trace_coordinates, - const std::vector& trace_timestamps, - Matching::SubMatchingList& sub_matchings, - osrm::json::Object& _debug_info) const + const std::vector &trace_coordinates, + const std::vector &trace_timestamps, + Matching::SubMatchingList &sub_matchings, + osrm::json::Object &_debug_info) const { BOOST_ASSERT(candidates_list.size() > 0); @@ -359,16 +329,18 @@ template class MapMatching final { osrm::json::Object _debug_state; _debug_state.values["transitions"] = osrm::json::Array(); - _debug_state.values["coordinate"] = makeJSONArray(candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, - candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); + _debug_state.values["coordinate"] = osrm::json::makeArray( + candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, + candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); if (t < initial_timestamp) { - _debug_state.values["viterbi"] = makeJSONSafe(Matching::IMPOSSIBLE_LOG_PROB); + _debug_state.values["viterbi"] = + osrm::json::clampFloat(Matching::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["viterbi"] = osrm::json::clampFloat(model.viterbi[t][s]); _debug_state.values["pruned"] = static_cast(model.pruned[t][s]); } _debug_timestamps.values.push_back(_debug_state); @@ -384,17 +356,17 @@ template class MapMatching final for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t) { 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 = candidates_list[prev_unbroken_timestamp]; - const auto& prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; + 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& 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 = candidates_list[t]; - const auto& current_coordinate = trace_coordinates[t]; + auto ¤t_viterbi = model.viterbi[t]; + auto ¤t_pruned = model.pruned[t]; + auto ¤t_parents = model.parents[t]; + auto ¤t_lengths = model.path_lengths[t]; + const auto ¤t_timestamps_list = candidates_list[t]; + const auto ¤t_coordinate = trace_coordinates[t]; // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) @@ -405,14 +377,16 @@ template class MapMatching final for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { // how likely is candidate s_prime at time t to be emitted? - const double emission_pr = log_emission_probability(candidates_list[t][s_prime].second); + const double emission_pr = + log_emission_probability(candidates_list[t][s_prime].second); double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) continue; // get distance diff between loc1/2 and locs/s_prime - const auto network_distance = get_network_distance(prev_unbroken_timestamps_list[s].first, - current_timestamps_list[s_prime].first); + const auto network_distance = + get_network_distance(prev_unbroken_timestamps_list[s].first, + current_timestamps_list[s_prime].first); const auto great_circle_distance = coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate); @@ -427,18 +401,23 @@ template class MapMatching final new_value += transition_pr; osrm::json::Object _debug_transistion; - _debug_transistion.values["to"] = makeJSONArray(t, s_prime); - _debug_transistion.values["properties"] = makeJSONArray( - makeJSONSafe(prev_viterbi[s]), - makeJSONSafe(emission_pr), - makeJSONSafe(transition_pr), - network_distance, - great_circle_distance - ); + _debug_transistion.values["to"] = osrm::json::makeArray(t, s_prime); + _debug_transistion.values["properties"] = + osrm::json::makeArray(osrm::json::clampFloat(prev_viterbi[s]), + osrm::json::clampFloat(emission_pr), + osrm::json::clampFloat(transition_pr), + network_distance, + great_circle_distance); _debug_states.values[prev_unbroken_timestamp] - .get().get().values[s] - .get().get().values["transitions"] - .get().get().values.push_back(_debug_transistion); + .get() + .get() + .values[s] + .get() + .get() + .values["transitions"] + .get() + .get() + .values.push_back(_debug_transistion); if (new_value > current_viterbi[s_prime]) { @@ -454,11 +433,19 @@ template class MapMatching final for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) { _debug_states.values[t] - .get().get().values[s_prime] - .get().get().values["viterbi"] = makeJSONSafe(current_viterbi[s_prime]); + .get() + .get() + .values[s_prime] + .get() + .get() + .values["viterbi"] = osrm::json::clampFloat(current_viterbi[s_prime]); _debug_states.values[t] - .get().get().values[s_prime] - .get().get().values["pruned"] = static_cast(current_pruned[s_prime]); + .get() + .get() + .values[s_prime] + .get() + .get() + .values["pruned"] = static_cast(current_pruned[s_prime]); } if (model.breakage[t]) @@ -479,11 +466,15 @@ template class MapMatching final // use temporal information to determine a split if available if (trace_timestamps.size() > 0) { - trace_split = trace_split || (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > Matching::MAX_BROKEN_TIME); + trace_split = + trace_split || + (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > + 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() > + Matching::MAX_BROKEN_STATES); } // we reached the beginning of the trace and it is still broken @@ -516,7 +507,7 @@ template class MapMatching final if (prev_unbroken_timestamps.size() > 0) { - split_points.push_back(prev_unbroken_timestamps.back()+1); + split_points.push_back(prev_unbroken_timestamps.back() + 1); } unsigned sub_matching_begin = initial_timestamp; @@ -526,8 +517,9 @@ template class MapMatching final // find real end of trace // not sure if this is really needed - unsigned parent_timestamp_index = sub_matching_end-1; - while (parent_timestamp_index >= sub_matching_begin && model.breakage[parent_timestamp_index]) + unsigned parent_timestamp_index = sub_matching_end - 1; + while (parent_timestamp_index >= sub_matching_begin && + model.breakage[parent_timestamp_index]) { parent_timestamp_index--; } @@ -543,7 +535,8 @@ template class MapMatching final auto max_element_iter = std::max_element(model.viterbi[parent_timestamp_index].begin(), model.viterbi[parent_timestamp_index].end()); - unsigned parent_candidate_index = std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter); + unsigned parent_candidate_index = + std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter); std::deque> reconstructed_indices; while (parent_timestamp_index > sub_matching_begin) @@ -554,7 +547,7 @@ template class MapMatching final } reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index); - const auto& next = model.parents[parent_timestamp_index][parent_candidate_index]; + const auto &next = model.parents[parent_timestamp_index][parent_candidate_index]; parent_timestamp_index = next.first; parent_candidate_index = next.second; } @@ -578,8 +571,12 @@ template class MapMatching final matching.length += model.path_lengths[timestamp_index][location_index]; _debug_states.values[timestamp_index] - .get().get().values[location_index] - .get().get().values["chosen"] = true; + .get() + .get() + .values[location_index] + .get() + .get() + .values["chosen"] = true; } sub_matchings.push_back(matching); @@ -588,7 +585,8 @@ template class MapMatching final } osrm::json::Array _debug_breakage; - for (auto b : model.breakage) { + for (auto b : model.breakage) + { _debug_breakage.values.push_back(static_cast(b)); } @@ -597,6 +595,7 @@ template class MapMatching final } }; -//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM GIS +//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM +//GIS #endif /* MAP_MATCHING_HPP */ diff --git a/util/json_util.hpp b/util/json_util.hpp new file mode 100644 index 000000000..0e362701a --- /dev/null +++ b/util/json_util.hpp @@ -0,0 +1,75 @@ +/* + +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. + +*/ + +// based on +// https://svn.apache.org/repos/asf/mesos/tags/release-0.9.0-incubating-RC0/src/common/json.hpp + +#ifndef JSON_UTIL_HPP +#define JSON_UTIL_HPP + +#include + +#include +#include + +namespace osrm +{ +namespace json +{ + +template T clampFloat(T d) +{ + if (std::isnan(d) || std::numeric_limits::infinity() == d) + { + return std::numeric_limits::max(); + } + if (-std::numeric_limits::infinity() == d) + { + return -std::numeric_limits::max(); + } + + return d; +} + +void appendToArray(osrm::json::Array &a) {} +template +void appendToArray(osrm::json::Array &a, T value, Args... args) +{ + a.values.emplace_back(value); + appendToJSONArray(a, args...); +} + +template osrm::json::Array makeArray(Args... args) +{ + osrm::json::Array a; + appendToJSONArray(a, args...); + return a; +} + +} // namespace json +} // namespace osrm +#endif // JSON_RENDERER_HPP From a760aec791cc5caf47bf54de9db8c4e5a4429566 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 28 Feb 2015 18:44:17 +0100 Subject: [PATCH 39/67] Add json logger to map_matching This adds additional data to the json response, when OSRM is compiled in debug mode. --- plugins/map_matching.hpp | 9 +- routing_algorithms/map_matching.hpp | 203 +++++++++++++++++----------- util/json_logger.hpp | 77 +++++++++++ util/json_util.hpp | 38 +++++- 4 files changed, 239 insertions(+), 88 deletions(-) create mode 100644 util/json_logger.hpp diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 3868572ff..a3ef84563 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "../util/compute_angle.hpp" #include "../util/integer_range.hpp" #include "../util/simple_logger.hpp" +#include "../util/json_logger.hpp" #include "../util/string_util.hpp" #include @@ -237,9 +238,10 @@ template class MapMatchingPlugin : public BasePlugin } // call the actual map matching - osrm::json::Object debug_info; + if (osrm::json::Logger::get()) + osrm::json::Logger::get()->initialize("matching"); Matching::SubMatchingList sub_matchings; - search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, sub_matchings, debug_info); + search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, sub_matchings); if (1 > sub_matchings.size()) { @@ -283,7 +285,8 @@ template class MapMatchingPlugin : public BasePlugin matchings.values.emplace_back(submatchingToJSON(sub, route_parameters, raw_route)); } - json_result.values["debug"] = debug_info; + if (osrm::json::Logger::get()) + osrm::json::Logger::get()->render("matching", json_result); json_result.values["matchings"] = matchings; return 200; diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index afa1b2811..8a751ba1f 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "../data_structures/coordinate_calculation.hpp" #include "../util/simple_logger.hpp" #include "../util/json_util.hpp" +#include "../util/json_logger.hpp" #include #include @@ -43,9 +44,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -using JSONVariantArray = mapbox::util::recursive_wrapper; -using JSONVariantObject = mapbox::util::recursive_wrapper; - namespace Matching { @@ -299,6 +297,119 @@ template class MapMatching final } }; + struct DebugInfo + { + DebugInfo(const osrm::json::Logger* logger) + : logger(logger) + { + if (logger) + { + object = &logger->map->at("matching"); + } + } + + void initialize(const HiddenMarkovModel& model, + unsigned initial_timestamp, + 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); + if (t < initial_timestamp) + { + state.values["viterbi"] = osrm::json::clamp_float(Matching::IMPOSSIBLE_LOG_PROB); + state.values["pruned"] = 0u; + } + else if (t == initial_timestamp) + { + state.values["viterbi"] = osrm::json::clamp_float(model.viterbi[t][s]); + state.values["pruned"] = static_cast(model.pruned[t][s]); + } + 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 add_viterbi(const unsigned t, + const std::vector& current_viterbi, + const std::vector& current_pruned) + { + // json logger not enabled + if (!logger) + return; + + for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) + { + osrm::json::get(*object, "states", t, s_prime, "viterbi") = osrm::json::clamp_float(current_viterbi[s_prime]); + osrm::json::get(*object, "states", t, s_prime, "pruned") = static_cast(current_pruned[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) @@ -308,8 +419,7 @@ template class MapMatching final void operator()(const Matching::CandidateLists &candidates_list, const std::vector &trace_coordinates, const std::vector &trace_timestamps, - Matching::SubMatchingList &sub_matchings, - osrm::json::Object &_debug_info) const + Matching::SubMatchingList &sub_matchings) const { BOOST_ASSERT(candidates_list.size() > 0); @@ -321,32 +431,8 @@ template class MapMatching final return; } - osrm::json::Array _debug_states; - for (unsigned t = 0; t < candidates_list.size(); t++) - { - osrm::json::Array _debug_timestamps; - for (unsigned s = 0; s < candidates_list[t].size(); s++) - { - osrm::json::Object _debug_state; - _debug_state.values["transitions"] = osrm::json::Array(); - _debug_state.values["coordinate"] = osrm::json::makeArray( - candidates_list[t][s].first.location.lat / COORDINATE_PRECISION, - candidates_list[t][s].first.location.lon / COORDINATE_PRECISION); - if (t < initial_timestamp) - { - _debug_state.values["viterbi"] = - osrm::json::clampFloat(Matching::IMPOSSIBLE_LOG_PROB); - _debug_state.values["pruned"] = 0u; - } - else if (t == initial_timestamp) - { - _debug_state.values["viterbi"] = osrm::json::clampFloat(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); - } + DebugInfo debug(osrm::json::Logger::get()); + debug.initialize(model, initial_timestamp, candidates_list); unsigned breakage_begin = std::numeric_limits::max(); std::vector split_points; @@ -400,24 +486,12 @@ template class MapMatching final const double transition_pr = log_transition_probability(d_t, beta); new_value += transition_pr; - osrm::json::Object _debug_transistion; - _debug_transistion.values["to"] = osrm::json::makeArray(t, s_prime); - _debug_transistion.values["properties"] = - osrm::json::makeArray(osrm::json::clampFloat(prev_viterbi[s]), - osrm::json::clampFloat(emission_pr), - osrm::json::clampFloat(transition_pr), + debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime, + prev_viterbi[s], + emission_pr, + transition_pr, network_distance, great_circle_distance); - _debug_states.values[prev_unbroken_timestamp] - .get() - .get() - .values[s] - .get() - .get() - .values["transitions"] - .get() - .get() - .values.push_back(_debug_transistion); if (new_value > current_viterbi[s_prime]) { @@ -430,23 +504,7 @@ template class MapMatching final } } - for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) - { - _debug_states.values[t] - .get() - .get() - .values[s_prime] - .get() - .get() - .values["viterbi"] = osrm::json::clampFloat(current_viterbi[s_prime]); - _debug_states.values[t] - .get() - .get() - .values[s_prime] - .get() - .get() - .values["pruned"] = static_cast(current_pruned[s_prime]); - } + debug.add_viterbi(t, current_viterbi, current_pruned); if (model.breakage[t]) { @@ -570,13 +628,7 @@ template class MapMatching final matching.nodes[i] = candidates_list[timestamp_index][location_index].first; matching.length += model.path_lengths[timestamp_index][location_index]; - _debug_states.values[timestamp_index] - .get() - .get() - .values[location_index] - .get() - .get() - .values["chosen"] = true; + debug.add_chosen(timestamp_index, location_index); } sub_matchings.push_back(matching); @@ -584,14 +636,7 @@ template class MapMatching final sub_matching_begin = sub_matching_end; } - osrm::json::Array _debug_breakage; - for (auto b : model.breakage) - { - _debug_breakage.values.push_back(static_cast(b)); - } - - _debug_info.values["breakage"] = _debug_breakage; - _debug_info.values["states"] = _debug_states; + debug.add_breakage(model.breakage); } }; diff --git a/util/json_logger.hpp b/util/json_logger.hpp new file mode 100644 index 000000000..872a926d5 --- /dev/null +++ b/util/json_logger.hpp @@ -0,0 +1,77 @@ +/* + +Copyright (c) 2015, Project OSRM, Dennis Luxen, others +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 JSON_LOGGER_HPP +#define JSON_LOGGER_HPP + +#include + +#include + +namespace osrm +{ +namespace json +{ + +class Logger +{ + using MapT = std::unordered_map; + + public: + static Logger* get() + { + static Logger logger; + +#ifdef NDEBUG + return nullptr; +#else + return &logger; +#endif + } + + void initialize(const std::string& name) + { + if (!map.get()) + { + map.reset(new MapT()); + } + (*map)[name] = Object(); + } + + void render(const std::string& name, Object& obj) const + { + obj.values["debug"] = map->at(name); + } + + boost::thread_specific_ptr map; +}; + + +} +} + +#endif /* SIMPLE_LOGGER_HPP */ diff --git a/util/json_util.hpp b/util/json_util.hpp index 0e362701a..563440d19 100644 --- a/util/json_util.hpp +++ b/util/json_util.hpp @@ -41,7 +41,8 @@ namespace osrm namespace json { -template T clampFloat(T d) +// Make sure we don't have inf and NaN values +template T clamp_float(T d) { if (std::isnan(d) || std::numeric_limits::infinity() == d) { @@ -55,21 +56,46 @@ template T clampFloat(T d) return d; } -void appendToArray(osrm::json::Array &a) {} +void append_to_array(osrm::json::Array &a) {} template -void appendToArray(osrm::json::Array &a, T value, Args... args) +void append_to_array(osrm::json::Array &a, T value, Args... args) { a.values.emplace_back(value); - appendToJSONArray(a, args...); + append_to_array(a, args...); } -template osrm::json::Array makeArray(Args... args) +template osrm::json::Array make_array(Args... args) { osrm::json::Array a; - appendToJSONArray(a, args...); + append_to_array(a, args...); return a; } +template osrm::json::Array make_array(const std::vector& vector) +{ + osrm::json::Array a; + for (const auto& v : vector) + a.values.emplace_back(v); + return a; +} + +// Easy acces to object hierachies +osrm::json::Value& get(osrm::json::Value& value) { return value; } + +template +osrm::json::Value& get(osrm::json::Value& value, const char* key, Keys... keys) +{ + using recursive_object_t = mapbox::util::recursive_wrapper; + return get(value.get().get().values[key], keys...); +} + +template +osrm::json::Value& get(osrm::json::Value& value, unsigned key, Keys... keys) +{ + using recursive_array_t = mapbox::util::recursive_wrapper; + return get(value.get().get().values[key], keys...); +} + } // namespace json } // namespace osrm #endif // JSON_RENDERER_HPP From d89b171f495a6b823fd984009d43fad42ea468c9 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 28 Feb 2015 18:50:01 +0100 Subject: [PATCH 40/67] Simplify json code in map matching plugin --- plugins/map_matching.hpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index a3ef84563..5bb58e6fc 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -34,13 +34,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "../algorithms/object_encoder.hpp" #include "../data_structures/search_engine.hpp" #include "../descriptors/descriptor_base.hpp" -#include "../descriptors/gpx_descriptor.hpp" #include "../descriptors/json_descriptor.hpp" #include "../routing_algorithms/map_matching.hpp" #include "../util/compute_angle.hpp" #include "../util/integer_range.hpp" #include "../util/simple_logger.hpp" #include "../util/json_logger.hpp" +#include "../util/json_util.hpp" #include "../util/string_util.hpp" #include @@ -194,21 +194,14 @@ template class MapMatchingPlugin : public BasePlugin subtrace.values["geometry"] = factory.AppendGeometryString(route_parameters.compression); } - osrm::json::Array indices; - for (const auto& i : sub.indices) - { - indices.values.emplace_back(i); - } - subtrace.values["indices"] = indices; + subtrace.values["indices"] = osrm::json::make_array(sub.indices); osrm::json::Array points; for (const auto& node : sub.nodes) { - osrm::json::Array coordinate; - coordinate.values.emplace_back(node.location.lat / COORDINATE_PRECISION); - coordinate.values.emplace_back(node.location.lon / COORDINATE_PRECISION); - points.values.emplace_back(coordinate); + points.values.emplace_back(osrm::json::make_array(node.location.lat / COORDINATE_PRECISION, + node.location.lon / COORDINATE_PRECISION)); } subtrace.values["matched_points"] = points; From e5830b0116bd843c37d61dd915fc1eb50a61f089 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 2 Mar 2015 23:12:44 +0100 Subject: [PATCH 41/67] Add parameters for map matching --- data_structures/route_parameters.cpp | 9 ++- include/osrm/route_parameters.hpp | 9 +++ plugins/map_matching.hpp | 37 ++++++---- routing_algorithms/map_matching.hpp | 105 +++++++++++---------------- server/api_grammar.hpp | 11 ++- 5 files changed, 94 insertions(+), 77 deletions(-) diff --git a/data_structures/route_parameters.cpp b/data_structures/route_parameters.cpp index 7429e4902..3b615e2c4 100644 --- a/data_structures/route_parameters.cpp +++ b/data_structures/route_parameters.cpp @@ -33,7 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. RouteParameters::RouteParameters() : zoom_level(18), print_instructions(false), alternate_route(true), geometry(true), - compression(true), deprecatedAPI(false), uturn_default(false), check_sum(-1), num_results(1) + compression(true), deprecatedAPI(false), uturn_default(false), classify(false), + matching_beta(-1.0), gps_precision(-1.0), check_sum(-1), num_results(1) { } @@ -83,6 +84,12 @@ void RouteParameters::setInstructionFlag(const bool flag) { print_instructions = void RouteParameters::setService(const std::string &service_string) { service = service_string; } +void RouteParameters::setClassify(const bool flag) { classify = flag; } + +void RouteParameters::setMatchingBeta(const double beta) { matching_beta = beta; } + +void RouteParameters::setGPSPrecision(const double precision) { gps_precision = precision; } + void RouteParameters::setOutputFormat(const std::string &format) { output_format = format; } void RouteParameters::setJSONpParameter(const std::string ¶meter) diff --git a/include/osrm/route_parameters.hpp b/include/osrm/route_parameters.hpp index 5fe454cee..9babbd763 100644 --- a/include/osrm/route_parameters.hpp +++ b/include/osrm/route_parameters.hpp @@ -49,6 +49,12 @@ struct RouteParameters void setAllUTurns(const bool flag); + void setClassify(const bool classify); + + void setMatchingBeta(const double beta); + + void setGPSPrecision(const double precision); + void setDeprecatedAPIFlag(const std::string &); void setChecksum(const unsigned check_sum); @@ -80,6 +86,9 @@ struct RouteParameters bool compression; bool deprecatedAPI; bool uturn_default; + bool classify; + double matching_beta; + double gps_precision; unsigned check_sum; short num_results; std::string service; diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 5bb58e6fc..bacddfc1b 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -171,7 +171,8 @@ template class MapMatchingPlugin : public BasePlugin { osrm::json::Object subtrace; - subtrace.values["confidence"] = sub.confidence; + if (route_parameters.classify) + subtrace.values["confidence"] = sub.confidence; if (route_parameters.geometry) { @@ -230,11 +231,18 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - // call the actual map matching + // setup logging if enabled if (osrm::json::Logger::get()) osrm::json::Logger::get()->initialize("matching"); + + // call the actual map matching Matching::SubMatchingList sub_matchings; - search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, sub_matchings); + search_engine_ptr->map_matching(candidates_lists, + input_coords, + input_timestamps, + route_parameters.matching_beta, + route_parameters.gps_precision, + sub_matchings); if (1 > sub_matchings.size()) { @@ -245,17 +253,20 @@ template class MapMatchingPlugin : public BasePlugin for (auto& sub : sub_matchings) { // classify result - double trace_length = sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()]; - TraceClassification classification = classify(trace_length, - sub.length, - (sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size()); - if (classification.first == ClassifierT::ClassLabel::POSITIVE) + if (route_parameters.classify) { - sub.confidence = classification.second; - } - else - { - sub.confidence = 1-classification.second; + double trace_length = sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()]; + TraceClassification classification = classify(trace_length, + sub.length, + (sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size()); + if (classification.first == ClassifierT::ClassLabel::POSITIVE) + { + sub.confidence = classification.second; + } + else + { + sub.confidence = 1-classification.second; + } } BOOST_ASSERT(sub.nodes.size() > 1); diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 8a751ba1f..1fa881bf2 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -75,67 +75,42 @@ template class MapMatching final SearchEngineData &engine_working_data; // FIXME this value should be a table based on samples/meter (or samples/min) - constexpr static const double beta = 10.0; - constexpr static const double sigma_z = 4.07; - constexpr static const double log_sigma_z = std::log(sigma_z); + constexpr static const double default_beta = 10.0; + constexpr static const double default_sigma_z = 4.07; constexpr static const double log_2_pi = std::log(2 * M_PI); - constexpr static double emission_probability(const double distance) + // closures to precompute log -> only simple floating point operations + struct EmissionLogProbability { - return (1. / (std::sqrt(2. * M_PI) * sigma_z)) * - std::exp(-0.5 * std::pow((distance / sigma_z), 2.)); - } + double sigma_z; + double log_sigma_z; - constexpr static double transition_probability(const float d_t, const float beta) + 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 { - return (1. / beta) * std::exp(-d_t / beta); - } + double beta; + double log_beta; + TransitionLogProbability(const double beta) + : beta(beta) + , log_beta(std::log(beta)) + { + } - constexpr static double log_emission_probability(const double distance) - { - return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z; - } - - constexpr static double log_transition_probability(const float d_t, const float beta) - { - return -std::log(beta) - d_t / beta; - } - - // TODO: needs to be estimated from the input locations - // FIXME These values seem wrong. Higher beta for more samples/minute? Should be inverse - // proportional. - // 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 + 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 @@ -231,9 +206,10 @@ template class MapMatching final std::vector breakage; const Matching::CandidateLists &candidates_list; + const EmissionLogProbability& emission_log_probability; - HiddenMarkovModel(const Matching::CandidateLists &candidates_list) - : breakage(candidates_list.size()), candidates_list(candidates_list) + 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) { @@ -271,7 +247,7 @@ template class MapMatching final for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) { viterbi[initial_timestamp][s] = - log_emission_probability(candidates_list[initial_timestamp][s].second); + 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; @@ -297,6 +273,7 @@ template class MapMatching final } }; + // Provides the debug interface for introspection tools struct DebugInfo { DebugInfo(const osrm::json::Logger* logger) @@ -419,11 +396,17 @@ template class MapMatching final void operator()(const 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 { BOOST_ASSERT(candidates_list.size() > 0); - HiddenMarkovModel model(candidates_list); + // 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); + + HiddenMarkovModel model(candidates_list, emission_log_probability); unsigned initial_timestamp = model.initialize(0); if (initial_timestamp == Matching::INVALID_STATE) @@ -464,7 +447,7 @@ template class MapMatching final { // how likely is candidate s_prime at time t to be emitted? const double emission_pr = - log_emission_probability(candidates_list[t][s_prime].second); + emission_log_probability(candidates_list[t][s_prime].second); double new_value = prev_viterbi[s] + emission_pr; if (current_viterbi[s_prime] > new_value) continue; @@ -483,7 +466,7 @@ template class MapMatching final if (d_t > 500) continue; - const double transition_pr = log_transition_probability(d_t, beta); + const double transition_pr = transition_log_probability(d_t); new_value += transition_pr; debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime, diff --git a/server/api_grammar.hpp b/server/api_grammar.hpp index 3d7f4ea4b..5c7dcde45 100644 --- a/server/api_grammar.hpp +++ b/server/api_grammar.hpp @@ -41,7 +41,8 @@ template struct APIGrammar : qi::grammar> string[boost::bind(&HandlerT::setService, handler, ::_1)] >> *(query) >> -(uturns); query = ('?') >> (+(zoom | output | jsonp | checksum | location | hint | timestamp | u | cmp | - language | instruction | geometry | alt_route | old_API | num_results)); + language | instruction | geometry | alt_route | old_API | num_results | + matching_beta | gps_precision | classify)); zoom = (-qi::lit('&')) >> qi::lit('z') >> '=' >> qi::short_[boost::bind(&HandlerT::setZoomLevel, handler, ::_1)]; @@ -64,6 +65,12 @@ template struct APIGrammar : qi::grammar> qi::lit("t") >> '=' >> qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)]; + matching_beta = (-qi::lit('&')) >> qi::lit("matching_beta") >> '=' >> + qi::short_[boost::bind(&HandlerT::setMatchingBeta, handler, ::_1)]; + gps_precision = (-qi::lit('&')) >> qi::lit("gps_precision") >> '=' >> + qi::short_[boost::bind(&HandlerT::setGPSPrecision, handler, ::_1)]; + classify = (-qi::lit('&')) >> qi::lit("classify") >> '=' >> + qi::bool_[boost::bind(&HandlerT::setClassify, handler, ::_1)]; u = (-qi::lit('&')) >> qi::lit("u") >> '=' >> qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)]; uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >> @@ -85,7 +92,7 @@ template struct APIGrammar : qi::grammar api_call, query; qi::rule service, zoom, output, string, jsonp, checksum, location, - hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, + hint, timestamp, matching_beta, gps_precision, classify, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, uturns, old_API, num_results; HandlerT *handler; From d8d46e0f3e0ea789e0a2884a10ca0effcf5ce9f2 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 2 Mar 2015 23:39:53 +0100 Subject: [PATCH 42/67] Add routed parameter to limit matching size --- include/osrm/libosrm_config.hpp | 12 +++++-- library/osrm_impl.cpp | 3 +- plugins/map_matching.hpp | 11 +++++- routed.cpp | 3 +- util/routed_options.hpp | 64 ++++++++++++++++++--------------- 5 files changed, 58 insertions(+), 35 deletions(-) diff --git a/include/osrm/libosrm_config.hpp b/include/osrm/libosrm_config.hpp index 777a0ccb9..500abf5bd 100644 --- a/include/osrm/libosrm_config.hpp +++ b/include/osrm/libosrm_config.hpp @@ -33,15 +33,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. struct libosrm_config { libosrm_config(const libosrm_config &) = delete; - libosrm_config() : max_locations_distance_table(100), use_shared_memory(false) {} + libosrm_config() + : max_locations_distance_table(100), max_locations_map_matching(-1), + use_shared_memory(false) + { + } - libosrm_config(const ServerPaths &paths, const bool flag, const int max) - : server_paths(paths), max_locations_distance_table(max), use_shared_memory(flag) + libosrm_config(const ServerPaths &paths, const bool flag, const int max_table, const int max_matching) + : server_paths(paths), max_locations_distance_table(max_table), + max_locations_map_matching(max_matching), use_shared_memory(flag) { } ServerPaths server_paths; int max_locations_distance_table; + int max_locations_map_matching; bool use_shared_memory; }; diff --git a/library/osrm_impl.cpp b/library/osrm_impl.cpp index 9ea009912..f12445f2f 100644 --- a/library/osrm_impl.cpp +++ b/library/osrm_impl.cpp @@ -82,7 +82,8 @@ 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 MapMatchingPlugin>( + query_data_facade, lib_config.max_locations_map_matching)); RegisterPlugin(new TimestampPlugin>(query_data_facade)); RegisterPlugin(new ViaRoutePlugin>(query_data_facade)); } diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index bacddfc1b..3bf2026f8 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -59,9 +59,10 @@ template class MapMatchingPlugin : public BasePlugin using TraceClassification = ClassifierT::ClassificationT; public: - MapMatchingPlugin(DataFacadeT *facade) + MapMatchingPlugin(DataFacadeT *facade, const int max_locations_map_matching) : descriptor_string("match") , facade(facade) + , max_locations_map_matching(max_locations_map_matching) // the values where derived from fitting a laplace distribution // to the values of manually classified traces , classifier(LaplaceDistribution(0.0057154021891018675, 0.020294704891166186), @@ -225,6 +226,13 @@ template class MapMatchingPlugin : public BasePlugin { return 400; } + + // enforce maximum number of locations for performance reasons + if (max_locations_map_matching > 0 && static_cast(input_coords.size()) > max_locations_map_matching) + { + return 400; + } + bool found_candidates = getCandiates(input_coords, sub_trace_lengths, candidates_lists); if (!found_candidates) { @@ -299,6 +307,7 @@ template class MapMatchingPlugin : public BasePlugin private: std::string descriptor_string; DataFacadeT *facade; + int max_locations_map_matching; ClassifierT classifier; }; diff --git a/routed.cpp b/routed.cpp index ce5691715..8b0e2e88b 100644 --- a/routed.cpp +++ b/routed.cpp @@ -77,7 +77,8 @@ int main(int argc, const char *argv[]) const unsigned init_result = GenerateServerProgramOptions( argc, argv, lib_config.server_paths, ip_address, ip_port, requested_thread_num, - lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table); + lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table, + lib_config.max_locations_map_matching); if (init_result == INIT_OK_DO_NOT_START_ENGINE) { return 0; diff --git a/util/routed_options.hpp b/util/routed_options.hpp index 322737d6d..0aef32147 100644 --- a/util/routed_options.hpp +++ b/util/routed_options.hpp @@ -151,7 +151,8 @@ inline unsigned GenerateServerProgramOptions(const int argc, int &requested_num_threads, bool &use_shared_memory, bool &trial, - int &max_locations_distance_table) + int &max_locations_distance_table, + int &max_locations_map_matching) { // declare a group of options that will be allowed only on command line boost::program_options::options_description generic_options("Options"); @@ -165,34 +166,35 @@ inline unsigned GenerateServerProgramOptions(const int argc, // declare a group of options that will be allowed both on command line // as well as in a config file boost::program_options::options_description config_options("Configuration"); - config_options.add_options()( - "hsgrdata", boost::program_options::value(&paths["hsgrdata"]), - ".hsgr file")("nodesdata", - boost::program_options::value(&paths["nodesdata"]), - ".nodes file")( - "edgesdata", boost::program_options::value(&paths["edgesdata"]), - ".edges file")("geometry", - boost::program_options::value(&paths["geometries"]), - ".geometry file")( - "ramindex", boost::program_options::value(&paths["ramindex"]), - ".ramIndex file")( - "fileindex", boost::program_options::value(&paths["fileindex"]), - "File index file")( - "namesdata", boost::program_options::value(&paths["namesdata"]), - ".names file")("timestamp", - boost::program_options::value(&paths["timestamp"]), - ".timestamp file")( - "ip,i", boost::program_options::value(&ip_address)->default_value("0.0.0.0"), - "IP address")("port,p", boost::program_options::value(&ip_port)->default_value(5000), - "TCP/IP port")( - "threads,t", boost::program_options::value(&requested_num_threads)->default_value(8), - "Number of threads to use")( - "shared-memory,s", - boost::program_options::value(&use_shared_memory)->implicit_value(true), - "Load data from shared memory")( - "max-table-size,m", - boost::program_options::value(&max_locations_distance_table)->default_value(100), - "Max. locations supported in distance table query"); + config_options.add_options() + ("hsgrdata", boost::program_options::value(&paths["hsgrdata"]), + ".hsgr file") + ("nodesdata", boost::program_options::value(&paths["nodesdata"]), + ".nodes file") + ("edgesdata", boost::program_options::value(&paths["edgesdata"]), + ".edges file") + ("geometry", boost::program_options::value(&paths["geometries"]), + ".geometry file") + ("ramindex", boost::program_options::value(&paths["ramindex"]), + ".ramIndex file") + ("fileindex", boost::program_options::value(&paths["fileindex"]), + "File index file") + ("namesdata", boost::program_options::value(&paths["namesdata"]), + ".names file") + ("timestamp", boost::program_options::value(&paths["timestamp"]), + ".timestamp file") + ("ip,i", boost::program_options::value(&ip_address)->default_value("0.0.0.0"), + "IP address") + ("port,p", boost::program_options::value(&ip_port)->default_value(5000), + "TCP/IP port") + ("threads,t", boost::program_options::value(&requested_num_threads)->default_value(8), + "Number of threads to use") + ("shared-memory,s", boost::program_options::value(&use_shared_memory)->implicit_value(true), + "Load data from shared memory") + ("max-table-size,m", boost::program_options::value(&max_locations_distance_table)->default_value(100), + "Max. locations supported in distance table query") + ("max-matching-size,m", boost::program_options::value(&max_locations_map_matching)->default_value(-1), + "Max. locations supported in map matching query"); // hidden options, will be allowed both on command line and in config // file, but will not be shown to the user @@ -269,6 +271,10 @@ inline unsigned GenerateServerProgramOptions(const int argc, { throw osrm::exception("Max location for distance table must be a positive number"); } + if (2 > max_locations_map_matching) + { + throw osrm::exception("Max location for map matching must be at least two"); + } SimpleLogger().Write() << visible_options; return INIT_OK_DO_NOT_START_ENGINE; From 0d879ed2907714b0ffb0db7e42b87187ee7e5531 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Tue, 3 Mar 2015 00:38:09 +0100 Subject: [PATCH 43/67] Split trace if timestamp delta is over threshold Even when matching is not broken we split the trace, if the sampling frequency goes below 2 samples/minute. --- routing_algorithms/map_matching.hpp | 124 ++++++++++++++-------------- 1 file changed, 61 insertions(+), 63 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 1fa881bf2..5400a74d4 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -63,7 +63,7 @@ constexpr static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits 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 = 180; +constexpr static const unsigned MAX_BROKEN_TIME = 30; } // implements a hidden markov model map matching algorithm @@ -285,9 +285,7 @@ template class MapMatching final } } - void initialize(const HiddenMarkovModel& model, - unsigned initial_timestamp, - const Matching::CandidateLists& candidates_list) + void initialize(const Matching::CandidateLists& candidates_list) { // json logger not enabled if (!logger) @@ -304,16 +302,8 @@ template class MapMatching final 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); - if (t < initial_timestamp) - { - state.values["viterbi"] = osrm::json::clamp_float(Matching::IMPOSSIBLE_LOG_PROB); - state.values["pruned"] = 0u; - } - else if (t == initial_timestamp) - { - state.values["viterbi"] = osrm::json::clamp_float(model.viterbi[t][s]); - state.values["pruned"] = static_cast(model.pruned[t][s]); - } + 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); @@ -350,18 +340,20 @@ template class MapMatching final } - void add_viterbi(const unsigned t, - const std::vector& current_viterbi, - const std::vector& current_pruned) + void set_viterbi(const std::vector>& viterbi, + const std::vector>& pruned) { // json logger not enabled if (!logger) return; - for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) + for (auto t = 0u; t < viterbi.size(); t++) { - osrm::json::get(*object, "states", t, s_prime, "viterbi") = osrm::json::clamp_float(current_viterbi[s_prime]); - osrm::json::get(*object, "states", t, s_prime, "pruned") = static_cast(current_pruned[s_prime]); + 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]); + } } } @@ -415,7 +407,7 @@ template class MapMatching final } DebugInfo debug(osrm::json::Logger::get()); - debug.initialize(model, initial_timestamp, candidates_list); + debug.initialize(candidates_list); unsigned breakage_begin = std::numeric_limits::max(); std::vector split_points; @@ -424,7 +416,51 @@ template class MapMatching final prev_unbroken_timestamps.push_back(initial_timestamp); for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t) { + // breakage recover has removed all previous good points + bool trace_split = prev_unbroken_timestamps.size() < 1; + + // use temporal information if available to determine a split + if (trace_timestamps.size() > 0) + { + trace_split = + trace_split || + (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > + Matching::MAX_BROKEN_TIME); + } + else + { + trace_split = trace_split || (t - prev_unbroken_timestamps.back() > + Matching::MAX_BROKEN_STATES); + } + + if (trace_split) + { + unsigned split_index = t; + if (breakage_begin != std::numeric_limits::max()) + { + split_index = breakage_begin; + breakage_begin = std::numeric_limits::max(); + } + split_points.push_back(split_index); + + // note: this preserves everything before split_index + model.clear(split_index); + unsigned new_start = model.initialize(split_index); + // no new start was found -> stop viterbi calculation + if (new_start == Matching::INVALID_STATE) + { + break; + } + prev_unbroken_timestamps.clear(); + prev_unbroken_timestamps.push_back(new_start); + // Important: We potentially go back here! + // However since t > new_start >= breakge_begin + // we can only reset trace_coordindates.size() times. + t = new_start+1; + } + 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 = candidates_list[prev_unbroken_timestamp]; @@ -487,58 +523,18 @@ template class MapMatching final } } - debug.add_viterbi(t, current_viterbi, current_pruned); - if (model.breakage[t]) { - BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); - // 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(); - - bool trace_split = prev_unbroken_timestamps.size() < 1; - - // use temporal information to determine a split if available - if (trace_timestamps.size() > 0) - { - trace_split = - trace_split || - (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > - Matching::MAX_BROKEN_TIME); - } - else - { - trace_split = trace_split || (t - prev_unbroken_timestamps.back() > - Matching::MAX_BROKEN_STATES); - } - - // we reached the beginning of the trace and it is still broken - // -> split the trace here - if (trace_split) - { - split_points.push_back(breakage_begin); - // note: this preserves everything before breakage_begin - model.clear(breakage_begin); - unsigned new_start = model.initialize(breakage_begin); - // no new start was found -> stop viterbi calculation - if (new_start == Matching::INVALID_STATE) - { - break; - } - prev_unbroken_timestamps.clear(); - prev_unbroken_timestamps.push_back(new_start); - // Important: We potentially go back here! - // However since t+1 > new_start >= breakge_begin - // we can only reset trace_coordindates.size() times. - t = new_start; - breakage_begin = std::numeric_limits::max(); - } } else { @@ -546,6 +542,8 @@ template class MapMatching final } } + debug.set_viterbi(model.viterbi, model.pruned); + if (prev_unbroken_timestamps.size() > 0) { split_points.push_back(prev_unbroken_timestamps.back() + 1); From 6fb8fdc2bd6eb8943a46007fed82e78cddea9e91 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 11:46:24 +0100 Subject: [PATCH 44/67] fix compilation - define max_number_of_candidates where its used - add curly braces - reformat --- plugins/map_matching.hpp | 115 ++++++++++++++++++++------------------- 1 file changed, 58 insertions(+), 57 deletions(-) diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 3bf2026f8..a67a9e075 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -52,7 +52,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. template class MapMatchingPlugin : public BasePlugin { - private: + constexpr static const unsigned max_number_of_candidates = 10; + std::shared_ptr> search_engine_ptr; using ClassifierT = BayesClassifier; @@ -60,19 +61,19 @@ template class MapMatchingPlugin : public BasePlugin public: MapMatchingPlugin(DataFacadeT *facade, const int max_locations_map_matching) - : descriptor_string("match") - , facade(facade) - , max_locations_map_matching(max_locations_map_matching) - // the values where derived from fitting a laplace distribution - // to the values of manually classified traces - , classifier(LaplaceDistribution(0.0057154021891018675, 0.020294704891166186), - LaplaceDistribution(0.11467696742821254, 0.49918444000368756), - 0.7977883096366508) // valid apriori probability + : descriptor_string("match"), facade(facade), + max_locations_map_matching(max_locations_map_matching) + // the values where derived from fitting a laplace distribution + // to the values of manually classified traces + , + classifier(LaplaceDistribution(0.0057154021891018675, 0.020294704891166186), + LaplaceDistribution(0.11467696742821254, 0.49918444000368756), + 0.7977883096366508) // valid apriori probability { search_engine_ptr = std::make_shared>(facade); } - virtual ~MapMatchingPlugin() { } + virtual ~MapMatchingPlugin() {} const std::string GetDescriptor() const final { return descriptor_string; } @@ -86,7 +87,7 @@ template class MapMatchingPlugin : public BasePlugin return std::make_pair(ClassifierT::ClassLabel::NEGATIVE, 1.0); } - auto label_with_confidence = classifier.classify(distance_feature); + auto label_with_confidence = classifier.classify(distance_feature); // "second stage classifier": if we need to remove points there is something fishy if (removed_points > 0) @@ -97,11 +98,12 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - bool getCandiates(const std::vector& input_coords, std::vector& sub_trace_lengths, Matching::CandidateLists& candidates_lists) + bool getCandiates(const std::vector &input_coords, + std::vector &sub_trace_lengths, + Matching::CandidateLists &candidates_lists) { - double last_distance = coordinate_calculation::great_circle_distance( - input_coords[0], - input_coords[1]); + double last_distance = + coordinate_calculation::great_circle_distance(input_coords[0], input_coords[1]); sub_trace_lengths.resize(input_coords.size()); sub_trace_lengths[0] = 0; for (const auto current_coordinate : osrm::irange(0, input_coords.size())) @@ -110,17 +112,16 @@ template class MapMatchingPlugin : public BasePlugin if (0 < current_coordinate) { last_distance = coordinate_calculation::great_circle_distance( - input_coords[current_coordinate - 1], - input_coords[current_coordinate]); - sub_trace_lengths[current_coordinate] += sub_trace_lengths[current_coordinate-1] + last_distance; + input_coords[current_coordinate - 1], input_coords[current_coordinate]); + sub_trace_lengths[current_coordinate] += + sub_trace_lengths[current_coordinate - 1] + last_distance; } - if (input_coords.size()-1 > current_coordinate && 0 < current_coordinate) + if (input_coords.size() - 1 > current_coordinate && 0 < current_coordinate) { double turn_angle = ComputeAngle::OfThreeFixedPointCoordinates( - input_coords[current_coordinate-1], - input_coords[current_coordinate], - input_coords[current_coordinate+1]); + input_coords[current_coordinate - 1], input_coords[current_coordinate], + input_coords[current_coordinate + 1]); // sharp turns indicate a possible uturn if (turn_angle < 100.0 || turn_angle > 260.0) @@ -131,11 +132,8 @@ template class MapMatchingPlugin : public BasePlugin std::vector> candidates; if (!facade->IncrementalFindPhantomNodeForCoordinateWithMaxDistance( - input_coords[current_coordinate], - candidates, - last_distance/2.0, - 5, - Matching::max_number_of_candidates)) + input_coords[current_coordinate], candidates, last_distance / 2.0, 5, + max_number_of_candidates)) { return false; } @@ -150,8 +148,8 @@ template class MapMatchingPlugin : public BasePlugin for (const auto i : osrm::irange(0u, compact_size)) { // Split edge if it is bidirectional and append reverse direction to end of list - if (candidates[i].first.forward_node_id != SPECIAL_NODEID - && candidates[i].first.reverse_node_id != SPECIAL_NODEID) + if (candidates[i].first.forward_node_id != SPECIAL_NODEID && + candidates[i].first.reverse_node_id != SPECIAL_NODEID) { PhantomNode reverse_node(candidates[i].first); reverse_node.forward_node_id = SPECIAL_NODEID; @@ -162,27 +160,30 @@ template class MapMatchingPlugin : public BasePlugin } candidates_lists.push_back(candidates); } - } return true; } - osrm::json::Object submatchingToJSON(const Matching::SubMatching& sub, const RouteParameters& route_parameters, const InternalRouteResult& raw_route) + osrm::json::Object submatchingToJSON(const Matching::SubMatching &sub, + const RouteParameters &route_parameters, + const InternalRouteResult &raw_route) { osrm::json::Object subtrace; if (route_parameters.classify) + { subtrace.values["confidence"] = sub.confidence; + } if (route_parameters.geometry) { DescriptionFactory factory; FixedPointCoordinate current_coordinate; - factory.SetStartSegment( - raw_route.segment_end_coordinates.front().source_phantom, - raw_route.source_traversed_in_reverse.front()); - for (const auto i : osrm::irange(0, raw_route.unpacked_path_segments.size())) + factory.SetStartSegment(raw_route.segment_end_coordinates.front().source_phantom, + raw_route.source_traversed_in_reverse.front()); + for (const auto i : + osrm::irange(0, raw_route.unpacked_path_segments.size())) { for (const PathData &path_data : raw_route.unpacked_path_segments[i]) { @@ -193,24 +194,26 @@ template class MapMatchingPlugin : public BasePlugin raw_route.target_traversed_in_reverse[i], raw_route.is_via_leg(i)); } - subtrace.values["geometry"] = factory.AppendGeometryString(route_parameters.compression); + subtrace.values["geometry"] = + factory.AppendGeometryString(route_parameters.compression); } subtrace.values["indices"] = osrm::json::make_array(sub.indices); - osrm::json::Array points; - for (const auto& node : sub.nodes) + for (const auto &node : sub.nodes) { - points.values.emplace_back(osrm::json::make_array(node.location.lat / COORDINATE_PRECISION, - node.location.lon / COORDINATE_PRECISION)); + points.values.emplace_back( + osrm::json::make_array(node.location.lat / COORDINATE_PRECISION, + node.location.lon / COORDINATE_PRECISION)); } subtrace.values["matched_points"] = points; return subtrace; } - int HandleRequest(const RouteParameters &route_parameters, osrm::json::Object &json_result) final + int HandleRequest(const RouteParameters &route_parameters, + osrm::json::Object &json_result) final { // check number of parameters if (!check_all_coordinates(route_parameters.coordinates)) @@ -220,15 +223,16 @@ template class MapMatchingPlugin : public BasePlugin std::vector sub_trace_lengths; Matching::CandidateLists candidates_lists; - const auto& input_coords = route_parameters.coordinates; - const auto& input_timestamps = route_parameters.timestamps; + 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()) { return 400; } // enforce maximum number of locations for performance reasons - if (max_locations_map_matching > 0 && static_cast(input_coords.size()) > max_locations_map_matching) + if (max_locations_map_matching > 0 && + static_cast(input_coords.size()) > max_locations_map_matching) { return 400; } @@ -245,12 +249,9 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching Matching::SubMatchingList sub_matchings; - search_engine_ptr->map_matching(candidates_lists, - input_coords, - input_timestamps, + search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps, route_parameters.matching_beta, - route_parameters.gps_precision, - sub_matchings); + route_parameters.gps_precision, sub_matchings); if (1 > sub_matchings.size()) { @@ -258,22 +259,23 @@ template class MapMatchingPlugin : public BasePlugin } osrm::json::Array matchings; - for (auto& sub : sub_matchings) + for (auto &sub : sub_matchings) { // classify result if (route_parameters.classify) { - double trace_length = sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()]; - TraceClassification classification = classify(trace_length, - sub.length, - (sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size()); + double trace_length = + sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()]; + TraceClassification classification = + classify(trace_length, sub.length, + (sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size()); if (classification.first == ClassifierT::ClassLabel::POSITIVE) { sub.confidence = classification.second; } else { - sub.confidence = 1-classification.second; + sub.confidence = 1 - classification.second; } } @@ -291,8 +293,7 @@ template class MapMatchingPlugin : public BasePlugin } search_engine_ptr->shortest_path( raw_route.segment_end_coordinates, - std::vector(raw_route.segment_end_coordinates.size(), true), - raw_route); + std::vector(raw_route.segment_end_coordinates.size(), true), raw_route); matchings.values.emplace_back(submatchingToJSON(sub, route_parameters, raw_route)); } From 76aa494be4d7d346d747b74895fa6ecbe75a4869 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 11:48:15 +0100 Subject: [PATCH 45/67] fix compilation - std::log and M_PI are not constexpr's by the standard. replace by a constant - reformat --- routing_algorithms/map_matching.hpp | 95 +++++++++++++---------------- 1 file changed, 43 insertions(+), 52 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 5400a74d4..d5fb30e12 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -58,7 +58,6 @@ struct SubMatching using CandidateList = std::vector>; using CandidateLists = std::vector; using SubMatchingList = std::vector; -constexpr static const unsigned max_number_of_candidates = 10; 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(); @@ -67,8 +66,8 @@ constexpr static const unsigned MAX_BROKEN_TIME = 30; } // implements a hidden markov model map matching algorithm -template class MapMatching final - : public BasicRoutingInterface> +template +class MapMatching final : public BasicRoutingInterface> { using super = BasicRoutingInterface>; using QueryHeap = SearchEngineData::QueryHeap; @@ -77,7 +76,7 @@ template class MapMatching final // FIXME this value should be a table based on samples/meter (or samples/min) constexpr static const double default_beta = 10.0; constexpr static const double default_sigma_z = 4.07; - constexpr static const double log_2_pi = std::log(2 * M_PI); + constexpr static const double log_2_pi = 1.837877066409346; // std::log(2. * M_PI); // closures to precompute log -> only simple floating point operations struct EmissionLogProbability @@ -86,8 +85,7 @@ template class MapMatching final double log_sigma_z; EmissionLogProbability(const double sigma_z) - : sigma_z(sigma_z) - , log_sigma_z(std::log(sigma_z)) + : sigma_z(sigma_z), log_sigma_z(std::log(sigma_z)) { } @@ -100,16 +98,9 @@ template class MapMatching final { double beta; double log_beta; - TransitionLogProbability(const double beta) - : beta(beta) - , log_beta(std::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 operator()(const double d_t) const { return -log_beta - d_t / beta; } }; double get_network_distance(const PhantomNode &source_phantom, @@ -159,13 +150,13 @@ template class MapMatching final { if (0 < forward_heap.Size()) { - super::RoutingStep( - forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, true); + 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); + super::RoutingStep(reverse_heap, forward_heap, &middle_node, &upper_bound, + edge_offset, false); } } @@ -206,10 +197,12 @@ template class MapMatching final std::vector breakage; const Matching::CandidateLists &candidates_list; - const EmissionLogProbability& emission_log_probability; + 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) + 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) { @@ -276,8 +269,7 @@ template class MapMatching final // Provides the debug interface for introspection tools struct DebugInfo { - DebugInfo(const osrm::json::Logger* logger) - : logger(logger) + DebugInfo(const osrm::json::Logger *logger) : logger(logger) { if (logger) { @@ -285,7 +277,7 @@ template class MapMatching final } } - void initialize(const Matching::CandidateLists& candidates_list) + void initialize(const Matching::CandidateLists &candidates_list) { // json logger not enabled if (!logger) @@ -302,7 +294,8 @@ template class MapMatching final 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["viterbi"] = + osrm::json::clamp_float(Matching::IMPOSSIBLE_LOG_PROB); state.values["pruned"] = 0u; timestamps.values.push_back(state); } @@ -327,21 +320,18 @@ template class MapMatching final 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); + 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); - + .get() + .values.push_back(transistion); } - void set_viterbi(const std::vector>& viterbi, - const std::vector>& pruned) + void set_viterbi(const std::vector> &viterbi, + const std::vector> &pruned) { // json logger not enabled if (!logger) @@ -351,8 +341,10 @@ template class MapMatching final { 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]); + 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]); } } } @@ -366,7 +358,7 @@ template class MapMatching final osrm::json::get(*object, "states", t, s, "chosen") = true; } - void add_breakage(const std::vector& breakage) + void add_breakage(const std::vector &breakage) { // json logger not enabled if (!logger) @@ -375,8 +367,8 @@ template class MapMatching final osrm::json::get(*object, "breakage") = osrm::json::make_array(breakage); } - const osrm::json::Logger* logger; - osrm::json::Value* object; + const osrm::json::Logger *logger; + osrm::json::Value *object; }; public: @@ -395,8 +387,10 @@ template class MapMatching final 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 + : default_sigma_z); + TransitionLogProbability transition_log_probability(matching_beta > 0 ? matching_beta + : default_beta); HiddenMarkovModel model(candidates_list, emission_log_probability); @@ -429,8 +423,8 @@ template class MapMatching final } else { - trace_split = trace_split || (t - prev_unbroken_timestamps.back() > - Matching::MAX_BROKEN_STATES); + trace_split = trace_split || + (t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES); } if (trace_split) @@ -456,7 +450,7 @@ template class MapMatching final // Important: We potentially go back here! // However since t > new_start >= breakge_begin // we can only reset trace_coordindates.size() times. - t = new_start+1; + t = new_start + 1; } unsigned prev_unbroken_timestamp = prev_unbroken_timestamps.back(); @@ -506,11 +500,8 @@ template class MapMatching final new_value += transition_pr; debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime, - prev_viterbi[s], - emission_pr, - transition_pr, - network_distance, - great_circle_distance); + prev_viterbi[s], emission_pr, transition_pr, + network_distance, great_circle_distance); if (new_value > current_viterbi[s_prime]) { @@ -622,6 +613,6 @@ template class MapMatching final }; //[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM -//GIS +// GIS #endif /* MAP_MATCHING_HPP */ From 20091e94c8a7748306d82fd2907ea8e2aef212eb Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 11:50:02 +0100 Subject: [PATCH 46/67] fix compilation - remove wrong comments - fix include guard footer - add curly braces - add template specialization for std::vector in make_array - reformat --- util/json_util.hpp | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/util/json_util.hpp b/util/json_util.hpp index 563440d19..013ada408 100644 --- a/util/json_util.hpp +++ b/util/json_util.hpp @@ -25,9 +25,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -// based on -// https://svn.apache.org/repos/asf/mesos/tags/release-0.9.0-incubating-RC0/src/common/json.hpp - #ifndef JSON_UTIL_HPP #define JSON_UTIL_HPP @@ -71,26 +68,39 @@ template osrm::json::Array make_array(Args... args) return a; } -template osrm::json::Array make_array(const std::vector& vector) +template osrm::json::Array make_array(const std::vector &vector) { osrm::json::Array a; - for (const auto& v : vector) + for (const auto &v : vector) + { a.values.emplace_back(v); + } + return a; +} + +// template specialization needed as clang does not play nice +template <> osrm::json::Array make_array(const std::vector &vector) +{ + osrm::json::Array a; + for (const bool v : vector) + { + a.values.emplace_back(v); + } return a; } // Easy acces to object hierachies -osrm::json::Value& get(osrm::json::Value& value) { return value; } +osrm::json::Value &get(osrm::json::Value &value) { return value; } -template -osrm::json::Value& get(osrm::json::Value& value, const char* key, Keys... keys) +template +osrm::json::Value &get(osrm::json::Value &value, const char *key, Keys... keys) { using recursive_object_t = mapbox::util::recursive_wrapper; return get(value.get().get().values[key], keys...); } -template -osrm::json::Value& get(osrm::json::Value& value, unsigned key, Keys... keys) +template +osrm::json::Value &get(osrm::json::Value &value, unsigned key, Keys... keys) { using recursive_array_t = mapbox::util::recursive_wrapper; return get(value.get().get().values[key], keys...); @@ -98,4 +108,4 @@ osrm::json::Value& get(osrm::json::Value& value, unsigned key, Keys... keys) } // namespace json } // namespace osrm -#endif // JSON_RENDERER_HPP +#endif // JSON_UTIL_HPP From 91792f45ea527398375bc1f9afacfbf6448040a8 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 11:50:37 +0100 Subject: [PATCH 47/67] fix compilation - missing new 10th parameter to GenerateServerProgramOptions --- tools/simpleclient.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tools/simpleclient.cpp b/tools/simpleclient.cpp index 40407ced2..ce590ccd8 100644 --- a/tools/simpleclient.cpp +++ b/tools/simpleclient.cpp @@ -43,12 +43,13 @@ int main(int argc, const char *argv[]) try { std::string ip_address; - int ip_port, requested_thread_num; + int ip_port, requested_thread_num, max_locations_map_matching; bool trial_run = false; libosrm_config lib_config; const unsigned init_result = GenerateServerProgramOptions( argc, argv, lib_config.server_paths, ip_address, ip_port, requested_thread_num, - lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table); + lib_config.use_shared_memory, trial_run, lib_config.max_locations_distance_table, + max_locations_map_matching); if (init_result == INIT_OK_DO_NOT_START_ENGINE) { From 4df215e67413ec07eafa0ca8adea1eb70c107968 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 11:53:31 +0100 Subject: [PATCH 48/67] replace -std::numeric_limits::max() with ::lowest() --- util/json_util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util/json_util.hpp b/util/json_util.hpp index 013ada408..461148ea3 100644 --- a/util/json_util.hpp +++ b/util/json_util.hpp @@ -47,7 +47,7 @@ template T clamp_float(T d) } if (-std::numeric_limits::infinity() == d) { - return -std::numeric_limits::max(); + return std::numeric_limits::lowest(); } return d; From 592bebaf294446069a2cafe03d67dd45c7cea3e0 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 12:01:40 +0100 Subject: [PATCH 49/67] renamed: plugins/map_matching.hpp -> plugins/match.hpp to avoid confusion with routing_algorithms/map_matching.hpp --- library/osrm_impl.cpp | 2 +- plugins/{map_matching.hpp => match.hpp} | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) rename plugins/{map_matching.hpp => match.hpp} (99%) diff --git a/library/osrm_impl.cpp b/library/osrm_impl.cpp index f12445f2f..5bb58d174 100644 --- a/library/osrm_impl.cpp +++ b/library/osrm_impl.cpp @@ -42,7 +42,7 @@ class named_mutex; #include "../plugins/nearest.hpp" #include "../plugins/timestamp.hpp" #include "../plugins/viaroute.hpp" -#include "../plugins/map_matching.hpp" +#include "../plugins/match.hpp" #include "../server/data_structures/datafacade_base.hpp" #include "../server/data_structures/internal_datafacade.hpp" #include "../server/data_structures/shared_barriers.hpp" diff --git a/plugins/map_matching.hpp b/plugins/match.hpp similarity index 99% rename from plugins/map_matching.hpp rename to plugins/match.hpp index a67a9e075..b67a4f344 100644 --- a/plugins/map_matching.hpp +++ b/plugins/match.hpp @@ -25,8 +25,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef MAP_MATCHING_PLUGIN_HPP -#define MAP_MATCHING_PLUGIN_HPP +#ifndef MATCH_HPP +#define MATCH_HPP #include "plugin_base.hpp" @@ -312,4 +312,4 @@ template class MapMatchingPlugin : public BasePlugin ClassifierT classifier; }; -#endif /* MAP_MATCHING_HPP */ +#endif // MATCH_HPP From a9c3b343fc25708f57d7b8542f2448662efa6857 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 12:48:33 +0100 Subject: [PATCH 50/67] 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 From 643ab92cd2f23dbeb266cd11b911fc1847f35851 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 12:55:42 +0100 Subject: [PATCH 51/67] fix default value for max_locations_map_matching --- util/routed_options.hpp | 60 +++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/util/routed_options.hpp b/util/routed_options.hpp index 0aef32147..046ab71b3 100644 --- a/util/routed_options.hpp +++ b/util/routed_options.hpp @@ -166,35 +166,37 @@ inline unsigned GenerateServerProgramOptions(const int argc, // declare a group of options that will be allowed both on command line // as well as in a config file boost::program_options::options_description config_options("Configuration"); - config_options.add_options() - ("hsgrdata", boost::program_options::value(&paths["hsgrdata"]), - ".hsgr file") - ("nodesdata", boost::program_options::value(&paths["nodesdata"]), - ".nodes file") - ("edgesdata", boost::program_options::value(&paths["edgesdata"]), - ".edges file") - ("geometry", boost::program_options::value(&paths["geometries"]), - ".geometry file") - ("ramindex", boost::program_options::value(&paths["ramindex"]), - ".ramIndex file") - ("fileindex", boost::program_options::value(&paths["fileindex"]), - "File index file") - ("namesdata", boost::program_options::value(&paths["namesdata"]), - ".names file") - ("timestamp", boost::program_options::value(&paths["timestamp"]), - ".timestamp file") - ("ip,i", boost::program_options::value(&ip_address)->default_value("0.0.0.0"), - "IP address") - ("port,p", boost::program_options::value(&ip_port)->default_value(5000), - "TCP/IP port") - ("threads,t", boost::program_options::value(&requested_num_threads)->default_value(8), - "Number of threads to use") - ("shared-memory,s", boost::program_options::value(&use_shared_memory)->implicit_value(true), - "Load data from shared memory") - ("max-table-size,m", boost::program_options::value(&max_locations_distance_table)->default_value(100), - "Max. locations supported in distance table query") - ("max-matching-size,m", boost::program_options::value(&max_locations_map_matching)->default_value(-1), - "Max. locations supported in map matching query"); + config_options.add_options()( + "hsgrdata", boost::program_options::value(&paths["hsgrdata"]), + ".hsgr file")("nodesdata", + boost::program_options::value(&paths["nodesdata"]), + ".nodes file")( + "edgesdata", boost::program_options::value(&paths["edgesdata"]), + ".edges file")("geometry", + boost::program_options::value(&paths["geometries"]), + ".geometry file")( + "ramindex", boost::program_options::value(&paths["ramindex"]), + ".ramIndex file")( + "fileindex", boost::program_options::value(&paths["fileindex"]), + "File index file")( + "namesdata", boost::program_options::value(&paths["namesdata"]), + ".names file")("timestamp", + boost::program_options::value(&paths["timestamp"]), + ".timestamp file")( + "ip,i", boost::program_options::value(&ip_address)->default_value("0.0.0.0"), + "IP address")("port,p", boost::program_options::value(&ip_port)->default_value(5000), + "TCP/IP port")( + "threads,t", boost::program_options::value(&requested_num_threads)->default_value(8), + "Number of threads to use")( + "shared-memory,s", + boost::program_options::value(&use_shared_memory)->implicit_value(true), + "Load data from shared memory")( + "max-table-size,m", + boost::program_options::value(&max_locations_distance_table)->default_value(100), + "Max. locations supported in distance table query")( + "max-matching-size,m", + boost::program_options::value(&max_locations_map_matching)->default_value(2), + "Max. locations supported in map matching query"); // hidden options, will be allowed both on command line and in config // file, but will not be shown to the user From 402ca780bf8115b1274bc50fec4c1639519ca305 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 12:56:37 +0100 Subject: [PATCH 52/67] fix test expectation osrm-routed help output --- features/options/routed/help.feature | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/features/options/routed/help.feature b/features/options/routed/help.feature index 4b89dcb24..c0fca4717 100644 --- a/features/options/routed/help.feature +++ b/features/options/routed/help.feature @@ -26,7 +26,8 @@ Feature: osrm-routed command line options: help And stdout should contain "--threads" And stdout should contain "--shared-memory" And stdout should contain "--max-table-size" - And stdout should contain 24 lines + And stdout should contain "--max-matching-size" + And stdout should contain 26 lines And it should exit with code 0 Scenario: osrm-routed - Help, short @@ -51,7 +52,8 @@ Feature: osrm-routed command line options: help And stdout should contain "--threads" And stdout should contain "--shared-memory" And stdout should contain "--max-table-size" - And stdout should contain 24 lines + And stdout should contain "--max-matching-size" + And stdout should contain 26 lines And it should exit with code 0 Scenario: osrm-routed - Help, long @@ -76,5 +78,6 @@ Feature: osrm-routed command line options: help And stdout should contain "--threads" And stdout should contain "--shared-memory" And stdout should contain "--max-table-size" - And stdout should contain 24 lines + And stdout should contain "--max-matching-size" + And stdout should contain 26 lines And it should exit with code 0 From 6460fdc62b5409cd867500e552050617da3c896d Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 13:03:00 +0100 Subject: [PATCH 53/67] use std::size_t for timestamps to avoid implicit casts, use range-based for loops --- data_structures/hidden_markov_model.hpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp index affaf3013..db58d96a0 100644 --- a/data_structures/hidden_markov_model.hpp +++ b/data_structures/hidden_markov_model.hpp @@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef HIDDEN_MARKOV_MODEL #define HIDDEN_MARKOV_MODEL +#include "../util/integer_range.hpp" + #include #include @@ -44,7 +46,7 @@ constexpr static const double log_2_pi = 1.837877066409346; // std::log(2. * M_P 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(); +constexpr static const std::size_t INVALID_STATE = std::numeric_limits::max(); } // namespace matching } // namespace osrm @@ -101,12 +103,12 @@ template struct HiddenMarkovModel clear(0); } - void clear(unsigned initial_timestamp) + 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 (unsigned t = initial_timestamp; t < viterbi.size(); t++) + for (const auto t : osrm::irange(initial_timestamp, viterbi.size())) { 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)); @@ -116,13 +118,13 @@ template struct HiddenMarkovModel std::fill(breakage.begin() + initial_timestamp, breakage.end(), true); } - unsigned initialize(unsigned initial_timestamp) + std::size_t initialize(std::size_t initial_timestamp) { BOOST_ASSERT(initial_timestamp < candidates_list.size()); do { - for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s) + for (const auto s : osrm::irange(0u, viterbi[initial_timestamp].size())) { viterbi[initial_timestamp][s] = emission_log_probability(candidates_list[initial_timestamp][s].second); From 5af0ceb2d29073ce2a941023390c1bfb433c4042 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 14:22:27 +0100 Subject: [PATCH 54/67] use range based for loop --- data_structures/hidden_markov_model.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp index db58d96a0..047c5bb3c 100644 --- a/data_structures/hidden_markov_model.hpp +++ b/data_structures/hidden_markov_model.hpp @@ -124,7 +124,7 @@ template struct HiddenMarkovModel do { - for (const auto s : osrm::irange(0u, viterbi[initial_timestamp].size())) + for (const auto s : osrm::irange(0u, viterbi[initial_timestamp].size())) { viterbi[initial_timestamp][s] = emission_log_probability(candidates_list[initial_timestamp][s].second); From e02c721c2b5fec7ad3e6ff43e26603a2eb00cdd9 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 14:22:52 +0100 Subject: [PATCH 55/67] further untangle model from functionality and put classes into seperate headers - move get_network_distance() into routing base class - don't reallocate queues every time but clear them. Should be cheaper --- routing_algorithms/map_matching.hpp | 106 +++++----------------------- routing_algorithms/routing_base.hpp | 79 +++++++++++++++++++++ 2 files changed, 97 insertions(+), 88 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 2000242d3..d2e3c395b 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -80,91 +80,6 @@ class MapMatching final : public BasicRoutingInterfaceGetNumberOfNodes()); - 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); - } - } - - double distance = std::numeric_limits::max(); - if (upper_bound != INVALID_EDGE_WEIGHT) - { - std::vector packed_leg; - super::RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg); - std::vector unpacked_path; - PhantomNodes nodes; - nodes.source_phantom = source_phantom; - nodes.target_phantom = target_phantom; - super::UnpackPath(packed_leg, nodes, unpacked_path); - - FixedPointCoordinate previous_coordinate = source_phantom.location; - FixedPointCoordinate current_coordinate; - distance = 0; - for (const auto &p : unpacked_path) - { - current_coordinate = super::facade->GetCoordinateOfNode(p.node); - distance += coordinate_calculation::great_circle_distance(previous_coordinate, - current_coordinate); - previous_coordinate = current_coordinate; - } - distance += coordinate_calculation::great_circle_distance(previous_coordinate, - target_phantom.location); - } - - return distance; - } - public: MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data) : super(facade), engine_working_data(engine_working_data) @@ -261,6 +176,14 @@ class MapMatching final : public BasicRoutingInterfaceGetNumberOfNodes()); + 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); + // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { @@ -274,12 +197,17 @@ class MapMatching final : public BasicRoutingInterface new_value) + { continue; + } + + forward_heap.Clear(); + reverse_heap.Clear(); // get distance diff between loc1/2 and locs/s_prime - const auto network_distance = - get_network_distance(prev_unbroken_timestamps_list[s].first, - current_timestamps_list[s_prime].first); + const auto network_distance = super::get_network_distance( + forward_heap, reverse_heap, prev_unbroken_timestamps_list[s].first, + current_timestamps_list[s_prime].first); const auto great_circle_distance = coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate); @@ -288,7 +216,9 @@ class MapMatching final : public BasicRoutingInterface prune if (d_t > 500) + { continue; + } const double transition_pr = transition_log_probability(d_t); new_value += transition_pr; diff --git a/routing_algorithms/routing_base.hpp b/routing_algorithms/routing_base.hpp index e10c86a59..52c16a77e 100644 --- a/routing_algorithms/routing_base.hpp +++ b/routing_algorithms/routing_base.hpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef ROUTING_BASE_HPP #define ROUTING_BASE_HPP +#include "../data_structures/coordinate_calculation.hpp" #include "../data_structures/internal_route_result.hpp" #include "../data_structures/search_engine_data.hpp" #include "../data_structures/turn_instructions.hpp" @@ -410,6 +411,84 @@ template class BasicRoutingInterface packed_path.emplace_back(current_node_id); } } + + double get_network_distance(SearchEngineData::QueryHeap &forward_heap, + SearchEngineData::QueryHeap &reverse_heap, + 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()); + + 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()) + { + RoutingStep(forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, + true); + } + if (0 < reverse_heap.Size()) + { + RoutingStep(reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, + false); + } + } + + double distance = std::numeric_limits::max(); + if (upper_bound != INVALID_EDGE_WEIGHT) + { + std::vector packed_leg; + RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg); + std::vector unpacked_path; + PhantomNodes nodes; + nodes.source_phantom = source_phantom; + nodes.target_phantom = target_phantom; + UnpackPath(packed_leg, nodes, unpacked_path); + + FixedPointCoordinate previous_coordinate = source_phantom.location; + FixedPointCoordinate current_coordinate; + distance = 0; + for (const auto &p : unpacked_path) + { + current_coordinate = facade->GetCoordinateOfNode(p.node); + distance += coordinate_calculation::great_circle_distance(previous_coordinate, + current_coordinate); + previous_coordinate = current_coordinate; + } + distance += coordinate_calculation::great_circle_distance(previous_coordinate, + target_phantom.location); + } + return distance; + } }; #endif // ROUTING_BASE_HPP From 133e382aaee25db49dcf2ef53a3d0115ee456340 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 14:43:38 +0100 Subject: [PATCH 56/67] remove constexpr qualifier as numeric_limits is not yet constexpr on MSVC compiler --- data_structures/hidden_markov_model.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp index 047c5bb3c..bae2ba55a 100644 --- a/data_structures/hidden_markov_model.hpp +++ b/data_structures/hidden_markov_model.hpp @@ -42,11 +42,10 @@ 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 std::size_t INVALID_STATE = std::numeric_limits::max(); +static const double log_2_pi = std::log(2. * M_PI); +static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits::infinity(); +static const double MINIMAL_LOG_PROB = std::numeric_limits::lowest(); +static const std::size_t INVALID_STATE = std::numeric_limits::max(); } // namespace matching } // namespace osrm From 31cae8f05f15c946ad43c08ca86d76894d761828 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 17:41:12 +0100 Subject: [PATCH 57/67] several copy edits to brush up the code - fix copyright header - rename probabilityDensityFunction -> density_function - use double-precision fp literal to indicate intent - remove redundant enum class start value - replace C-style comments with C++ style - make functions const --- algorithms/bayes_classifier.hpp | 59 +++++++++++++++++---------------- 1 file changed, 31 insertions(+), 28 deletions(-) diff --git a/algorithms/bayes_classifier.hpp b/algorithms/bayes_classifier.hpp index 042259111..3358144c7 100644 --- a/algorithms/bayes_classifier.hpp +++ b/algorithms/bayes_classifier.hpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2015, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM contributors All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -28,72 +28,75 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef BAYES_CLASSIFIER_HPP #define BAYES_CLASSIFIER_HPP -#include #include +#include + struct NormalDistribution { NormalDistribution(const double mean, const double standard_deviation) - : mean(mean) - , standard_deviation(standard_deviation) + : mean(mean), standard_deviation(standard_deviation) { } // FIXME implement log-probability version since its faster - double probabilityDensityFunction(const double val) + double density_function(const double val) const { const double x = val - mean; - return 1.0 / (std::sqrt(2*M_PI) * standard_deviation) * std::exp(-x*x / (standard_deviation * standard_deviation)); + return 1.0 / (std::sqrt(2. * M_PI) * standard_deviation) * + std::exp(-x * x / (standard_deviation * standard_deviation)); } double mean; double standard_deviation; }; - struct LaplaceDistribution { LaplaceDistribution(const double location, const double scale) - : location(location) - , scale(scale) + : location(location), scale(scale) { } // FIXME implement log-probability version since its faster - double probabilityDensityFunction(const double val) const + double density_function(const double val) const { const double x = std::abs(val - location); - return 1.0 / (2*scale) * std::exp(-x / scale); + return 1.0 / (2. * scale) * std::exp(-x / scale); } double location; double scale; }; -template +template class BayesClassifier { -public: - enum class ClassLabel : unsigned {NEGATIVE = 0, POSITIVE}; + public: + enum class ClassLabel : unsigned + { + NEGATIVE, + POSITIVE + }; using ClassificationT = std::pair; - BayesClassifier(const PositiveDistributionT& positive_distribution, - const NegativeDistributionT& negative_distribution, + BayesClassifier(const PositiveDistributionT &positive_distribution, + const NegativeDistributionT &negative_distribution, const double positive_apriori_probability) - : positive_distribution(positive_distribution) - , negative_distribution(negative_distribution) - , positive_apriori_probability(positive_apriori_probability) - , negative_apriori_probability(1 - positive_apriori_probability) + : positive_distribution(positive_distribution), + negative_distribution(negative_distribution), + positive_apriori_probability(positive_apriori_probability), + negative_apriori_probability(1. - positive_apriori_probability) { } - /* - * Returns label and the probability of the label. - */ - ClassificationT classify(const ValueT& v) const + // Returns label and the probability of the label. + ClassificationT classify(const ValueT &v) const { - const double positive_postpriori = positive_apriori_probability * positive_distribution.probabilityDensityFunction(v); - const double negative_postpriori = negative_apriori_probability * negative_distribution.probabilityDensityFunction(v); + const double positive_postpriori = + positive_apriori_probability * positive_distribution.density_function(v); + const double negative_postpriori = + negative_apriori_probability * negative_distribution.density_function(v); const double norm = positive_postpriori + negative_postpriori; if (positive_postpriori > negative_postpriori) @@ -104,11 +107,11 @@ public: return std::make_pair(ClassLabel::NEGATIVE, negative_postpriori / norm); } -private: + private: PositiveDistributionT positive_distribution; NegativeDistributionT negative_distribution; double positive_apriori_probability; double negative_apriori_probability; }; -#endif /* BAYES_CLASSIFIER_HPP */ +#endif // BAYES_CLASSIFIER_HPP From b9922bc90b15d7cb7469180776c339fb88f60150 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 17:43:37 +0100 Subject: [PATCH 58/67] replace C-style comments --- data_structures/static_rtree.hpp | 41 ++++++++++++++++---------------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/data_structures/static_rtree.hpp b/data_structures/static_rtree.hpp index f9cfbfca0..f219a6475 100644 --- a/data_structures/static_rtree.hpp +++ b/data_structures/static_rtree.hpp @@ -793,12 +793,10 @@ class StaticRTree return !result_phantom_node_vector.empty(); } - /** - * Returns elements within max_distance. - * If the minium of elements could not be found in the search radius, widen - * it until the minimum can be satisfied. - * At the number of returned nodes is capped at the given maximum. - */ + // Returns elements within max_distance. + // If the minium of elements could not be found in the search radius, widen + // it until the minimum can be satisfied. + // At the number of returned nodes is capped at the given maximum. bool IncrementalFindPhantomNodeForCoordinateWithDistance( const FixedPointCoordinate &input_coordinate, std::vector> &result_phantom_node_vector, @@ -888,7 +886,7 @@ class StaticRTree // 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-1 && + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes - 1 && current_segment.is_in_tiny_cc()) { continue; @@ -899,14 +897,14 @@ class StaticRTree 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), - m_coordinate_list->at(current_segment.v), input_coordinate, - projected_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); - if (number_of_elements_from_big_cc > 0 - && result_phantom_node_vector.size() >= min_number_of_phantom_nodes - && current_perpendicular_distance >= max_distance) + if (number_of_elements_from_big_cc > 0 && + result_phantom_node_vector.size() >= min_number_of_phantom_nodes && + current_perpendicular_distance >= max_distance) { traversal_queue = std::priority_queue{}; continue; @@ -914,13 +912,14 @@ class StaticRTree // 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), + 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); // Hack to fix rounding errors and wandering via nodes. From cfaacf7cb2fcb7b8a1e5bbbc0e8bfcfbd8f71f38 Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 17:58:17 +0100 Subject: [PATCH 59/67] put util/compute_angle.cpp into OBJECT library to avoid repetetive compiles --- CMakeLists.txt | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bbebb485a..ee97ccfdc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,24 +59,25 @@ add_library(LOGGER OBJECT util/simple_logger.cpp) add_library(PHANTOMNODE OBJECT data_structures/phantom_node.cpp) add_library(EXCEPTION OBJECT util/osrm_exception.cpp) add_library(MERCATOR OBJECT util/mercator.cpp) +add_library(ANGLE OBJECT util/compute_angle.cpp) set(ExtractorSources extract.cpp ${ExtractorGlob}) add_executable(osrm-extract ${ExtractorSources} $ $ $ $ $ $ $) add_library(RESTRICTION OBJECT data_structures/restriction_map.cpp) -file(GLOB PrepareGlob contractor/*.cpp data_structures/hilbert_value.cpp util/compute_angle.cpp {RestrictionMapGlob}) +file(GLOB PrepareGlob contractor/*.cpp data_structures/hilbert_value.cpp {RestrictionMapGlob}) set(PrepareSources prepare.cpp ${PrepareGlob}) -add_executable(osrm-prepare ${PrepareSources} $ $ $ $ $ $ $ $) +add_executable(osrm-prepare ${PrepareSources} $ $ $ $ $ $ $ $ $) -file(GLOB ServerGlob server/*.cpp util/compute_angle.cpp) +file(GLOB ServerGlob server/*.cpp) file(GLOB DescriptorGlob descriptors/*.cpp) file(GLOB DatastructureGlob data_structures/search_engine_data.cpp data_structures/route_parameters.cpp util/bearing.cpp) list(REMOVE_ITEM DatastructureGlob data_structures/Coordinate.cpp) file(GLOB CoordinateGlob data_structures/coordinate*.cpp) file(GLOB AlgorithmGlob algorithms/*.cpp) file(GLOB HttpGlob server/http/*.cpp) -file(GLOB LibOSRMGlob library/*.cpp util/compute_angle.cpp) +file(GLOB LibOSRMGlob library/*.cpp) file(GLOB DataStructureTestsGlob unit_tests/data_structures/*.cpp data_structures/hilbert_value.cpp) file(GLOB AlgorithmTestsGlob unit_tests/algorithms/*.cpp) @@ -93,7 +94,7 @@ set( add_library(COORDINATE OBJECT ${CoordinateGlob}) add_library(FINGERPRINT OBJECT util/fingerprint.cpp) add_library(GITDESCRIPTION OBJECT util/git_sha.cpp) -add_library(OSRM ${OSRMSources} $ $ $ $ $ $ $) +add_library(OSRM ${OSRMSources} $ $ $ $ $ $ $ $ $) add_dependencies(FINGERPRINT FingerPrintConfigure) add_executable(osrm-routed routed.cpp ${ServerGlob} $) From d43716612b5f7730bbc3767367c2b797957b0e2a Mon Sep 17 00:00:00 2001 From: Dennis Luxen Date: Tue, 3 Mar 2015 18:17:12 +0100 Subject: [PATCH 60/67] several copyedits to brush up code - remove unneeded includes - replace size() <==> 0 comparisions with calls to empty() - use fp instead of integer literals - use range-based for loops with integer ranges - add a couple of consts --- routing_algorithms/map_matching.hpp | 36 +++++++++++++---------------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index d2e3c395b..4246539ef 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -33,15 +33,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #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" -#include #include -#include - #include #include #include @@ -93,13 +88,13 @@ class MapMatching final : public BasicRoutingInterface 0); + BOOST_ASSERT(!candidates_list.empty()); // TODO replace default values with table lookup based on sampling frequency EmissionLogProbability emission_log_probability( - gps_precision > 0 ? gps_precision : osrm::matching::default_sigma_z); + gps_precision > 0. ? gps_precision : osrm::matching::default_sigma_z); TransitionLogProbability transition_log_probability( - matching_beta > 0 ? matching_beta : osrm::matching::default_beta); + matching_beta > 0. ? matching_beta : osrm::matching::default_beta); osrm::matching::HMM model(candidates_list, emission_log_probability); @@ -120,10 +115,10 @@ class MapMatching final : public BasicRoutingInterface 0) + if (!trace_timestamps.empty()) { trace_split = trace_split || @@ -162,7 +157,8 @@ class MapMatching final : public BasicRoutingInterface(0u, prev_viterbi.size())) { if (prev_pruned[s]) + { continue; + } - for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime) + for (const auto s_prime : osrm::irange(0u, current_viterbi.size())) { // how likely is candidate s_prime at time t to be emitted? const double emission_pr = @@ -247,7 +245,6 @@ class MapMatching final : public BasicRoutingInterface 0); - // remove both ends of the breakage prev_unbroken_timestamps.pop_back(); } @@ -259,7 +256,7 @@ class MapMatching final : public BasicRoutingInterface 0) + if (!prev_unbroken_timestamps.empty()) { split_points.push_back(prev_unbroken_timestamps.back() + 1); } @@ -286,8 +283,9 @@ class MapMatching final : public BasicRoutingInterface(0u, reconstructed_indices.size())) { auto timestamp_index = reconstructed_indices[i].first; auto location_index = reconstructed_indices[i].second; @@ -328,10 +326,8 @@ class MapMatching final : public BasicRoutingInterface Date: Tue, 3 Mar 2015 18:26:38 +0100 Subject: [PATCH 61/67] add override specifier --- server/data_structures/internal_datafacade.hpp | 15 ++++++++------- server/data_structures/shared_datafacade.hpp | 15 ++++++++------- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/server/data_structures/internal_datafacade.hpp b/server/data_structures/internal_datafacade.hpp index a85a78e30..640a5f974 100644 --- a/server/data_structures/internal_datafacade.hpp +++ b/server/data_structures/internal_datafacade.hpp @@ -416,12 +416,12 @@ template class InternalDataFacade final : public BaseDataFacad input_coordinate, resulting_phantom_node_vector, number_of_results); } - bool - IncrementalFindPhantomNodeForCoordinateWithMaxDistance(const FixedPointCoordinate &input_coordinate, - std::vector> &resulting_phantom_node_vector, - const double max_distance, - const unsigned min_number_of_phantom_nodes, - const unsigned max_number_of_phantom_nodes) final + bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance( + const FixedPointCoordinate &input_coordinate, + std::vector> &resulting_phantom_node_vector, + const double max_distance, + const unsigned min_number_of_phantom_nodes, + const unsigned max_number_of_phantom_nodes) override final { if (!m_static_rtree.get()) { @@ -429,7 +429,8 @@ template class InternalDataFacade final : public BaseDataFacad } return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes); + input_coordinate, resulting_phantom_node_vector, max_distance, + min_number_of_phantom_nodes, max_number_of_phantom_nodes); } unsigned GetCheckSum() const override final { return m_check_sum; } diff --git a/server/data_structures/shared_datafacade.hpp b/server/data_structures/shared_datafacade.hpp index 87af81929..93dfdb95d 100644 --- a/server/data_structures/shared_datafacade.hpp +++ b/server/data_structures/shared_datafacade.hpp @@ -404,12 +404,12 @@ template class SharedDataFacade final : public BaseDataFacade< input_coordinate, resulting_phantom_node_vector, number_of_results); } - bool - IncrementalFindPhantomNodeForCoordinateWithMaxDistance(const FixedPointCoordinate &input_coordinate, - std::vector> &resulting_phantom_node_vector, - const double max_distance, - const unsigned min_number_of_phantom_nodes, - const unsigned max_number_of_phantom_nodes) final + bool IncrementalFindPhantomNodeForCoordinateWithMaxDistance( + const FixedPointCoordinate &input_coordinate, + std::vector> &resulting_phantom_node_vector, + const double max_distance, + const unsigned min_number_of_phantom_nodes, + const unsigned max_number_of_phantom_nodes) override final { if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first) { @@ -417,7 +417,8 @@ template class SharedDataFacade final : public BaseDataFacade< } return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance( - input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes); + input_coordinate, resulting_phantom_node_vector, max_distance, + min_number_of_phantom_nodes, max_number_of_phantom_nodes); } unsigned GetCheckSum() const override final { return m_check_sum; } From 98dba11c5ef9c8bf90b58e1e9b6b94618a02599e Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Wed, 4 Mar 2015 01:34:45 +0100 Subject: [PATCH 62/67] Address some of the remaining issues of the code review --- routing_algorithms/map_matching.hpp | 2 +- server/api_grammar.hpp | 16 ++++++++-------- util/json_logger.hpp | 6 ++++-- util/json_util.hpp | 12 ++---------- 4 files changed, 15 insertions(+), 21 deletions(-) diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 4246539ef..89c4e357c 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -272,7 +272,7 @@ class MapMatching final : public BasicRoutingInterface= sub_matching_begin && model.breakage[parent_timestamp_index]) { - parent_timestamp_index--; + --parent_timestamp_index; } // matchings that only consist of one candidate are invalid diff --git a/server/api_grammar.hpp b/server/api_grammar.hpp index 5c7dcde45..a629cfbf7 100644 --- a/server/api_grammar.hpp +++ b/server/api_grammar.hpp @@ -65,12 +65,6 @@ template struct APIGrammar : qi::grammar> qi::lit("t") >> '=' >> qi::uint_[boost::bind(&HandlerT::addTimestamp, handler, ::_1)]; - matching_beta = (-qi::lit('&')) >> qi::lit("matching_beta") >> '=' >> - qi::short_[boost::bind(&HandlerT::setMatchingBeta, handler, ::_1)]; - gps_precision = (-qi::lit('&')) >> qi::lit("gps_precision") >> '=' >> - qi::short_[boost::bind(&HandlerT::setGPSPrecision, handler, ::_1)]; - classify = (-qi::lit('&')) >> qi::lit("classify") >> '=' >> - qi::bool_[boost::bind(&HandlerT::setClassify, handler, ::_1)]; u = (-qi::lit('&')) >> qi::lit("u") >> '=' >> qi::bool_[boost::bind(&HandlerT::setUTurn, handler, ::_1)]; uturns = (-qi::lit('&')) >> qi::lit("uturns") >> '=' >> @@ -83,6 +77,12 @@ template struct APIGrammar : qi::grammar> qi::lit("num_results") >> '=' >> qi::short_[boost::bind(&HandlerT::setNumberOfResults, handler, ::_1)]; + matching_beta = (-qi::lit('&')) >> qi::lit("matching_beta") >> '=' >> + qi::short_[boost::bind(&HandlerT::setMatchingBeta, handler, ::_1)]; + gps_precision = (-qi::lit('&')) >> qi::lit("gps_precision") >> '=' >> + qi::short_[boost::bind(&HandlerT::setGPSPrecision, handler, ::_1)]; + classify = (-qi::lit('&')) >> qi::lit("classify") >> '=' >> + qi::bool_[boost::bind(&HandlerT::setClassify, handler, ::_1)]; string = +(qi::char_("a-zA-Z")); stringwithDot = +(qi::char_("a-zA-Z0-9_.-")); @@ -92,8 +92,8 @@ template struct APIGrammar : qi::grammar api_call, query; qi::rule service, zoom, output, string, jsonp, checksum, location, - hint, timestamp, matching_beta, gps_precision, classify, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, - uturns, old_API, num_results; + hint, timestamp, stringwithDot, stringwithPercent, language, instruction, geometry, cmp, alt_route, u, + uturns, old_API, num_results, matching_beta, gps_precision, classify; HandlerT *handler; }; diff --git a/util/json_logger.hpp b/util/json_logger.hpp index 872a926d5..03f8a171a 100644 --- a/util/json_logger.hpp +++ b/util/json_logger.hpp @@ -1,6 +1,6 @@ /* -Copyright (c) 2015, Project OSRM, Dennis Luxen, others +Copyright (c) 2015, Project OSRM contributors All rights reserved. Redistribution and use in source and binary forms, with or without modification, @@ -37,6 +37,8 @@ namespace osrm namespace json { +// Used to append additional debugging information to the JSON response in a +// thread safe manner. class Logger { using MapT = std::unordered_map; @@ -74,4 +76,4 @@ class Logger } } -#endif /* SIMPLE_LOGGER_HPP */ +#endif /* JSON_LOGGER_HPP */ diff --git a/util/json_util.hpp b/util/json_util.hpp index 461148ea3..bfb8a8bc4 100644 --- a/util/json_util.hpp +++ b/util/json_util.hpp @@ -30,8 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include +#include namespace osrm { @@ -53,18 +53,10 @@ template T clamp_float(T d) return d; } -void append_to_array(osrm::json::Array &a) {} -template -void append_to_array(osrm::json::Array &a, T value, Args... args) -{ - a.values.emplace_back(value); - append_to_array(a, args...); -} - template osrm::json::Array make_array(Args... args) { osrm::json::Array a; - append_to_array(a, args...); + append_to_container(a.values, args...); return a; } From 7829e3c132e31bed27a625c34067dab4b735187c Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Thu, 5 Mar 2015 00:12:26 +0100 Subject: [PATCH 63/67] Add step definition and support code for matching --- features/step_definitions/matching.rb | 88 +++++++++++++++++++++++++++ features/support/match.rb | 30 +++++++++ 2 files changed, 118 insertions(+) create mode 100644 features/step_definitions/matching.rb create mode 100644 features/support/match.rb diff --git a/features/step_definitions/matching.rb b/features/step_definitions/matching.rb new file mode 100644 index 000000000..c5d25f3d8 --- /dev/null +++ b/features/step_definitions/matching.rb @@ -0,0 +1,88 @@ +When /^I match I should get$/ do |table| + reprocess + actual = [] + OSRMLoader.load(self,"#{prepared_file}.osrm") do + table.hashes.each_with_index do |row,ri| + if row['request'] + got = {'request' => row['request'] } + response = request_url row['request'] + else + params = @query_params + trace = [] + timestamps = [] + if row['trace'] + row['trace'].split(',').each do |n| + node = find_node_by_name(n.strip) + raise "*** unknown waypoint node '#{n.strip}" unless node + trace << node + end + got = {'trace' => row['trace'] } + response = request_matching trace, timestamps, params + else + raise "*** no waypoints" + end + end + + row.each_pair do |k,v| + if k =~ /param:(.*)/ + if v=='(nil)' + params[$1]=nil + elsif v!=nil + params[$1]=v + end + got[k]=v + end + end + + if response.body.empty? == false + json = JSON.parse response.body + end + + if table.headers.include? 'status' + got['status'] = json['status'].to_s + end + if table.headers.include? 'message' + got['message'] = json['status_message'] + end + if table.headers.include? '#' # comment column + got['#'] = row['#'] # copy value so it always match + end + + sub_matchings = [] + if response.code == "200" + if table.headers.include? 'matchings' + sub_matchings = json['matchings'].compact.map { |sub| sub['matched_points']} + end + end + + ok = true + encoded_result = "" + extended_target = "" + row['matchings'].split(',').each_with_index do |sub, sub_idx| + sub.length.times do |node_idx| + node = find_node_by_name(sub[node_idx]) + out_node = sub_matchings[sub_idx][node_idx] + if FuzzyMatch.match_location out_node, node + encoded_result += sub[node_idx] + extended_target += sub[node_idx] + else + encoded_result += "[#{out_node[0]},#{out_node[1]}]" + extended_target += "#{sub[node_idx]} [#{node.lat},#{node.lon}]" + ok = false + end + end + end + if ok + got['matchings'] = row['matchings'] + else + got['matchings'] = encoded_result + row['matchings'] = extended_target + log_fail row,got, { 'matching' => {:query => @query, :response => response} } + end + + actual << got + end + end + table.routing_diff! actual +end + diff --git a/features/support/match.rb b/features/support/match.rb new file mode 100644 index 000000000..9a18a3a15 --- /dev/null +++ b/features/support/match.rb @@ -0,0 +1,30 @@ +require 'net/http' + +HOST = "http://127.0.0.1:#{OSRM_PORT}" +DESTINATION_REACHED = 15 #OSRM instruction code + +class Hash + def to_param(namespace = nil) + collect do |key, value| + "#{key}=#{value}" + end.sort + end +end + +def request_matching trace=[], timestamps=[], options={} + defaults = { 'output' => 'json' } + locs = waypoints.compact.map { |w| "loc=#{w.lat},#{w.lon}" } + ts = timestamps.compact.map { |t| "t=#{t}" } + params = (locs + ts + defaults.merge(options).to_param).join('&') + params = nil if params=="" + uri = URI.parse ["#{HOST}/match", params].compact.join('?') + @query = uri.to_s + Timeout.timeout(OSRM_TIMEOUT) do + Net::HTTP.get_response uri + end +rescue Errno::ECONNREFUSED => e + raise "*** osrm-routed is not running." +rescue Timeout::Error + raise "*** osrm-routed did not respond." +end + From 736bc87480309ddf3cd20b534d1c6d0243607b35 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sat, 7 Mar 2015 23:02:14 +0100 Subject: [PATCH 64/67] Fix inverted operator in maximum check --- plugins/match.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/match.hpp b/plugins/match.hpp index 88454eeb9..74e0c8491 100644 --- a/plugins/match.hpp +++ b/plugins/match.hpp @@ -232,7 +232,7 @@ template class MapMatchingPlugin : public BasePlugin // enforce maximum number of locations for performance reasons if (max_locations_map_matching > 0 && - static_cast(input_coords.size()) > max_locations_map_matching) + static_cast(input_coords.size()) < max_locations_map_matching) { return 400; } From 0c735953c9408fe69fe3da64c983ea78e85a744c Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 8 Mar 2015 00:51:07 +0100 Subject: [PATCH 65/67] Make uturn detection a little less sensitive. --- plugins/match.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/match.hpp b/plugins/match.hpp index 74e0c8491..2a9fa043f 100644 --- a/plugins/match.hpp +++ b/plugins/match.hpp @@ -124,7 +124,7 @@ template class MapMatchingPlugin : public BasePlugin input_coords[current_coordinate + 1]); // sharp turns indicate a possible uturn - if (turn_angle < 100.0 || turn_angle > 260.0) + if (turn_angle <= 90.0 || turn_angle >= 270.0) { allow_uturn = true; } From 028fad94af5348965d2530ce37cea5df88e3ca1a Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 8 Mar 2015 00:53:15 +0100 Subject: [PATCH 66/67] Fix overflows when handling size_t --- data_structures/hidden_markov_model.hpp | 1 - routing_algorithms/map_matching.hpp | 31 +++++++++++++------------ 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/data_structures/hidden_markov_model.hpp b/data_structures/hidden_markov_model.hpp index bae2ba55a..403f76608 100644 --- a/data_structures/hidden_markov_model.hpp +++ b/data_structures/hidden_markov_model.hpp @@ -41,7 +41,6 @@ namespace osrm { namespace matching { -// FIXME this value should be a table based on samples/meter (or samples/min) static const double log_2_pi = std::log(2. * M_PI); static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits::infinity(); static const double MINIMAL_LOG_PROB = std::numeric_limits::lowest(); diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 89c4e357c..80fe49cce 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -88,7 +88,7 @@ class MapMatching final : public BasicRoutingInterface::max(); - std::vector split_points; - std::vector prev_unbroken_timestamps; + std::size_t breakage_begin = osrm::matching::INVALID_STATE; + std::vector split_points; + std::vector prev_unbroken_timestamps; prev_unbroken_timestamps.reserve(candidates_list.size()); prev_unbroken_timestamps.push_back(initial_timestamp); for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t) @@ -133,22 +133,23 @@ class MapMatching final : public BasicRoutingInterface::max()) + std::size_t split_index = t; + if (breakage_begin != osrm::matching::INVALID_STATE) { split_index = breakage_begin; - breakage_begin = std::numeric_limits::max(); + breakage_begin = osrm::matching::INVALID_STATE; } split_points.push_back(split_index); // note: this preserves everything before split_index model.clear(split_index); - unsigned new_start = model.initialize(split_index); + std::size_t new_start = model.initialize(split_index); // no new start was found -> stop viterbi calculation if (new_start == osrm::matching::INVALID_STATE) { break; } + prev_unbroken_timestamps.clear(); prev_unbroken_timestamps.push_back(new_start); // Important: We potentially go back here! @@ -158,7 +159,7 @@ class MapMatching final : public BasicRoutingInterface= sub_matching_begin && model.breakage[parent_timestamp_index]) { @@ -287,10 +288,10 @@ class MapMatching final : public BasicRoutingInterface> reconstructed_indices; + std::deque> reconstructed_indices; while (parent_timestamp_index > sub_matching_begin) { if (model.breakage[parent_timestamp_index]) From bc8666df8318dd5118bfbdda21ca63737bff961a Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Sun, 8 Mar 2015 01:32:13 +0100 Subject: [PATCH 67/67] Add tests for matching --- features/step_definitions/matching.rb | 20 +++++++--- features/support/match.rb | 18 ++++----- features/testbot/matching.feature | 55 +++++++++++++++++++++++++++ 3 files changed, 76 insertions(+), 17 deletions(-) create mode 100644 features/testbot/matching.feature diff --git a/features/step_definitions/matching.rb b/features/step_definitions/matching.rb index c5d25f3d8..3f3181f64 100644 --- a/features/step_definitions/matching.rb +++ b/features/step_definitions/matching.rb @@ -11,15 +11,18 @@ When /^I match I should get$/ do |table| trace = [] timestamps = [] if row['trace'] - row['trace'].split(',').each do |n| + row['trace'].each_char do |n| node = find_node_by_name(n.strip) raise "*** unknown waypoint node '#{n.strip}" unless node trace << node end + if row['timestamps'] + timestamps = row['timestamps'].split(" ").compact.map { |t| t.to_i} + end got = {'trace' => row['trace'] } response = request_matching trace, timestamps, params else - raise "*** no waypoints" + raise "*** no trace" end end @@ -59,6 +62,10 @@ When /^I match I should get$/ do |table| encoded_result = "" extended_target = "" row['matchings'].split(',').each_with_index do |sub, sub_idx| + if sub_idx >= sub_matchings.length + ok = false + break + end sub.length.times do |node_idx| node = find_node_by_name(sub[node_idx]) out_node = sub_matchings[sub_idx][node_idx] @@ -66,17 +73,18 @@ When /^I match I should get$/ do |table| encoded_result += sub[node_idx] extended_target += sub[node_idx] else - encoded_result += "[#{out_node[0]},#{out_node[1]}]" + encoded_result += "? [#{out_node[0]},#{out_node[1]}]" extended_target += "#{sub[node_idx]} [#{node.lat},#{node.lon}]" ok = false end end end if ok - got['matchings'] = row['matchings'] + got['matchings'] = row['matchings'] + got['timestamps'] = row['timestamps'] else - got['matchings'] = encoded_result - row['matchings'] = extended_target + got['matchings'] = encoded_result + row['matchings'] = extended_target log_fail row,got, { 'matching' => {:query => @query, :response => response} } end diff --git a/features/support/match.rb b/features/support/match.rb index 9a18a3a15..bf51189a4 100644 --- a/features/support/match.rb +++ b/features/support/match.rb @@ -1,21 +1,17 @@ require 'net/http' HOST = "http://127.0.0.1:#{OSRM_PORT}" -DESTINATION_REACHED = 15 #OSRM instruction code - -class Hash - def to_param(namespace = nil) - collect do |key, value| - "#{key}=#{value}" - end.sort - end -end def request_matching trace=[], timestamps=[], options={} defaults = { 'output' => 'json' } - locs = waypoints.compact.map { |w| "loc=#{w.lat},#{w.lon}" } + locs = trace.compact.map { |w| "loc=#{w.lat},#{w.lon}" } ts = timestamps.compact.map { |t| "t=#{t}" } - params = (locs + ts + defaults.merge(options).to_param).join('&') + if ts.length > 0 + trace_params = locs.zip(ts).map { |a| a.join('&')} + else + trace_params = locs + end + params = (trace_params + defaults.merge(options).to_param).join('&') params = nil if params=="" uri = URI.parse ["#{HOST}/match", params].compact.join('?') @query = uri.to_s diff --git a/features/testbot/matching.feature b/features/testbot/matching.feature new file mode 100644 index 000000000..47630e9b6 --- /dev/null +++ b/features/testbot/matching.feature @@ -0,0 +1,55 @@ +@match @testbot +Feature: Basic Map Matching + + Background: + Given the profile "testbot" + Given a grid size of 10 meters + + Scenario: Testbot - Map matching with trace splitting + Given the node map + | a | b | c | d | + | | | e | | + + And the ways + | nodes | oneway | + | abcd | no | + + When I match I should get + | trace | timestamps | matchings | + | abcd | 0 1 42 43 | ab,cd | + + Scenario: Testbot - Map matching with small distortion + Given the node map + | a | b | c | d | e | + | | f | | | | + | | | | | | + | | | | | | + | | | | | | + | | h | | | k | + + # The second way does not need to be a oneway + # but the grid spacing triggers the uturn + # detection on f + And the ways + | nodes | oneway | + | abcde | no | + | bfhke | yes | + + When I match I should get + | trace | matchings | + | afcde | abcde | + + Scenario: Testbot - Map matching with oneways + Given the node map + | a | b | c | d | + | e | f | g | h | + + And the ways + | nodes | oneway | + | abcd | yes | + | hgfe | yes | + + When I match I should get + | trace | matchings | + | dcba | hgfe | +