Optimise getNetworkDistance in MLD even more (#6884)

2024-05-23 20:46:13 +02:00 · 2024-05-23 20:46:13 +02:00 · 21607e0cb2
commit 21607e0cb2
parent 24646aada9
5 changed files with 268 additions and 81 deletions
--- a/CHANGELOG.md
+++ b/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:
--- a/include/engine/routing_algorithms/routing_base_mld.hpp
+++ b/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);
 }

--- a/include/engine/search_engine_data.hpp
+++ b/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);
--- a/src/engine/routing_algorithms/map_matching.cpp
+++ b/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;
--- a/src/engine/search_engine_data.cpp
+++ b/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)
 {