From 2b38c936d5510aea11ef3ee37c4f8c5ec2b5f4ce Mon Sep 17 00:00:00 2001 From: Siarhei Fedartsou Date: Sun, 28 Jul 2024 13:31:33 +0200 Subject: [PATCH] somehow works --- .../routing_algorithms/routing_base_ch.hpp | 18 +- .../routing_algorithms/routing_base_mld.hpp | 306 ++++++++++++++++-- .../routing_algorithms/map_matching.cpp | 31 +- 3 files changed, 311 insertions(+), 44 deletions(-) diff --git a/include/engine/routing_algorithms/routing_base_ch.hpp b/include/engine/routing_algorithms/routing_base_ch.hpp index e39702611..e268b302e 100644 --- a/include/engine/routing_algorithms/routing_base_ch.hpp +++ b/include/engine/routing_algorithms/routing_base_ch.hpp @@ -460,16 +460,18 @@ void search(SearchEngineData &engine_working_data, duration_upper_bound); } -inline std::vector getNetworkDistances(SearchEngineData &, - const DataFacade &, - SearchEngineData::QueryHeap &, - SearchEngineData::QueryHeap &, - const PhantomNode &, - const std::vector &, - EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT) { +inline std::vector +getNetworkDistances(SearchEngineData &, + const DataFacade &, + SearchEngineData::QueryHeap &, + SearchEngineData::QueryHeap &, + const PhantomNode &, + const std::vector &, + EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT) +{ std::vector distances; return distances; - } +} // Requires the heaps for be empty // If heaps should be adjusted to be initialized outside of this function, diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index 8b1e3fdf9..ae8ffa86e 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -705,8 +705,6 @@ void unpackPath(const FacadeT &facade, annotatePath(facade, route_endpoints, unpacked_nodes, unpacked_edges, unpacked_path); } - - template double getNetworkDistance(SearchEngineData &engine_working_data, const DataFacade &facade, @@ -765,14 +763,228 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return from_alias(distance); } +template +auto routingStep2(const DataFacade &facade, Heap &forward_heap, const Args &...args) +{ + const auto heapNode = forward_heap.DeleteMinGetHeapNode(); + // const auto weight = heapNode.weight; + + BOOST_ASSERT(!facade.ExcludeNode(heapNode.node)); + + // // Upper bound for the path source -> target with + // // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight + // // is weight + reverse_weight + // // More tighter upper bound requires additional condition reverse_heap.WasRemoved(to) + // // with weight(to -> target) = reverse_weight and all weights ≥ 0 + // const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node); + // if (reverseHeapNode) + // { + // auto reverse_weight = reverseHeapNode->weight; + // auto path_weight = weight + reverse_weight; + + // if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) && + // (path_weight >= EdgeWeight{0}) && (path_weight < path_upper_bound)) + // { + // middle_node = heapNode.node; + // path_upper_bound = path_weight; + // } + // } + + // Relax outgoing edges from node + relaxOutgoingEdges(facade, forward_heap, heapNode, args...); + + return heapNode; +} + +template +std::vector>> +runSearch2(const DataFacade &facade, + Heap &forward_heap, + const std::vector> &reverse_heap, + const std::vector &force_step_nodes, + EdgeWeight weight_upper_bound, + const Args &...args) +{ + // if (forward_heap.Empty() || reverse_heap.Empty()) + // { + // return {}; + // } + + // BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT); + // BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT); + + std::vector middles; + std::vector weights; + + middles.resize(reverse_heap.size(), SPECIAL_NODEID); + weights.resize(reverse_heap.size(), 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) + { + reverse_heap_mins.push_back(heap->MinKey()); + } + + auto shouldContinue = [&]() + { + bool cont = false; + for (size_t i = 0; i < reverse_heap.size(); ++i) + { + if (reverse_heap_mins[i] < weights[i]) + { + cont = true; + break; + } + } + return cont; + }; + + while ((forward_heap.Size() + std::accumulate(reverse_heap.begin(), + reverse_heap.end(), + 0, + [](auto sum, const auto &heap) + { return sum + heap->Size(); }) > + 0) && + shouldContinue()) + { + if (!forward_heap.Empty()) + { + auto heapNode = routingStep2(facade, forward_heap, args...); + + for (size_t i = 0; i < reverse_heap.size(); ++i) + { + auto &rh = reverse_heap[i]; + const auto reverseHeapNode = rh->GetHeapNodeIfWasInserted(heapNode.node); + if (reverseHeapNode) + { + auto reverse_weight = reverseHeapNode->weight; + auto path_weight = heapNode.weight + reverse_weight; + + if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) && + (path_weight >= EdgeWeight{0}) && (path_weight < weights[i])) + { + middles[i] = heapNode.node; + weights[i] = path_weight; + } + } + } + + if (!forward_heap.Empty()) + forward_heap_min = forward_heap.MinKey(); + } + for (size_t i = 0; i < reverse_heap.size(); ++i) + { + if (!reverse_heap[i]->Empty()) + { + auto heapNode = routingStep2(facade, *reverse_heap[i], args...); + const auto reverseHeapNode = forward_heap.GetHeapNodeIfWasInserted(heapNode.node); + if (reverseHeapNode) + { + auto reverse_weight = reverseHeapNode->weight; + auto path_weight = heapNode.weight + reverse_weight; + + if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) && + (path_weight >= EdgeWeight{0}) && (path_weight < weights[i])) + { + middles[i] = heapNode.node; + weights[i] = path_weight; + } + } + if (!reverse_heap[i]->Empty()) + reverse_heap_mins[i] = reverse_heap[i]->MinKey(); + } + } + }; + + std::vector>> results; + for (size_t i = 0; i < reverse_heap.size(); ++i) + { + if (weights[i] >= weight_upper_bound || SPECIAL_NODEID == middles[i]) + { + results.push_back({}); + } + else + { + results.push_back({{middles[i], weights[i]}}); + } + } + return results; + + // // run two-Target Dijkstra routing step. + // NodeID middle = SPECIAL_NODEID; + // EdgeWeight weight = weight_upper_bound; + // EdgeWeight forward_heap_min = forward_heap.MinKey(); + // EdgeWeight reverse_heap_min = reverse_heap.MinKey(); + // while (forward_heap.Size() + reverse_heap.Size() > 0 && + // forward_heap_min + reverse_heap_min < weight) + // { + // if (!forward_heap.Empty()) + // { + // routingStep( + // facade, forward_heap, reverse_heap, middle, weight, force_step_nodes, args...); + // if (!forward_heap.Empty()) + // forward_heap_min = forward_heap.MinKey(); + // } + // if (!reverse_heap.Empty()) + // { + // routingStep( + // facade, reverse_heap, forward_heap, middle, weight, force_step_nodes, args...); + // if (!reverse_heap.Empty()) + // reverse_heap_min = reverse_heap.MinKey(); + // } + // }; + + // // No path found for both target nodes? + // if (weight >= weight_upper_bound || SPECIAL_NODEID == middle) + // { + // return {}; + // } + + // return {{middle, weight}}; +} + +template +std::vector searchDistance2( + SearchEngineData &, + const DataFacade &facade, + typename SearchEngineData::MapMatchingQueryHeap &forward_heap, + const std::vector::MapMatchingQueryHeap>> + &reverse_heaps, + const std::vector &force_step_nodes, + EdgeWeight weight_upper_bound, + const Args &...args) +{ + auto searchResults = runSearch2( + facade, forward_heap, reverse_heaps, force_step_nodes, weight_upper_bound, args...); + std::vector res; + for (size_t i = 0; i < searchResults.size(); ++i) + { + if (!searchResults[i]) + { + res.push_back(INVALID_EDGE_DISTANCE); + } + else + { + auto [middle, _] = *searchResults[i]; + auto distance = + forward_heap.GetData(middle).distance + reverse_heaps[i]->GetData(middle).distance; + res.push_back(distance); + } + } + return res; +} template -std::vector getNetworkDistances(SearchEngineData &engine_working_data, - const DataFacade &facade, - typename SearchEngineData::MapMatchingQueryHeap &forward_heap, - typename SearchEngineData::MapMatchingQueryHeap &reverse_heap, - const PhantomNode &source_phantom, - const std::vector &target_phantoms, - EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT) { +std::vector +getNetworkDistances(SearchEngineData &engine_working_data, + const DataFacade &facade, + typename SearchEngineData::MapMatchingQueryHeap &forward_heap, + typename SearchEngineData::MapMatchingQueryHeap &, + 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; @@ -802,9 +1014,10 @@ std::vector getNetworkDistances(SearchEngineData &engine_work EdgeDistance{0} - source_phantom.GetReverseDistance()}); } - for (size_t i = 0; i < target_phantoms.size(); ++i) { - auto& reverse_heap = *reverse_heaps[i]; - const auto& target_phantom = target_phantoms[i]; + for (size_t i = 0; i < target_phantoms.size(); ++i) + { + auto &reverse_heap = *reverse_heaps[i]; + const auto &target_phantom = target_phantoms[i]; if (target_phantom.IsValidForwardTarget()) { reverse_heap.Insert( @@ -821,22 +1034,69 @@ std::vector getNetworkDistances(SearchEngineData &engine_work {target_phantom.reverse_segment_id.id, false, target_phantom.GetReverseDistance()}); } } - - - std::vector distances; + // PhantomEndpoints endpoints{}; + // endpoints.push_back(source_phantom); + // for (const auto &target_phantom : target_phantoms) + // { + // endpoints.push_back(target_phantom); + // } + PhantomNodeCandidates phantom_candidates; + phantom_candidates.push_back(source_phantom); for (const auto &target_phantom : target_phantoms) { - auto distance = getNetworkDistance(engine_working_data, - facade, - forward_heap, - reverse_heap, - source_phantom, - target_phantom, - duration_upper_bound); - distances.push_back(distance); + phantom_candidates.push_back(target_phantom); + } + + auto distances2 = searchDistance2(engine_working_data, + facade, + forward_heap, + reverse_heaps, + {}, + weight_upper_bound, + phantom_candidates); + std::vector distances; + for (auto d : distances2) + { + if (d == INVALID_EDGE_DISTANCE) + { + distances.push_back(std::numeric_limits::max()); + } + else + { + distances.push_back(from_alias(d)); + } } return distances; + + // for (const auto &target_phantom : target_phantoms) + // { + // // forward_heap.Clear(); + // // auto& reverse_heap = *reverse_heaps[0]; + // // reverse_heap.Clear(); + // // const PhantomEndpoints endpoints{source_phantom, target_phantom}; + + // auto distance = searchDistance2( + // engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound, + // endpoints); + + // // if (distance == INVALID_EDGE_DISTANCE) + // // { + // // distances.push_back(std::numeric_limits::max()); + // // } else { + // // distances.push_back(from_alias(distance)); + // // } + // // return from_alias(distance); + // auto distance = getNetworkDistance(engine_working_data, + // facade, + // forward_heap, + // *reverse_heaps[0], + // source_phantom, + // target_phantom, + // weight_upper_bound); + // distances.push_back(distance); + // } + // return distances; } } // namespace osrm::engine::routing_algorithms::mld diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index be11d8bc3..263d8d510 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -237,19 +237,20 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node); } - auto new_distances = getNetworkDistances(engine_working_data, - facade, - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - target_phantom_nodes, - weight_upper_bound); + auto new_distances = + getNetworkDistances(engine_working_data, + facade, + forward_heap, + reverse_heap, + prev_unbroken_timestamps_list[s].phantom_node, + target_phantom_nodes, + weight_upper_bound); std::vector old_distances; - for (const auto& pn: target_phantom_nodes) + for (const auto &pn : target_phantom_nodes) { - double network_distance = + double network_distance = getNetworkDistance(engine_working_data, facade, forward_heap, @@ -257,14 +258,18 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, prev_unbroken_timestamps_list[s].phantom_node, pn, weight_upper_bound); - old_distances.push_back(network_distance); + old_distances.push_back(network_distance); } - if (old_distances != new_distances) { - std::cerr << "OOPS " << std::endl; + 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; + } } - for (const auto s_prime : util::irange(0UL, current_viterbi.size())) { const double emission_pr = emission_log_probabilities[t][s_prime];