wip

include/engine/routing_algorithms/routing_base_ch.hpp

@@ -464,7 +464,7 @@ inline std::vector<double>
 getNetworkDistances(SearchEngineData<Algorithm> &,
                     const DataFacade<ch::Algorithm> &,
                     SearchEngineData<Algorithm>::QueryHeap &,
-                    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)

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 <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,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<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 +463,12 @@ 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 +656,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;
@@ -796,14 +809,15 @@ auto routingStep2(const DataFacade<Algorithm> &facade, Heap &forward_heap, const
     return heapNode;
 }
 
-template <typename Algorithm, typename Heap, typename... Args>
+template <typename Algorithm, typename Heap>
 std::vector<std::optional<std::pair<NodeID, EdgeWeight>>>
 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 Args &...args)
+           const PhantomEndpointCandidates& candidates)
 {
     // if (forward_heap.Empty() || reverse_heap.Empty())
     // {
@@ -816,23 +830,23 @@ runSearch2(const DataFacade<Algorithm> &facade,
     std::vector<NodeID> middles;
     std::vector<EdgeWeight> weights;
 
-    middles.resize(reverse_heap.size(), SPECIAL_NODEID);
+    middles.resize(candidatesCount, SPECIAL_NODEID);
-    weights.resize(reverse_heap.size(), weight_upper_bound);
+    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 (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<Algorithm> &facade,
     {
         if (!forward_heap.Empty())
         {
-            auto heapNode = routingStep2<FORWARD_DIRECTION>(facade, forward_heap, args...);
+            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 < 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<Algorithm> &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<FORWARD_DIRECTION>(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<REVERSE_DIRECTION>(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<Algorithm> &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<REVERSE_DIRECTION>(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<Algorithm> &facade,
     };
 
     std::vector<std::optional<std::pair<NodeID, EdgeWeight>>> 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<Algorithm> &facade,
     // return {{middle, weight}};
 }
 
-template <typename Algorithm, typename... Args>
+template <typename Algorithm>
 std::vector<EdgeDistance> 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 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<EdgeDistance> res;
     for (size_t i = 0; i < searchResults.size(); ++i)
     {
@@ -968,6 +1002,9 @@ std::vector<EdgeDistance> 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<double>
 getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
                     const DataFacade<Algorithm> &facade,
                     typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
-                    typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &,
+                    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)
 {
-    using Heap = typename SearchEngineData<Algorithm>::MapMatchingQueryHeap;
     forward_heap.Clear();
-    std::vector<std::unique_ptr<Heap>> reverse_heaps;
+    for (const auto &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;
+        heap->Clear();
-        reverse_heaps.emplace_back(std::make_unique<Heap>(nodes_number, border_nodes_number));
     }
+    // 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())
     {
@@ -1041,17 +1081,16 @@ getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
     // {
     //     endpoints.push_back(target_phantom);
     // }
-    PhantomNodeCandidates phantom_candidates;
+    std::vector<PhantomNode> source_phantomes;
-    phantom_candidates.push_back(source_phantom);
+    source_phantomes.push_back(source_phantom);
-    for (const auto &target_phantom : target_phantoms)
+    PhantomEndpointCandidates phantom_candidates{source_phantomes, target_phantoms};
-    {
+
-        phantom_candidates.push_back(target_phantom);
-    }
 
     auto distances2 = searchDistance2(engine_working_data,
                                       facade,
                                       forward_heap,
                                       reverse_heaps,
+                                      target_phantoms.size(),
                                       {},
                                       weight_upper_bound,
                                       phantom_candidates);

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);

src/benchmarks/bench.cpp

@@ -395,9 +395,9 @@ void runMatchBenchmark(const OSRM &osrm, const GPSTraces &gpsTraces, int iterati
         std::optional<size_t> radius = std::nullopt;
     };
 
-    std::vector<Benchmark> benchmarks = {{"500 matches, default radius"},
+    std::vector<Benchmark> benchmarks = {{"500 matches, default radius", 10},
-                                         {"500 matches, radius=10", 10},
+                                         /*{"500 matches, radius=10", 10},
-                                         {"500 matches, radius=20", 20}};
+                                         {"500 matches, radius=20", 20}*/};
 
     runBenchmarks(benchmarks,
                   iterations,

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,86 @@ 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,
-                                                                       border_nodes_number);
+                                                                       border_nodes_number,
+                                                                       max_candidates);
 }
+
+#include <iostream>
+#include <fstream>
+
+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 +217,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,7 +305,19 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                     continue;
                 }
                 
-                std::vector<PhantomNode> target_phantom_nodes;
+            //    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];
@@ -237,16 +329,22 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &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);
-
+                // TIMER_STOP(NEW_DIST);
-                std::vector<double> old_distances;
+#else
+                // TIMER_START(OLD_DIST);
+                (void)reverse_heaps;
+                std::vector<double> distances;
 
                 for (const auto &pn : target_phantom_nodes)
                 {
@@ -258,18 +356,26 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &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)
+                // std::cerr << "Old: " << TIMER_MSEC(OLD_DIST) << " New: " << TIMER_MSEC(NEW_DIST)
-                {
+                //           << std::endl;
-                    if (std::abs(old_distances[i] - new_distances[i]) > 0.01)
-                    {
-                        std::cerr << "OOPS " << old_distances[i] << " " << new_distances[i]
-                                  << 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<std::size_t>(0UL, current_viterbi.size()))
                 {
                     const double emission_pr = emission_log_probabilities[t][s_prime];
@@ -279,14 +385,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,7 @@ 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 +124,10 @@ 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 +148,15 @@ 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(