diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp index e268b302e..004d7e376 100644 --- a/include/engine/routing_algorithms/routing_base_ch.hpp +++ b/include/engine/routing_algorithms/routing_base_ch.hpp @@ -464,7 +464,7 @@ inline std::vector getNetworkDistances(SearchEngineData &, const DataFacade &, SearchEngineData::QueryHeap &, - SearchEngineData::QueryHeap &, + const std::vector::QueryHeap>> &, const PhantomNode &, const std::vector &, EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT) diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index ae8ffa86e..334e3b830 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -38,10 +38,13 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, return INVALID_LEVEL_ID; }; - return std::min(std::min(level(source.forward_segment_id, target.forward_segment_id), + auto res = std::min(std::min(level(source.forward_segment_id, target.forward_segment_id), level(source.forward_segment_id, target.reverse_segment_id)), std::min(level(source.reverse_segment_id, target.forward_segment_id), level(source.reverse_segment_id, target.reverse_segment_id))); + + // std::cerr << "OLD!!! " << (int)res << std::endl; + return res; } template @@ -92,6 +95,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, getNodeQueryLevel(partition, node, source, target)); })); }); + // std::cerr << "NEW " << (int)min_level << std::endl; return min_level; } @@ -140,7 +144,10 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, highest_different_level(phantom_node.reverse_segment_id)); return std::min(current_level, highest_level); }); - return node_level; + + // std::cerr << "NEW!!! " << (int)node_level << std::endl; + return node_level; + } // Unrestricted search with a single phantom node and a vector of phantom nodes: @@ -300,7 +307,6 @@ void relaxOutgoingEdges(const DataFacade &facade, const auto &metric = facade.GetCellMetric(); const auto level = getNodeQueryLevel(partition, heapNode.node, args...); - static constexpr auto IS_MAP_MATCHING = std::is_same_v::MapMatchingQueryHeap, Heap>; @@ -457,6 +463,12 @@ void routingStep(const DataFacade &facade, BOOST_ASSERT(!facade.ExcludeNode(heapNode.node)); + if (DIRECTION == FORWARD_DIRECTION) { + // std::cerr << "FORWARDO " << heapNode.node << std::endl; + } else { + //std::cerr << "REVERSEO " << heapNode.node << std::endl; + } + // Upper bound for the path source -> target with // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight // is weight + reverse_weight @@ -644,6 +656,7 @@ searchDistance(SearchEngineData &, auto [middle, _] = *searchResult; + // std::cerr << "old " << middle << std::endl; auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance; return distance; @@ -796,14 +809,15 @@ auto routingStep2(const DataFacade &facade, Heap &forward_heap, const return heapNode; } -template +template std::vector>> runSearch2(const DataFacade &facade, Heap &forward_heap, const std::vector> &reverse_heap, + size_t candidatesCount, const std::vector &force_step_nodes, EdgeWeight weight_upper_bound, - const Args &...args) + const PhantomEndpointCandidates& candidates) { // if (forward_heap.Empty() || reverse_heap.Empty()) // { @@ -816,23 +830,23 @@ runSearch2(const DataFacade &facade, std::vector middles; std::vector weights; - middles.resize(reverse_heap.size(), SPECIAL_NODEID); - weights.resize(reverse_heap.size(), weight_upper_bound); + middles.resize(candidatesCount, SPECIAL_NODEID); + weights.resize(candidatesCount, weight_upper_bound); // run two-Target Dijkstra routing step. EdgeWeight forward_heap_min = forward_heap.MinKey(); std::vector reverse_heap_mins; - for (const auto &heap : reverse_heap) + for (size_t i = 0; i < candidatesCount; ++i) { - reverse_heap_mins.push_back(heap->MinKey()); + reverse_heap_mins.push_back(reverse_heap[i]->MinKey()); } auto shouldContinue = [&]() { bool cont = false; - for (size_t i = 0; i < reverse_heap.size(); ++i) + for (size_t i = 0; i < candidatesCount; ++i) { - if (reverse_heap_mins[i] < weights[i]) + if ((forward_heap_min + reverse_heap_mins[i]) < weights[i]) { cont = true; break; @@ -851,9 +865,11 @@ runSearch2(const DataFacade &facade, { if (!forward_heap.Empty()) { - auto heapNode = routingStep2(facade, forward_heap, args...); + const auto heapNode = forward_heap.DeleteMinGetHeapNode(); + // std::cerr << "FORWARDN " << heapNode.node << std::endl; + //auto heapNode = routingStep2(facade, forward_heap, args...); - for (size_t i = 0; i < reverse_heap.size(); ++i) + for (size_t i = 0; i < candidatesCount; ++i) { auto &rh = reverse_heap[i]; const auto reverseHeapNode = rh->GetHeapNodeIfWasInserted(heapNode.node); @@ -867,18 +883,27 @@ runSearch2(const DataFacade &facade, { middles[i] = heapNode.node; weights[i] = path_weight; + + // auto distance = + // forward_heap.GetData(middles[i]).distance + reverse_heap[i]->GetData(middles[i]).distance; + // std::cerr << "RFOUNDN " << i <<" " << distance << std::endl; + } } } + relaxOutgoingEdges(facade, forward_heap, heapNode, candidates); + if (!forward_heap.Empty()) forward_heap_min = forward_heap.MinKey(); } - for (size_t i = 0; i < reverse_heap.size(); ++i) + for (size_t i = 0; i < candidatesCount; ++i) { - if (!reverse_heap[i]->Empty()) + if (!reverse_heap[i]->Empty() && (forward_heap_min + reverse_heap_mins[i]) < weights[i]) { - auto heapNode = routingStep2(facade, *reverse_heap[i], args...); + const auto heapNode = reverse_heap[i]->DeleteMinGetHeapNode(); + //std::cerr << "REVERSEN " << i << " " << heapNode.node << std::endl; + const auto reverseHeapNode = forward_heap.GetHeapNodeIfWasInserted(heapNode.node); if (reverseHeapNode) { @@ -888,10 +913,18 @@ runSearch2(const DataFacade &facade, if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) && (path_weight >= EdgeWeight{0}) && (path_weight < weights[i])) { + middles[i] = heapNode.node; weights[i] = path_weight; + + // auto distance = + // forward_heap.GetData(middles[i]).distance + reverse_heap[i]->GetData(middles[i]).distance; + // std::cerr << "FFOUNDN " << i << " " << distance << std::endl; + } } + relaxOutgoingEdges(facade, *reverse_heap[i], heapNode, candidates); + if (!reverse_heap[i]->Empty()) reverse_heap_mins[i] = reverse_heap[i]->MinKey(); } @@ -899,7 +932,7 @@ runSearch2(const DataFacade &facade, }; std::vector>> results; - for (size_t i = 0; i < reverse_heap.size(); ++i) + for (size_t i = 0; i < candidatesCount; ++i) { if (weights[i] >= weight_upper_bound || SPECIAL_NODEID == middles[i]) { @@ -945,19 +978,20 @@ runSearch2(const DataFacade &facade, // return {{middle, weight}}; } -template +template std::vector searchDistance2( SearchEngineData &, const DataFacade &facade, typename SearchEngineData::MapMatchingQueryHeap &forward_heap, const std::vector::MapMatchingQueryHeap>> &reverse_heaps, + size_t candidatesCount, const std::vector &force_step_nodes, EdgeWeight weight_upper_bound, - const Args &...args) + const PhantomEndpointCandidates& candidates) { auto searchResults = runSearch2( - facade, forward_heap, reverse_heaps, force_step_nodes, weight_upper_bound, args...); + facade, forward_heap, reverse_heaps, candidatesCount, force_step_nodes, weight_upper_bound, candidates); std::vector res; for (size_t i = 0; i < searchResults.size(); ++i) { @@ -968,6 +1002,9 @@ std::vector searchDistance2( else { auto [middle, _] = *searchResults[i]; + + //std::cerr << "new " << i << " " << middle << std::endl; + auto distance = forward_heap.GetData(middle).distance + reverse_heaps[i]->GetData(middle).distance; res.push_back(distance); @@ -980,21 +1017,24 @@ std::vector getNetworkDistances(SearchEngineData &engine_working_data, const DataFacade &facade, typename SearchEngineData::MapMatchingQueryHeap &forward_heap, - typename SearchEngineData::MapMatchingQueryHeap &, + const std::vector::MapMatchingQueryHeap>> &reverse_heaps, const PhantomNode &source_phantom, const std::vector &target_phantoms, EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT) { - using Heap = typename SearchEngineData::MapMatchingQueryHeap; forward_heap.Clear(); - std::vector> reverse_heaps; - const auto nodes_number = facade.GetNumberOfNodes(); - const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1; - for (const auto &target_phantom : target_phantoms) + for (const auto &heap : reverse_heaps) { - (void)target_phantom; - reverse_heaps.emplace_back(std::make_unique(nodes_number, border_nodes_number)); + heap->Clear(); } + // std::vector> reverse_heaps; + // const auto nodes_number = facade.GetNumberOfNodes(); + // const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1; + // for (const auto &target_phantom : target_phantoms) + // { + // (void)target_phantom; + // reverse_heaps.emplace_back(std::make_unique(nodes_number, border_nodes_number)); + // } if (source_phantom.IsValidForwardSource()) { @@ -1041,17 +1081,16 @@ getNetworkDistances(SearchEngineData &engine_working_data, // { // endpoints.push_back(target_phantom); // } - PhantomNodeCandidates phantom_candidates; - phantom_candidates.push_back(source_phantom); - for (const auto &target_phantom : target_phantoms) - { - phantom_candidates.push_back(target_phantom); - } + std::vector source_phantomes; + source_phantomes.push_back(source_phantom); + PhantomEndpointCandidates phantom_candidates{source_phantomes, target_phantoms}; + auto distances2 = searchDistance2(engine_working_data, facade, forward_heap, reverse_heaps, + target_phantoms.size(), {}, weight_upper_bound, phantom_candidates); diff --git a/include/engine/search_engine_data.hpp b/include/engine/search_engine_data.hpp index 30d8e7c19..94126f944 100644 --- a/include/engine/search_engine_data.hpp +++ b/include/engine/search_engine_data.hpp @@ -56,6 +56,7 @@ template <> struct SearchEngineData static thread_local ManyToManyHeapPtr many_to_many_heap; static thread_local SearchEngineHeapPtr map_matching_forward_heap_1; static thread_local SearchEngineHeapPtr map_matching_reverse_heap_1; + static thread_local std::vector map_matching_reverse_heaps; void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes); @@ -133,13 +134,15 @@ template <> struct SearchEngineData static thread_local SearchEngineHeapPtr reverse_heap_1; static thread_local MapMatchingHeapPtr map_matching_forward_heap_1; static thread_local MapMatchingHeapPtr map_matching_reverse_heap_1; + static thread_local std::vector map_matching_reverse_heaps; static thread_local ManyToManyHeapPtr many_to_many_heap; void InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes, unsigned number_of_boundary_nodes); void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes, - unsigned number_of_boundary_nodes); + unsigned number_of_boundary_nodes, + size_t max_candidates); void InitializeOrClearManyToManyThreadLocalStorage(unsigned number_of_nodes, unsigned number_of_boundary_nodes); diff --git a/src/benchmarks/bench.cpp b/src/benchmarks/bench.cpp index 86b263a6b..b0621e29e 100644 --- a/src/benchmarks/bench.cpp +++ b/src/benchmarks/bench.cpp @@ -395,9 +395,9 @@ void runMatchBenchmark(const OSRM &osrm, const GPSTraces &gpsTraces, int iterati std::optional radius = std::nullopt; }; - std::vector benchmarks = {{"500 matches, default radius"}, - {"500 matches, radius=10", 10}, - {"500 matches, radius=20", 20}}; + std::vector benchmarks = {{"500 matches, default radius", 10}, + /*{"500 matches, radius=10", 10}, + {"500 matches, radius=20", 20}*/}; runBenchmarks(benchmarks, iterations, diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index 263d8d510..4de3a1794 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -45,7 +45,8 @@ unsigned getMedianSampleTime(const std::vector ×tamps) template inline void initializeHeap(SearchEngineData &engine_working_data, - const DataFacade &facade) + const DataFacade &facade, + size_t) { const auto nodes_number = facade.GetNumberOfNodes(); @@ -54,14 +55,86 @@ inline void initializeHeap(SearchEngineData &engine_working_data, template <> inline void initializeHeap(SearchEngineData &engine_working_data, - const DataFacade &facade) + const DataFacade &facade, + size_t max_candidates) { const auto nodes_number = facade.GetNumberOfNodes(); const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1; engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number, - border_nodes_number); + border_nodes_number, + max_candidates); } + +#include +#include + +template +void saveVectorToFile(const std::vector& data, const std::string& filename) { + std::ofstream outFile(filename, std::ios::binary); + if (!outFile) { + std::cerr << "Error opening file for writing: " << filename << std::endl; + return; + } + size_t size = data.size(); + outFile.write(reinterpret_cast(&size), sizeof(size)); + outFile.write(reinterpret_cast(data.data()), size * sizeof(T)); + outFile.close(); + if (!outFile.good()) { + std::cerr << "Error occurred at writing time!" << std::endl; + } +} + +template +bool loadVectorFromFile(std::vector& data, const std::string& filename) { + std::ifstream inFile(filename, std::ios::binary); + if (!inFile) { + std::cerr << "Error opening file for reading: " << filename << std::endl; + return false; + } + size_t size; + inFile.read(reinterpret_cast(&size), sizeof(size)); + data.resize(size); + inFile.read(reinterpret_cast(data.data()), size * sizeof(T)); + inFile.close(); + if (!inFile.good()) { + std::cerr << "Error occurred at reading time!" << std::endl; + return false; + } + return true; +} + +template +void saveStructToFile(const T& data, const std::string& filename) { + std::ofstream outFile(filename, std::ios::binary); + if (!outFile) { + std::cerr << "Error opening file for writing: " << filename << std::endl; + return; + } + outFile.write(reinterpret_cast(&data), sizeof(T)); + outFile.close(); + if (!outFile.good()) { + std::cerr << "Error occurred at writing time!" << std::endl; + } +} + + +template +bool loadStructFromFile(T& data, const std::string& filename) { + std::ifstream inFile(filename, std::ios::binary); + if (!inFile) { + std::cerr << "Error opening file for reading: " << filename << std::endl; + return false; + } + inFile.read(reinterpret_cast(&data), sizeof(T)); + inFile.close(); + if (!inFile.good()) { + std::cerr << "Error occurred at reading time!" << std::endl; + return false; + } + return true; +} + } // namespace template @@ -144,9 +217,16 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, return sub_matchings; } - initializeHeap(engine_working_data, facade); + size_t max_candidates = 0; + for (const auto &candidates : candidates_list) + { + max_candidates = std::max(max_candidates, candidates.size()); + } + + initializeHeap(engine_working_data, facade, max_candidates); auto &forward_heap = *engine_working_data.map_matching_forward_heap_1; auto &reverse_heap = *engine_working_data.map_matching_reverse_heap_1; + const auto &reverse_heaps = engine_working_data.map_matching_reverse_heaps; std::size_t breakage_begin = map_matching::INVALID_STATE; std::vector split_points; @@ -224,8 +304,20 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, { continue; } - - std::vector target_phantom_nodes; + + // PhantomNode source; + // loadStructFromFile(source, "source.bin"); + std::vector target_phantom_nodes; + // loadVectorFromFile(target_phantom_nodes, "target.bin"); + // target_phantom_nodes.erase(target_phantom_nodes.begin()); + // target_phantom_nodes.erase(target_phantom_nodes.begin()); + // target_phantom_nodes.erase(target_phantom_nodes.begin()); + // target_phantom_nodes.erase(target_phantom_nodes.begin()); + // target_phantom_nodes.pop_back(); + // target_phantom_nodes.pop_back(); + // target_phantom_nodes.erase(target_phantom_nodes.begin() + 1); + +// target_phantom_nodes.push_back(target); for (const auto s_prime : util::irange(0UL, current_viterbi.size())) { const double emission_pr = emission_log_probabilities[t][s_prime]; @@ -237,16 +329,22 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node); } - auto new_distances = + // TIMER_START(NEW_DIST); + #if 1 + (void)reverse_heap; + auto distances = getNetworkDistances(engine_working_data, facade, forward_heap, - reverse_heap, + reverse_heaps, prev_unbroken_timestamps_list[s].phantom_node, target_phantom_nodes, weight_upper_bound); - - std::vector old_distances; + // TIMER_STOP(NEW_DIST); +#else + // TIMER_START(OLD_DIST); + (void)reverse_heaps; + std::vector distances; for (const auto &pn : target_phantom_nodes) { @@ -258,18 +356,26 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, prev_unbroken_timestamps_list[s].phantom_node, pn, weight_upper_bound); - old_distances.push_back(network_distance); + distances.push_back(network_distance); } +#endif + // TIMER_STOP(OLD_DIST); - for (size_t i = 0; i < old_distances.size(); ++i) - { - if (std::abs(old_distances[i] - new_distances[i]) > 0.01) - { - std::cerr << "OOPS " << old_distances[i] << " " << new_distances[i] - << std::endl; - } - } + // std::cerr << "Old: " << TIMER_MSEC(OLD_DIST) << " New: " << TIMER_MSEC(NEW_DIST) + // << std::endl; + // for (size_t i = 0; i < old_distances.size(); ++i) + // { + // if (std::abs(old_distances[i] - new_distances[i]) > 0.01) + // { + // // saveStructToFile(prev_unbroken_timestamps_list[s].phantom_node, "source.bin"); + // // saveVectorToFile(target_phantom_nodes, "target.bin"); + // // std::cerr << "OOPS " << old_distances[i] << " " << new_distances[i] + // // << std::endl; + // // std::exit(1); + // } + // } + size_t distance_index = 0; for (const auto s_prime : util::irange(0UL, current_viterbi.size())) { const double emission_pr = emission_log_probabilities[t][s_prime]; @@ -279,14 +385,16 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, continue; } - double network_distance = - getNetworkDistance(engine_working_data, - facade, - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - weight_upper_bound); + double network_distance = distances[distance_index]; + ++distance_index; + // double network_distance = + // getNetworkDistance(engine_working_data, + // facade, + // forward_heap, + // reverse_heap, + // prev_unbroken_timestamps_list[s].phantom_node, + // current_timestamps_list[s_prime].phantom_node, + // weight_upper_bound); // get distance diff between loc1/2 and locs/s_prime const auto d_t = std::abs(network_distance - haversine_distance); diff --git a/src/engine/search_engine_data.cpp b/src/engine/search_engine_data.cpp index e749db6c4..f96ef448f 100644 --- a/src/engine/search_engine_data.cpp +++ b/src/engine/search_engine_data.cpp @@ -15,6 +15,7 @@ thread_local SearchEngineData::SearchEngineHeapPtr SearchEngineData::map_matching_forward_heap_1; thread_local SearchEngineData::SearchEngineHeapPtr SearchEngineData::map_matching_reverse_heap_1; +thread_local std::vector::SearchEngineHeapPtr> SearchEngineData::map_matching_reverse_heaps; thread_local SearchEngineData::ManyToManyHeapPtr SearchEngineData::many_to_many_heap; @@ -123,9 +124,10 @@ thread_local SearchEngineData::MapMatchingHeapPtr thread_local SearchEngineData::MapMatchingHeapPtr SearchEngineData::map_matching_reverse_heap_1; thread_local SearchEngineData::ManyToManyHeapPtr SearchEngineData::many_to_many_heap; +thread_local std::vector::MapMatchingHeapPtr> SearchEngineData::map_matching_reverse_heaps; void SearchEngineData::InitializeOrClearMapMatchingThreadLocalStorage( - unsigned number_of_nodes, unsigned number_of_boundary_nodes) + unsigned number_of_nodes, unsigned number_of_boundary_nodes, size_t max_candidates) { if (map_matching_forward_heap_1.get()) { @@ -146,6 +148,15 @@ void SearchEngineData::InitializeOrClearMapMatchingThreadLocalStorage( map_matching_reverse_heap_1.reset( new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes)); } + + if (max_candidates > map_matching_reverse_heaps.size()) { + size_t to_add = max_candidates - map_matching_reverse_heaps.size(); + for (unsigned i = 0; i < to_add; ++i) + { + map_matching_reverse_heaps.emplace_back(new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes)); + } + } + } void SearchEngineData::InitializeOrClearFirstThreadLocalStorage(