POC of one-to-many bidirectional routing in map-matching

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -460,6 +460,19 @@ void search(SearchEngineData<Algorithm> &engine_working_data,
                   duration_upper_bound);
 }
 
+inline std::vector<double> getNetworkDistances(
+    SearchEngineData<Algorithm> &,
+    const DataFacade<ch::Algorithm> &,
+    SearchEngineData<Algorithm>::QueryHeap &,
+    const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::QueryHeap>> &,
+    const PhantomNode &,
+    const std::vector<PhantomNode> &,
+    EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT)
+{
+    std::vector<double> distances;
+    return distances;
+}
+
 // Requires the heaps for be empty
 // If heaps should be adjusted to be initialized outside of this function,
 // the addition of force_step parameters might be required

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)),
+                                 level(source.forward_segment_id, target.reverse_segment_id)),
-                    std::min(level(source.reverse_segment_id, target.forward_segment_id),
+                        std::min(level(source.reverse_segment_id, target.forward_segment_id),
-                             level(source.reverse_segment_id, target.reverse_segment_id)));
+                                 level(source.reverse_segment_id, target.reverse_segment_id)));
+
+    //   std::cerr << "OLD!!! " << (int)res << std::endl;
+    return res;
 }
 
 template <typename MultiLevelPartition>
@@ -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,6 +144,8 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
                                          highest_different_level(phantom_node.reverse_segment_id));
                             return std::min(current_level, highest_level);
                         });
+
+    //     std::cerr << "NEW!!! " << (int)node_level << std::endl;
     return node_level;
 }
 
@@ -300,7 +306,6 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
     const auto &metric = facade.GetCellMetric();
 
     const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
-
     static constexpr auto IS_MAP_MATCHING =
         std::is_same_v<typename SearchEngineData<mld::Algorithm>::MapMatchingQueryHeap, Heap>;
 
@@ -457,6 +462,15 @@ void routingStep(const DataFacade<Algorithm> &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 +658,7 @@ searchDistance(SearchEngineData<Algorithm> &,
 
     auto [middle, _] = *searchResult;
 
+    //   std::cerr << "old " << middle << std::endl;
     auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance;
 
     return distance;
@@ -763,6 +778,307 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
     return from_alias<double>(distance);
 }
 
+
+template <typename Algorithm, typename Heap>
+std::vector<NodeID>
+runSearch2(const DataFacade<Algorithm> &facade,
+           Heap &forward_heap,
+           const std::vector<std::unique_ptr<Heap>> &reverse_heap,
+           size_t candidatesCount,
+           const std::vector<NodeID> &force_step_nodes,
+           EdgeWeight weight_upper_bound,
+           const PhantomEndpointCandidates &candidates)
+{
+    // 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<NodeID> middles;
+    std::vector<EdgeWeight> weights;
+
+    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<EdgeWeight> reverse_heap_mins;
+    for (size_t i = 0; i < candidatesCount; ++i)
+    {
+        reverse_heap_mins.push_back(reverse_heap[i]->MinKey());
+    }
+
+    auto shouldContinue = [&]()
+    {
+        bool cont = false;
+        for (size_t i = 0; i < candidatesCount; ++i)
+        {
+            if ((forward_heap.Size() + reverse_heap[i]->Size() > 0) &&
+                (forward_heap_min + reverse_heap_mins[i]) < weights[i])
+            {
+                cont = true;
+                break;
+            }
+        }
+        return cont;
+    };
+
+    bool cont = shouldContinue();
+    while (cont)
+    {
+        if (!forward_heap.Empty())
+        {
+            const auto heapNode = forward_heap.DeleteMinGetHeapNode();
+            //  std::cerr << "FORWARDN " << heapNode.node << std::endl;
+            // auto heapNode = routingStep2<FORWARD_DIRECTION>(facade, forward_heap, args...);
+
+            for (size_t i = 0; i < candidatesCount; ++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;
+
+                        //                                             auto distance =
+                        // forward_heap.GetData(middles[i]).distance +
+                        // reverse_heap[i]->GetData(middles[i]).distance;
+                        //    std::cerr << "RFOUNDN " << i <<" " << distance << std::endl;
+                    }
+                }
+            }
+
+            relaxOutgoingEdges<FORWARD_DIRECTION>(facade, forward_heap, heapNode, candidates);
+
+            if (!forward_heap.Empty())
+                forward_heap_min = forward_heap.MinKey();
+        }
+
+        cont = false;
+        for (size_t i = 0; i < candidatesCount; ++i)
+        {
+             if ((forward_heap.Size() + reverse_heap[i]->Size() > 0) &&
+                (forward_heap_min + reverse_heap_mins[i]) < weights[i])
+            {
+                cont = true;
+            }
+            if (!reverse_heap[i]->Empty() && (forward_heap_min + reverse_heap_mins[i]) < weights[i])
+            {
+                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)
+                {
+                    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;
+
+                        //                     auto distance =
+                        // forward_heap.GetData(middles[i]).distance +
+                        // reverse_heap[i]->GetData(middles[i]).distance;
+                        //     std::cerr << "FFOUNDN " << i << " " << distance << std::endl;
+                    }
+                }
+                relaxOutgoingEdges<REVERSE_DIRECTION>(
+                    facade, *reverse_heap[i], heapNode, candidates);
+
+                if (!reverse_heap[i]->Empty())
+                    reverse_heap_mins[i] = reverse_heap[i]->MinKey();
+            }
+        }
+    };
+
+return middles;
+    // std::vector<std::optional<std::pair<NodeID, EdgeWeight>>> results;
+    // results.reserve(candidatesCount);
+    // for (size_t i = 0; i < candidatesCount; ++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<FORWARD_DIRECTION>(
+    //             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<REVERSE_DIRECTION>(
+    //             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 <typename Algorithm>
+std::vector<double> searchDistance2(
+    SearchEngineData<Algorithm> &,
+    const DataFacade<Algorithm> &facade,
+    typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+    const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::MapMatchingQueryHeap>>
+        &reverse_heaps,
+    size_t candidatesCount,
+    const std::vector<NodeID> &force_step_nodes,
+    EdgeWeight weight_upper_bound,
+    const PhantomEndpointCandidates &candidates)
+{
+    auto searchResults = runSearch2(facade,
+                                    forward_heap,
+                                    reverse_heaps,
+                                    candidatesCount,
+                                    force_step_nodes,
+                                    weight_upper_bound,
+                                    candidates);
+    std::vector<double> res;
+    res.reserve(candidatesCount);
+    for (size_t i = 0; i < searchResults.size(); ++i)
+    {
+        if (searchResults[i] == SPECIAL_NODEID)
+        {
+            res.push_back(std::numeric_limits<double>::max());
+        }
+        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(from_alias<double>(distance));
+        }
+    }
+    return res;
+}
+template <typename Algorithm>
+std::vector<double> getNetworkDistances(
+    SearchEngineData<Algorithm> &engine_working_data,
+    const DataFacade<Algorithm> &facade,
+    typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+    const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::MapMatchingQueryHeap>>
+        &reverse_heaps,
+    const PhantomNode &source_phantom,
+    const std::vector<PhantomNode> &target_phantoms,
+    EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
+{
+    forward_heap.Clear();
+    for (const auto &heap : reverse_heaps)
+    {
+        heap->Clear();
+    }
+    // std::vector<std::unique_ptr<Heap>> 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<Heap>(nodes_number, border_nodes_number));
+    // }
+
+    if (source_phantom.IsValidForwardSource())
+    {
+        forward_heap.Insert(source_phantom.forward_segment_id.id,
+                            EdgeWeight{0} - source_phantom.GetForwardWeightPlusOffset(),
+                            {source_phantom.forward_segment_id.id,
+                             false,
+                             EdgeDistance{0} - source_phantom.GetForwardDistance()});
+    }
+
+    if (source_phantom.IsValidReverseSource())
+    {
+        forward_heap.Insert(source_phantom.reverse_segment_id.id,
+                            EdgeWeight{0} - source_phantom.GetReverseWeightPlusOffset(),
+                            {source_phantom.reverse_segment_id.id,
+                             false,
+                             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];
+        if (target_phantom.IsValidForwardTarget())
+        {
+            reverse_heap.Insert(
+                target_phantom.forward_segment_id.id,
+                target_phantom.GetForwardWeightPlusOffset(),
+                {target_phantom.forward_segment_id.id, false, target_phantom.GetForwardDistance()});
+        }
+
+        if (target_phantom.IsValidReverseTarget())
+        {
+            reverse_heap.Insert(
+                target_phantom.reverse_segment_id.id,
+                target_phantom.GetReverseWeightPlusOffset(),
+                {target_phantom.reverse_segment_id.id, false, target_phantom.GetReverseDistance()});
+        }
+    }
+
+    // PhantomEndpoints endpoints{};
+    // endpoints.push_back(source_phantom);
+    // for (const auto &target_phantom : target_phantoms)
+    // {
+    //     endpoints.push_back(target_phantom);
+    // }
+    std::vector<PhantomNode> source_phantomes;
+    source_phantomes.push_back(source_phantom);
+    PhantomEndpointCandidates phantom_candidates{source_phantomes, target_phantoms};
+
+    auto distances = searchDistance2(engine_working_data,
+                                      facade,
+                                      forward_heap,
+                                      reverse_heaps,
+                                      target_phantoms.size(),
+                                      {},
+                                      weight_upper_bound,
+                                      phantom_candidates);
+    return distances;
+}
+
 } // namespace osrm::engine::routing_algorithms::mld
 
 #endif // OSRM_ENGINE_ROUTING_BASE_MLD_HPP

include/engine/search_engine_data.hpp

@@ -56,6 +56,7 @@ template <> struct SearchEngineData<routing_algorithms::ch::Algorithm>
     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<SearchEngineHeapPtr> map_matching_reverse_heaps;
 
     void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes);
 
@@ -133,13 +134,15 @@ template <> struct SearchEngineData<routing_algorithms::mld::Algorithm>
     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<MapMatchingHeapPtr> 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);

scripts/ci/run_benchmarks.sh

@@ -50,26 +50,27 @@ function measure_peak_ram_and_time {
 }
 
 function run_benchmarks_for_folder {
+    rm -rf $RESULTS_FOLDER
     mkdir -p $RESULTS_FOLDER
 
     BENCHMARKS_FOLDER="$BINARIES_FOLDER/src/benchmarks"
 
-    echo "Running match-bench MLD"
+    # echo "Running match-bench MLD"
-    $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/match_mld.bench"
+    # $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/match_mld.bench"
-    echo "Running match-bench CH"
+    # echo "Running match-bench CH"
-    $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/match_ch.bench"
+    # $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/match_ch.bench"
-    echo "Running route-bench MLD"
+    # echo "Running route-bench MLD"
-    $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/route_mld.bench"
+    # $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/route_mld.bench"
-    echo "Running route-bench CH"
+    # echo "Running route-bench CH"
-    $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/route_ch.bench"
+    # $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/route_ch.bench"
-    echo "Running alias"
+    # echo "Running alias"
-    $BENCHMARKS_FOLDER/alias-bench > "$RESULTS_FOLDER/alias.bench"
+    # $BENCHMARKS_FOLDER/alias-bench > "$RESULTS_FOLDER/alias.bench"
-    echo "Running json-render-bench"
+    # echo "Running json-render-bench"
-    $BENCHMARKS_FOLDER/json-render-bench  "$FOLDER/test/data/portugal_to_korea.json" > "$RESULTS_FOLDER/json-render.bench"
+    # $BENCHMARKS_FOLDER/json-render-bench  "$FOLDER/test/data/portugal_to_korea.json" > "$RESULTS_FOLDER/json-render.bench"
-    echo "Running packedvector-bench"
+    # echo "Running packedvector-bench"
-    $BENCHMARKS_FOLDER/packedvector-bench > "$RESULTS_FOLDER/packedvector.bench"
+    # $BENCHMARKS_FOLDER/packedvector-bench > "$RESULTS_FOLDER/packedvector.bench"
-    echo "Running rtree-bench"
+    # echo "Running rtree-bench"
-    $BENCHMARKS_FOLDER/rtree-bench "$FOLDER/test/data/monaco.osrm.ramIndex" "$FOLDER/test/data/monaco.osrm.fileIndex" "$FOLDER/test/data/monaco.osrm.nbg_nodes" > "$RESULTS_FOLDER/rtree.bench"
+    # $BENCHMARKS_FOLDER/rtree-bench "$FOLDER/test/data/monaco.osrm.ramIndex" "$FOLDER/test/data/monaco.osrm.fileIndex" "$FOLDER/test/data/monaco.osrm.nbg_nodes" > "$RESULTS_FOLDER/rtree.bench"
 
     cp -rf $OSM_PBF $FOLDER/data.osm.pbf
 
@@ -83,19 +84,19 @@ function run_benchmarks_for_folder {
     measure_peak_ram_and_time "$BINARIES_FOLDER/osrm-contract $FOLDER/data.osrm" "$RESULTS_FOLDER/osrm_contract.bench"
 
 
-    for ALGORITHM in ch mld; do
+    # for ALGORITHM in ch mld; do
-        for BENCH in nearest table trip route match; do
+    #     for BENCH in nearest table trip route match; do
-            echo "Running node $BENCH $ALGORITHM"
+    #         echo "Running node $BENCH $ALGORITHM"
-            START=$(date +%s.%N)
+    #         START=$(date +%s.%N)
-            node $SCRIPTS_FOLDER/scripts/ci/bench.js $FOLDER/lib/binding/node_osrm.node $FOLDER/data.osrm $ALGORITHM $BENCH $GPS_TRACES > "$RESULTS_FOLDER/node_${BENCH}_${ALGORITHM}.bench" 5
+    #         node $SCRIPTS_FOLDER/scripts/ci/bench.js $FOLDER/lib/binding/node_osrm.node $FOLDER/data.osrm $ALGORITHM $BENCH $GPS_TRACES > "$RESULTS_FOLDER/node_${BENCH}_${ALGORITHM}.bench" 5
-            END=$(date +%s.%N)
+    #         END=$(date +%s.%N)
-            DIFF=$(echo "$END - $START" | bc)
+    #         DIFF=$(echo "$END - $START" | bc)
-            echo "Took: ${DIFF}s"
+    #         echo "Took: ${DIFF}s"
-        done
+    #     done
-    done
+    # done
-    
+
-    for ALGORITHM in ch mld; do
+    for ALGORITHM in mld; do
-        for BENCH in nearest table trip route match; do
+        for BENCH in match; do
             echo "Running random $BENCH $ALGORITHM"
             START=$(date +%s.%N)
             $BENCHMARKS_FOLDER/bench "$FOLDER/data.osrm" $ALGORITHM $GPS_TRACES ${BENCH} > "$RESULTS_FOLDER/random_${BENCH}_${ALGORITHM}.bench" 5 || true
@@ -106,28 +107,28 @@ function run_benchmarks_for_folder {
     done
 
 
-    for ALGORITHM in ch mld; do
+    # for ALGORITHM in ch mld; do
-        $BINARIES_FOLDER/osrm-routed --algorithm $ALGORITHM $FOLDER/data.osrm > /dev/null 2>&1 &
+    #     $BINARIES_FOLDER/osrm-routed --algorithm $ALGORITHM $FOLDER/data.osrm > /dev/null 2>&1 &
-        OSRM_ROUTED_PID=$!
+    #     OSRM_ROUTED_PID=$!
 
-        # wait for osrm-routed to start
+    #     # wait for osrm-routed to start
-        if ! curl --retry-delay 3 --retry 10 --retry-all-errors "http://127.0.0.1:5000/route/v1/driving/13.388860,52.517037;13.385983,52.496891?steps=true" > /dev/null 2>&1; then
+    #     if ! curl --retry-delay 3 --retry 10 --retry-all-errors "http://127.0.0.1:5000/route/v1/driving/13.388860,52.517037;13.385983,52.496891?steps=true" > /dev/null 2>&1; then
-            echo "osrm-routed failed to start for algorithm $ALGORITHM"
+    #         echo "osrm-routed failed to start for algorithm $ALGORITHM"
-            kill -9 $OSRM_ROUTED_PID
+    #         kill -9 $OSRM_ROUTED_PID
-            continue
+    #         continue
-        fi
+    #     fi
 
-        for METHOD in route nearest trip table match; do
+    #     for METHOD in route nearest trip table match; do
-            echo "Running e2e benchmark for $METHOD $ALGORITHM"
+    #         echo "Running e2e benchmark for $METHOD $ALGORITHM"
-            START=$(date +%s.%N)
+    #         START=$(date +%s.%N)
-            python3 $SCRIPTS_FOLDER/scripts/ci/e2e_benchmark.py --host http://localhost:5000 --method $METHOD --iterations 5 --num_requests 1000 --gps_traces_file_path $GPS_TRACES > $RESULTS_FOLDER/e2e_${METHOD}_${ALGORITHM}.bench
+    #         python3 $SCRIPTS_FOLDER/scripts/ci/e2e_benchmark.py --host http://localhost:5000 --method $METHOD --iterations 5 --num_requests 1000 --gps_traces_file_path $GPS_TRACES > $RESULTS_FOLDER/e2e_${METHOD}_${ALGORITHM}.bench
-            END=$(date +%s.%N)
+    #         END=$(date +%s.%N)
-            DIFF=$(echo "$END - $START" | bc)
+    #         DIFF=$(echo "$END - $START" | bc)
-            echo "Took: ${DIFF}s"
+    #         echo "Took: ${DIFF}s"
-        done
+    #     done
 
-        kill -9 $OSRM_ROUTED_PID
+    #     kill -9 $OSRM_ROUTED_PID
-    done
+    # done
 }
 
 run_benchmarks_for_folder

src/engine/routing_algorithms/map_matching.cpp

@@ -45,7 +45,8 @@ unsigned getMedianSampleTime(const std::vector<unsigned> &timestamps)
 
 template <typename Algorithm>
 inline void initializeHeap(SearchEngineData<Algorithm> &engine_working_data,
-                           const DataFacade<Algorithm> &facade)
+                           const DataFacade<Algorithm> &facade,
+                           size_t)
 {
 
     const auto nodes_number = facade.GetNumberOfNodes();
@@ -54,14 +55,92 @@ inline void initializeHeap(SearchEngineData<Algorithm> &engine_working_data,
 
 template <>
 inline void initializeHeap<mld::Algorithm>(SearchEngineData<mld::Algorithm> &engine_working_data,
-                                           const DataFacade<mld::Algorithm> &facade)
+                                           const DataFacade<mld::Algorithm> &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,
+    engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(
-                                                                       border_nodes_number);
+        nodes_number, border_nodes_number, max_candidates);
 }
+
+#include <fstream>
+#include <iostream>
+
+template <typename T> void saveVectorToFile(const std::vector<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;
+    }
+    size_t size = data.size();
+    outFile.write(reinterpret_cast<const char *>(&size), sizeof(size));
+    outFile.write(reinterpret_cast<const char *>(data.data()), size * sizeof(T));
+    outFile.close();
+    if (!outFile.good())
+    {
+        std::cerr << "Error occurred at writing time!" << std::endl;
+    }
+}
+
+template <typename T> bool loadVectorFromFile(std::vector<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;
+    }
+    size_t size;
+    inFile.read(reinterpret_cast<char *>(&size), sizeof(size));
+    data.resize(size);
+    inFile.read(reinterpret_cast<char *>(data.data()), size * sizeof(T));
+    inFile.close();
+    if (!inFile.good())
+    {
+        std::cerr << "Error occurred at reading time!" << std::endl;
+        return false;
+    }
+    return true;
+}
+
+template <typename T> 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<const char *>(&data), sizeof(T));
+    outFile.close();
+    if (!outFile.good())
+    {
+        std::cerr << "Error occurred at writing time!" << std::endl;
+    }
+}
+
+template <typename T> 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<char *>(&data), sizeof(T));
+    inFile.close();
+    if (!inFile.good())
+    {
+        std::cerr << "Error occurred at reading time!" << std::endl;
+        return false;
+    }
+    return true;
+}
+
 } // namespace
 
 template <typename Algorithm>
@@ -144,9 +223,16 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &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<std::size_t> split_points;
@@ -225,6 +311,108 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                     continue;
                 }
 
+                //    PhantomNode source;
+                //   loadStructFromFile<PhantomNode>(source, "source.bin");
+                std::vector<PhantomNode> 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<std::size_t>(0UL, current_viterbi.size()))
+                {
+                    const double emission_pr = emission_log_probabilities[t][s_prime];
+                    double new_value = prev_viterbi[s] + emission_pr;
+                    if (current_viterbi[s_prime] > new_value)
+                    {
+                        continue;
+                    }
+                    target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node);
+                }
+
+// TIMER_START(NEW_DIST);
+
+#define MODE 1
+
+#if MODE == 0
+                auto new_distances =
+                    getNetworkDistances(engine_working_data,
+                                        facade,
+                                        forward_heap,
+                                        reverse_heaps,
+                                        prev_unbroken_timestamps_list[s].phantom_node,
+                                        target_phantom_nodes,
+                                        weight_upper_bound);
+               std::vector<double> old_distances;
+
+                for (const auto &pn : target_phantom_nodes)
+                {
+                    double network_distance =
+                        getNetworkDistance(engine_working_data,
+                                           facade,
+                                           forward_heap,
+                                           reverse_heap,
+                                           prev_unbroken_timestamps_list[s].phantom_node,
+                                           pn,
+                                           weight_upper_bound);
+                    old_distances.push_back(network_distance);
+                }
+
+                 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);
+                    }
+                }
+
+                auto distances = old_distances;
+#elif MODE == 1 
+               (void)reverse_heap;
+                auto distances =
+                    getNetworkDistances(engine_working_data,
+                                        facade,
+                                        forward_heap,
+                                        reverse_heaps,
+                                        prev_unbroken_timestamps_list[s].phantom_node,
+                                        target_phantom_nodes,
+                                        weight_upper_bound);
+                // TIMER_STOP(NEW_DIST);
+#else
+                // TIMER_START(OLD_DIST);
+                (void)reverse_heaps;
+                std::vector<double> distances;
+
+                for (const auto &pn : target_phantom_nodes)
+                {
+                    double network_distance =
+                        getNetworkDistance(engine_working_data,
+                                           facade,
+                                           forward_heap,
+                                           reverse_heap,
+                                           prev_unbroken_timestamps_list[s].phantom_node,
+                                           pn,
+                                           weight_upper_bound);
+                    distances.push_back(network_distance);
+                }
+#endif
+                // TIMER_STOP(OLD_DIST);
+
+                // std::cerr << "Old: " << TIMER_MSEC(OLD_DIST) << " New: " << TIMER_MSEC(NEW_DIST)
+                //           << std::endl;
+
+               
+                size_t distance_index = 0;
                 for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
                 {
                     const double emission_pr = emission_log_probabilities[t][s_prime];
@@ -234,14 +422,16 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                         continue;
                     }
 
-                    double network_distance =
+                    double network_distance = distances[distance_index];
-                        getNetworkDistance(engine_working_data,
+                    ++distance_index;
-                                           facade,
+                    // double network_distance =
-                                           forward_heap,
+                    //     getNetworkDistance(engine_working_data,
-                                           reverse_heap,
+                    //                        facade,
-                                           prev_unbroken_timestamps_list[s].phantom_node,
+                    //                        forward_heap,
-                                           current_timestamps_list[s_prime].phantom_node,
+                    //                        reverse_heap,
-                                           weight_upper_bound);
+                    //                        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);

src/engine/search_engine_data.cpp

@@ -15,6 +15,8 @@ thread_local SearchEngineData<CH>::SearchEngineHeapPtr
     SearchEngineData<CH>::map_matching_forward_heap_1;
 thread_local SearchEngineData<CH>::SearchEngineHeapPtr
     SearchEngineData<CH>::map_matching_reverse_heap_1;
+thread_local std::vector<typename SearchEngineData<CH>::SearchEngineHeapPtr>
+    SearchEngineData<CH>::map_matching_reverse_heaps;
 
 thread_local SearchEngineData<CH>::ManyToManyHeapPtr SearchEngineData<CH>::many_to_many_heap;
 
@@ -123,9 +125,11 @@ thread_local SearchEngineData<MLD>::MapMatchingHeapPtr
 thread_local SearchEngineData<MLD>::MapMatchingHeapPtr
     SearchEngineData<MLD>::map_matching_reverse_heap_1;
 thread_local SearchEngineData<MLD>::ManyToManyHeapPtr SearchEngineData<MLD>::many_to_many_heap;
+thread_local std::vector<typename SearchEngineData<MLD>::MapMatchingHeapPtr>
+    SearchEngineData<MLD>::map_matching_reverse_heaps;
 
 void SearchEngineData<MLD>::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 +150,16 @@ void SearchEngineData<MLD>::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<MLD>::InitializeOrClearFirstThreadLocalStorage(