Optimise getNetworkDistance in MLD even more (#6884)

CHANGELOG.md

@@ -53,6 +53,7 @@
       - FIXED: Remove force-loop checks for routes with u-turns [#6858](https://github.com/Project-OSRM/osrm-backend/pull/6858)
       - FIXED: Correctly check runtime search conditions for forcing routing steps [#6866](https://github.com/Project-OSRM/osrm-backend/pull/6866)
     - Map Matching:
+      - CHANGED: Optimise path distance calculation in MLD map matching even more. [#6884](https://github.com/Project-OSRM/osrm-backend/pull/6884)
       - CHANGED: Optimise path distance calculation in MLD map matching. [#6876](https://github.com/Project-OSRM/osrm-backend/pull/6876)
       - CHANGED: Optimise R-tree queries in the case of map matching. [#6881](https://github.com/Project-OSRM/osrm-backend/pull/6876)
     - Debug tiles:

include/engine/routing_algorithms/routing_base_mld.hpp

@@ -11,6 +11,7 @@
 #include <boost/assert.hpp>
 
 #include <algorithm>
+#include <boost/core/ignore_unused.hpp>
 #include <iterator>
 #include <limits>
 #include <tuple>
@@ -269,10 +270,29 @@ retrievePackedPathFromHeap(const SearchEngineData<Algorithm>::QueryHeap &forward
     return packed_path;
 }
 
-template <bool DIRECTION, typename Algorithm, typename... Args>
+template <typename Heap>
+void insertOrUpdate(Heap &heap,
+                    const NodeID node,
+                    const EdgeWeight weight,
+                    const typename Heap::DataType &data)
+{
+    const auto heapNode = heap.GetHeapNodeIfWasInserted(node);
+    if (!heapNode)
+    {
+        heap.Insert(node, weight, data);
+    }
+    else if (weight < heapNode->weight)
+    {
+        heapNode->data = data;
+        heapNode->weight = weight;
+        heap.DecreaseKey(*heapNode);
+    }
+}
+
+template <bool DIRECTION, typename Algorithm, typename Heap, typename... Args>
 void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
-                        typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
-                        const typename SearchEngineData<Algorithm>::QueryHeap::HeapNode &heapNode,
+                        Heap &forward_heap,
+                        const typename Heap::HeapNode &heapNode,
                         const Args &...args)
 {
     const auto &partition = facade.GetMultiLevelPartition();
@@ -281,14 +301,31 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
 
     const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
 
+    static constexpr auto IS_MAP_MATCHING =
+        std::is_same_v<typename SearchEngineData<mld::Algorithm>::MapMatchingQueryHeap, Heap>;
+
     if (level >= 1 && !heapNode.data.from_clique_arc)
     {
-        if (DIRECTION == FORWARD_DIRECTION)
+        if constexpr (DIRECTION == FORWARD_DIRECTION)
         {
             // Shortcuts in forward direction
             const auto &cell =
                 cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
             auto destination = cell.GetDestinationNodes().begin();
+            auto distance = [&cell, node = heapNode.node ]() -> auto
+            {
+                if constexpr (IS_MAP_MATCHING)
+                {
+
+                    return cell.GetOutDistance(node).begin();
+                }
+                else
+                {
+                    boost::ignore_unused(cell, node);
+                    return 0;
+                }
+            }
+            ();
             for (auto shortcut_weight : cell.GetOutWeight(heapNode.node))
             {
                 BOOST_ASSERT(destination != cell.GetDestinationNodes().end());
@@ -298,19 +335,23 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                 {
                     const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
                     BOOST_ASSERT(to_weight >= heapNode.weight);
-                    const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
-                    if (!toHeapNode)
+
+                    if constexpr (IS_MAP_MATCHING)
                     {
-                        forward_heap.Insert(to, to_weight, {heapNode.node, true});
+                        const EdgeDistance to_distance = heapNode.data.distance + *distance;
+                        insertOrUpdate(
+                            forward_heap, to, to_weight, {heapNode.node, true, to_distance});
                     }
-                    else if (to_weight < toHeapNode->weight)
+                    else
                     {
-                        toHeapNode->data = {heapNode.node, true};
-                        toHeapNode->weight = to_weight;
-                        forward_heap.DecreaseKey(*toHeapNode);
+                        insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, true});
                     }
                 }
                 ++destination;
+                if constexpr (IS_MAP_MATCHING)
+                {
+                    ++distance;
+                }
             }
         }
         else
@@ -319,6 +360,20 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
             const auto &cell =
                 cells.GetCell(metric, level, partition.GetCell(level, heapNode.node));
             auto source = cell.GetSourceNodes().begin();
+            auto distance = [&cell, node = heapNode.node ]() -> auto
+            {
+                if constexpr (IS_MAP_MATCHING)
+                {
+
+                    return cell.GetInDistance(node).begin();
+                }
+                else
+                {
+                    boost::ignore_unused(cell, node);
+                    return 0;
+                }
+            }
+            ();
             for (auto shortcut_weight : cell.GetInWeight(heapNode.node))
             {
                 BOOST_ASSERT(source != cell.GetSourceNodes().end());
@@ -328,19 +383,22 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                 {
                     const EdgeWeight to_weight = heapNode.weight + shortcut_weight;
                     BOOST_ASSERT(to_weight >= heapNode.weight);
-                    const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
-                    if (!toHeapNode)
+                    if constexpr (IS_MAP_MATCHING)
                     {
-                        forward_heap.Insert(to, to_weight, {heapNode.node, true});
+                        const EdgeDistance to_distance = heapNode.data.distance + *distance;
+                        insertOrUpdate(
+                            forward_heap, to, to_weight, {heapNode.node, true, to_distance});
                     }
-                    else if (to_weight < toHeapNode->weight)
+                    else
                     {
-                        toHeapNode->data = {heapNode.node, true};
-                        toHeapNode->weight = to_weight;
-                        forward_heap.DecreaseKey(*toHeapNode);
+                        insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, true});
                     }
                 }
                 ++source;
+                if constexpr (IS_MAP_MATCHING)
+                {
+                    ++distance;
+                }
             }
         }
     }
@@ -367,26 +425,28 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
                 const EdgeWeight to_weight =
                     heapNode.weight + node_weight + alias_cast<EdgeWeight>(turn_penalty);
 
-                const auto toHeapNode = forward_heap.GetHeapNodeIfWasInserted(to);
-                if (!toHeapNode)
+                if constexpr (IS_MAP_MATCHING)
                 {
-                    forward_heap.Insert(to, to_weight, {heapNode.node, false});
+                    const auto node_distance =
+                        facade.GetNodeDistance(DIRECTION == FORWARD_DIRECTION ? heapNode.node : to);
+
+                    const EdgeDistance to_distance = heapNode.data.distance + node_distance;
+                    insertOrUpdate(
+                        forward_heap, to, to_weight, {heapNode.node, false, to_distance});
                 }
-                else if (to_weight < toHeapNode->weight)
+                else
                 {
-                    toHeapNode->data = {heapNode.node, false};
-                    toHeapNode->weight = to_weight;
-                    forward_heap.DecreaseKey(*toHeapNode);
+                    insertOrUpdate(forward_heap, to, to_weight, {heapNode.node, false});
                 }
             }
         }
     }
 }
 
-template <bool DIRECTION, typename Algorithm, typename... Args>
+template <bool DIRECTION, typename Algorithm, typename Heap, typename... Args>
 void routingStep(const DataFacade<Algorithm> &facade,
-                 typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
-                 typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
+                 Heap &forward_heap,
+                 Heap &reverse_heap,
                  NodeID &middle_node,
                  EdgeWeight &path_upper_bound,
                  const std::vector<NodeID> &force_step_nodes,
@@ -429,22 +489,19 @@ using UnpackedNodes = std::vector<NodeID>;
 using UnpackedEdges = std::vector<EdgeID>;
 using UnpackedPath = std::tuple<EdgeWeight, UnpackedNodes, UnpackedEdges>;
 
-template <typename Algorithm, typename... Args>
-UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
-                    const DataFacade<Algorithm> &facade,
-                    typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
-                    typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
-                    const std::vector<NodeID> &force_step_nodes,
-                    EdgeWeight weight_upper_bound,
-                    const Args &...args)
+template <typename Algorithm, typename Heap, typename... Args>
+std::optional<std::pair<NodeID, EdgeWeight>> runSearch(const DataFacade<Algorithm> &facade,
+                                                       Heap &forward_heap,
+                                                       Heap &reverse_heap,
+                                                       const std::vector<NodeID> &force_step_nodes,
+                                                       EdgeWeight weight_upper_bound,
+                                                       const Args &...args)
 {
     if (forward_heap.Empty() || reverse_heap.Empty())
     {
-        return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
+        return {};
     }
 
-    const auto &partition = facade.GetMultiLevelPartition();
-
     BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
     BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
 
@@ -474,10 +531,33 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
 
     // No path found for both target nodes?
     if (weight >= weight_upper_bound || SPECIAL_NODEID == middle)
+    {
+        return {};
+    }
+
+    return {{middle, weight}};
+}
+
+template <typename Algorithm, typename... Args>
+UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
+                    const DataFacade<Algorithm> &facade,
+                    typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
+                    typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
+                    const std::vector<NodeID> &force_step_nodes,
+                    EdgeWeight weight_upper_bound,
+                    const Args &...args)
+{
+    auto searchResult = runSearch(
+        facade, forward_heap, reverse_heap, force_step_nodes, weight_upper_bound, args...);
+    if (!searchResult)
     {
         return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
     }
 
+    auto [middle, weight] = *searchResult;
+
+    const auto &partition = facade.GetMultiLevelPartition();
+
     // Get packed path as edges {from node ID, to node ID, from_clique_arc}
     auto packed_path = retrievePackedPathFromHeap(forward_heap, reverse_heap, middle);
 
@@ -536,6 +616,31 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
     return std::make_tuple(weight, std::move(unpacked_nodes), std::move(unpacked_edges));
 }
 
+template <typename Algorithm, typename... Args>
+EdgeDistance
+searchDistance(SearchEngineData<Algorithm> &,
+               const DataFacade<Algorithm> &facade,
+               typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+               typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
+               const std::vector<NodeID> &force_step_nodes,
+               EdgeWeight weight_upper_bound,
+               const Args &...args)
+{
+
+    auto searchResult = runSearch(
+        facade, forward_heap, reverse_heap, force_step_nodes, weight_upper_bound, args...);
+    if (!searchResult)
+    {
+        return INVALID_EDGE_DISTANCE;
+    }
+
+    auto [middle, _] = *searchResult;
+
+    auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance;
+
+    return distance;
+}
+
 // Alias to be compatible with the CH-based search
 template <typename Algorithm, typename PhantomEndpointT>
 inline void search(SearchEngineData<Algorithm> &engine_working_data,
@@ -593,8 +698,8 @@ void unpackPath(const FacadeT &facade,
 template <typename Algorithm>
 double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                           const DataFacade<Algorithm> &facade,
-                          typename SearchEngineData<Algorithm>::QueryHeap &forward_heap,
-                          typename SearchEngineData<Algorithm>::QueryHeap &reverse_heap,
+                          typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
+                          typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
                           const PhantomNode &source_phantom,
                           const PhantomNode &target_phantom,
                           EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
@@ -602,48 +707,49 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
     forward_heap.Clear();
     reverse_heap.Clear();
 
-    const PhantomEndpoints endpoints{source_phantom, target_phantom};
-    insertNodesInHeaps(forward_heap, reverse_heap, endpoints);
+    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()});
+    }
 
-    auto [weight, unpacked_nodes, unpacked_edges] = search(
+    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()});
+    }
+
+    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()});
+    }
+
+    const PhantomEndpoints endpoints{source_phantom, target_phantom};
+
+    auto distance = searchDistance(
         engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound, endpoints);
 
-    if (weight == INVALID_EDGE_WEIGHT)
+    if (distance == INVALID_EDGE_DISTANCE)
     {
         return std::numeric_limits<double>::max();
     }
-
-    BOOST_ASSERT(unpacked_nodes.size() >= 1);
-
-    EdgeDistance distance = {0.0};
-
-    if (source_phantom.forward_segment_id.id == unpacked_nodes.front())
-    {
-        BOOST_ASSERT(source_phantom.forward_segment_id.enabled);
-        distance = EdgeDistance{0} - source_phantom.GetForwardDistance();
-    }
-    else if (source_phantom.reverse_segment_id.id == unpacked_nodes.front())
-    {
-        BOOST_ASSERT(source_phantom.reverse_segment_id.enabled);
-        distance = EdgeDistance{0} - source_phantom.GetReverseDistance();
-    }
-
-    for (size_t index = 0; index < unpacked_nodes.size() - 1; ++index)
-    {
-        distance += facade.GetNodeDistance(unpacked_nodes[index]);
-    }
-
-    if (target_phantom.forward_segment_id.id == unpacked_nodes.back())
-    {
-        BOOST_ASSERT(target_phantom.forward_segment_id.enabled);
-        distance += target_phantom.GetForwardDistance();
-    }
-    else if (target_phantom.reverse_segment_id.id == unpacked_nodes.back())
-    {
-        BOOST_ASSERT(target_phantom.reverse_segment_id.enabled);
-        distance += target_phantom.GetReverseDistance();
-    }
-
     return from_alias<double>(distance);
 }
 

include/engine/search_engine_data.hpp

@@ -47,6 +47,7 @@ template <> struct SearchEngineData<routing_algorithms::ch::Algorithm>
                                                 util::UnorderedMapStorage<NodeID, int>>;
 
     using SearchEngineHeapPtr = boost::thread_specific_ptr<QueryHeap>;
+
     using ManyToManyHeapPtr = boost::thread_specific_ptr<ManyToManyQueryHeap>;
 
     static SearchEngineHeapPtr forward_heap_1;
@@ -56,6 +57,10 @@ template <> struct SearchEngineData<routing_algorithms::ch::Algorithm>
     static SearchEngineHeapPtr forward_heap_3;
     static SearchEngineHeapPtr reverse_heap_3;
     static ManyToManyHeapPtr many_to_many_heap;
+    static SearchEngineHeapPtr map_matching_forward_heap_1;
+    static SearchEngineHeapPtr map_matching_reverse_heap_1;
+
+    void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes);
 
     void InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes);
 
@@ -74,6 +79,19 @@ struct MultiLayerDijkstraHeapData
     MultiLayerDijkstraHeapData(NodeID p, bool from) : parent(p), from_clique_arc(from) {}
 };
 
+struct MapMatchingMultiLayerDijkstraHeapData
+{
+    NodeID parent;
+    bool from_clique_arc;
+    EdgeDistance distance = {0};
+    MapMatchingMultiLayerDijkstraHeapData(NodeID p) : parent(p), from_clique_arc(false) {}
+    MapMatchingMultiLayerDijkstraHeapData(NodeID p, bool from) : parent(p), from_clique_arc(from) {}
+    MapMatchingMultiLayerDijkstraHeapData(NodeID p, bool from, EdgeDistance d)
+        : parent(p), from_clique_arc(from), distance(d)
+    {
+    }
+};
+
 struct ManyToManyMultiLayerDijkstraHeapData : MultiLayerDijkstraHeapData
 {
     EdgeDuration duration;
@@ -104,16 +122,27 @@ template <> struct SearchEngineData<routing_algorithms::mld::Algorithm>
                                                 EdgeWeight,
                                                 ManyToManyMultiLayerDijkstraHeapData,
                                                 util::TwoLevelStorage<NodeID, int>>;
+    using MapMatchingQueryHeap = util::QueryHeap<NodeID,
+                                                 NodeID,
+                                                 EdgeWeight,
+                                                 MapMatchingMultiLayerDijkstraHeapData,
+                                                 util::TwoLevelStorage<NodeID, int>>;
 
     using SearchEngineHeapPtr = boost::thread_specific_ptr<QueryHeap>;
     using ManyToManyHeapPtr = boost::thread_specific_ptr<ManyToManyQueryHeap>;
+    using MapMatchingHeapPtr = boost::thread_specific_ptr<MapMatchingQueryHeap>;
 
     static SearchEngineHeapPtr forward_heap_1;
     static SearchEngineHeapPtr reverse_heap_1;
+    static MapMatchingHeapPtr map_matching_forward_heap_1;
+    static MapMatchingHeapPtr map_matching_reverse_heap_1;
+
     static 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);
 
     void InitializeOrClearManyToManyThreadLocalStorage(unsigned number_of_nodes,
                                                        unsigned number_of_boundary_nodes);

src/engine/routing_algorithms/map_matching.cpp

@@ -49,7 +49,7 @@ inline void initializeHeap(SearchEngineData<Algorithm> &engine_working_data,
 {
 
     const auto nodes_number = facade.GetNumberOfNodes();
-    engine_working_data.InitializeOrClearFirstThreadLocalStorage(nodes_number);
+    engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number);
 }
 
 template <>
@@ -59,7 +59,8 @@ inline void initializeHeap<mld::Algorithm>(SearchEngineData<mld::Algorithm> &eng
 
     const auto nodes_number = facade.GetNumberOfNodes();
     const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1;
-    engine_working_data.InitializeOrClearFirstThreadLocalStorage(nodes_number, border_nodes_number);
+    engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number,
+                                                                       border_nodes_number);
 }
 } // namespace
 
@@ -144,8 +145,8 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
     }
 
     initializeHeap(engine_working_data, facade);
-    auto &forward_heap = *engine_working_data.forward_heap_1;
-    auto &reverse_heap = *engine_working_data.reverse_heap_1;
+    auto &forward_heap = *engine_working_data.map_matching_forward_heap_1;
+    auto &reverse_heap = *engine_working_data.map_matching_reverse_heap_1;
 
     std::size_t breakage_begin = map_matching::INVALID_STATE;
     std::vector<std::size_t> split_points;

src/engine/search_engine_data.cpp

@@ -11,8 +11,32 @@ SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::forward_heap_2;
 SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::reverse_heap_2;
 SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::forward_heap_3;
 SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::reverse_heap_3;
+SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::map_matching_forward_heap_1;
+SearchEngineData<CH>::SearchEngineHeapPtr SearchEngineData<CH>::map_matching_reverse_heap_1;
+
 SearchEngineData<CH>::ManyToManyHeapPtr SearchEngineData<CH>::many_to_many_heap;
 
+void SearchEngineData<CH>::InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes)
+{
+    if (map_matching_forward_heap_1.get())
+    {
+        map_matching_forward_heap_1->Clear();
+    }
+    else
+    {
+        map_matching_forward_heap_1.reset(new QueryHeap(number_of_nodes));
+    }
+
+    if (map_matching_reverse_heap_1.get())
+    {
+        map_matching_reverse_heap_1->Clear();
+    }
+    else
+    {
+        map_matching_reverse_heap_1.reset(new QueryHeap(number_of_nodes));
+    }
+}
+
 void SearchEngineData<CH>::InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes)
 {
     if (forward_heap_1.get())
@@ -92,8 +116,34 @@ void SearchEngineData<CH>::InitializeOrClearManyToManyThreadLocalStorage(unsigne
 using MLD = routing_algorithms::mld::Algorithm;
 SearchEngineData<MLD>::SearchEngineHeapPtr SearchEngineData<MLD>::forward_heap_1;
 SearchEngineData<MLD>::SearchEngineHeapPtr SearchEngineData<MLD>::reverse_heap_1;
+SearchEngineData<MLD>::MapMatchingHeapPtr SearchEngineData<MLD>::map_matching_forward_heap_1;
+SearchEngineData<MLD>::MapMatchingHeapPtr SearchEngineData<MLD>::map_matching_reverse_heap_1;
 SearchEngineData<MLD>::ManyToManyHeapPtr SearchEngineData<MLD>::many_to_many_heap;
 
+void SearchEngineData<MLD>::InitializeOrClearMapMatchingThreadLocalStorage(
+    unsigned number_of_nodes, unsigned number_of_boundary_nodes)
+{
+    if (map_matching_forward_heap_1.get())
+    {
+        map_matching_forward_heap_1->Clear();
+    }
+    else
+    {
+        map_matching_forward_heap_1.reset(
+            new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes));
+    }
+
+    if (map_matching_reverse_heap_1.get())
+    {
+        map_matching_reverse_heap_1->Clear();
+    }
+    else
+    {
+        map_matching_reverse_heap_1.reset(
+            new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes));
+    }
+}
+
 void SearchEngineData<MLD>::InitializeOrClearFirstThreadLocalStorage(
     unsigned number_of_nodes, unsigned number_of_boundary_nodes)
 {