From 3a5e41ed91beecd7fbfda0dfe1c37de338b709e8 Mon Sep 17 00:00:00 2001 From: Patrick Niklaus Date: Mon, 8 Dec 2014 14:46:31 -0800 Subject: [PATCH] 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; }