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

CHANGELOG.md

@@ -1,6 +1,7 @@
 # Unreleased
   - Changes from 5.27.1
     - Features
+      - ADDED: Route pedestrians over highway=platform [#6993](https://github.com/Project-OSRM/osrm-backend/pull/6993)
       - REMOVED: Remove all core-CH left-overs [#6920](https://github.com/Project-OSRM/osrm-backend/pull/6920)
       - ADDED: Add support for a keepalive_timeout flag. [#6674](https://github.com/Project-OSRM/osrm-backend/pull/6674)
       - ADDED: Add support for a default_radius flag. [#6575](https://github.com/Project-OSRM/osrm-backend/pull/6575)

features/foot/access.feature

@@ -26,7 +26,15 @@ Feature: Foot - Access tags on ways
             | motorway | no     |      |       |
             | motorway | no     | yes  | x     |
             | motorway | no     | no   |       |
-
+            | platform |        |      | x     |
+            | platform |        | yes  | x     |
+            | platform |        | no   |       |
+            | platform | yes    |      | x     |
+            | platform | yes    | yes  | x     |
+            | platform | yes    | no   |       |
+            | platform | no     |      |       |
+            | platform | no     | yes  | x     |
+            | platform | no     | no   |       |
 
     Scenario: Foot - Overwriting implied acccess on ways
         Then routability should be

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

include/util/meminfo.hpp

@@ -2,6 +2,7 @@
 #define MEMINFO_HPP
 
 #include "util/log.hpp"
+#include <cstddef>
 
 #ifndef _WIN32
 #include <sys/resource.h>
@@ -10,22 +11,31 @@
 namespace osrm::util
 {
 
-inline void DumpMemoryStats()
+inline size_t PeakRAMUsedInBytes()
 {
 #ifndef _WIN32
     rusage usage;
     getrusage(RUSAGE_SELF, &usage);
 #ifdef __linux__
     // Under linux, ru.maxrss is in kb
-    util::Log() << "RAM: peak bytes used: " << usage.ru_maxrss * 1024;
+    return usage.ru_maxrss * 1024;
 #else  // __linux__
     // Under BSD systems (OSX), it's in bytes
-    util::Log() << "RAM: peak bytes used: " << usage.ru_maxrss;
+    return usage.ru_maxrss;
 #endif // __linux__
+#else  // _WIN32
+    return 0;
+#endif // _WIN32
+}
+
+inline void DumpMemoryStats()
+{
+#ifndef _WIN32
+    util::Log() << "RAM: peak bytes used: " << PeakRAMUsedInBytes();
 #else  // _WIN32
     util::Log() << "RAM: peak bytes used: <not implemented on Windows>";
 #endif // _WIN32
 }
 } // namespace osrm::util
 
-#endif
+#endif

package-lock.json

@@ -10,7 +10,8 @@
       "hasInstallScript": true,
       "license": "BSD-2-Clause",
       "dependencies": {
-        "@mapbox/node-pre-gyp": "^1.0.11"
+        "@mapbox/node-pre-gyp": "^1.0.11",
+        "seedrandom": "^3.0.5"
       },
       "devDependencies": {
         "@babel/cli": "^7.18.10",
@@ -14652,6 +14653,11 @@
       "integrity": "sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg==",
       "devOptional": true
     },
+    "node_modules/seedrandom": {
+      "version": "3.0.5",
+      "resolved": "https://registry.npmjs.org/seedrandom/-/seedrandom-3.0.5.tgz",
+      "integrity": "sha512-8OwmbklUNzwezjGInmZ+2clQmExQPvomqjL7LFqOYqtmuxRgQYqOD3mHaU+MvZn5FLUeVxVfQjwLZW/n/JFuqg=="
+    },
     "node_modules/semver": {
       "version": "5.7.2",
       "resolved": "https://registry.npmjs.org/semver/-/semver-5.7.2.tgz",
@@ -30296,6 +30302,11 @@
       "integrity": "sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg==",
       "devOptional": true
     },
+    "seedrandom": {
+      "version": "3.0.5",
+      "resolved": "https://registry.npmjs.org/seedrandom/-/seedrandom-3.0.5.tgz",
+      "integrity": "sha512-8OwmbklUNzwezjGInmZ+2clQmExQPvomqjL7LFqOYqtmuxRgQYqOD3mHaU+MvZn5FLUeVxVfQjwLZW/n/JFuqg=="
+    },
     "semver": {
       "version": "5.7.2",
       "resolved": "https://registry.npmjs.org/semver/-/semver-5.7.2.tgz",

package.json

@@ -4,7 +4,8 @@
   "private": false,
   "description": "The Open Source Routing Machine is a high performance routing engine written in C++ designed to run on OpenStreetMap data.",
   "dependencies": {
-    "@mapbox/node-pre-gyp": "^1.0.11"
+    "@mapbox/node-pre-gyp": "^1.0.11",
+    "seedrandom": "^3.0.5"
   },
   "browserify": {
     "transform": [
@@ -57,6 +58,7 @@
     "jsonpath": "^1.1.1",
     "mkdirp": "^0.5.6",
     "node-addon-api": "^5.0.0",
+    "node-cmake": "^2.5.1",
     "node-timeout": "0.0.4",
     "polyline": "^0.2.0",
     "request": "^2.88.2",
@@ -64,12 +66,13 @@
     "tape": "^4.16.0",
     "turf": "^3.0.14",
     "uglify-js": "^3.17.0",
-    "xmlbuilder": "^4.2.1",
-    "node-cmake": "^2.5.1"
+    "xmlbuilder": "^4.2.1"
   },
   "main": "lib/index.js",
   "binary": {
-    "napi_versions": [8],
+    "napi_versions": [
+      8
+    ],
     "module_name": "node_osrm",
     "module_path": "./lib/binding_napi_v{napi_build_version}/",
     "host": "https://github.com",

profiles/foot.lua

@@ -90,6 +90,7 @@ function setup()
         path            = walking_speed,
         steps           = walking_speed,
         pedestrian      = walking_speed,
+        platform        = walking_speed,
         footway         = walking_speed,
         pier            = walking_speed,
       },

scripts/ci/bench.js

@@ -0,0 +1,211 @@
+const fs = require('fs');
+const path = require('path');
+const readline = require('readline');
+const seedrandom = require('seedrandom');
+
+
+let RNG;
+
+class GPSData {
+    constructor(gpsTracesFilePath) {
+        this.tracks = {};
+        this.coordinates = [];
+        this.trackIds = [];
+        this._loadGPSTraces(gpsTracesFilePath);
+    }
+
+    _loadGPSTraces(gpsTracesFilePath) {
+        const expandedPath = path.resolve(gpsTracesFilePath);
+        const data = fs.readFileSync(expandedPath, 'utf-8');
+        const lines = data.split('\n');
+        const headers = lines[0].split(',');
+
+        const latitudeIndex = headers.indexOf('Latitude');
+        const longitudeIndex = headers.indexOf('Longitude');
+        const trackIdIndex = headers.indexOf('TrackID');
+
+        for (let i = 1; i < lines.length; i++) {
+            if (lines[i].trim() === '') continue;
+            const row = lines[i].split(',');
+
+            const latitude = parseFloat(row[latitudeIndex]);
+            const longitude = parseFloat(row[longitudeIndex]);
+            const trackId = row[trackIdIndex];
+
+            const coord = [longitude, latitude];
+            this.coordinates.push(coord);
+
+            if (!this.tracks[trackId]) {
+                this.tracks[trackId] = [];
+            }
+            this.tracks[trackId].push(coord);
+        }
+
+        this.trackIds = Object.keys(this.tracks);
+    }
+
+    getRandomCoordinate() {
+        const randomIndex = Math.floor(RNG() * this.coordinates.length);
+        return this.coordinates[randomIndex];
+    }
+
+    getRandomTrack() {
+        const randomIndex = Math.floor(RNG() * this.trackIds.length);
+        const trackId = this.trackIds[randomIndex];
+        return this.tracks[trackId];
+    }
+};
+
+async function runOSRMMethod(osrm, method, coordinates) {
+    const time = await new Promise((resolve, reject) => {
+        const startTime = process.hrtime();
+        osrm[method]({coordinates}, (err, result) => {
+            if (err) {
+                if (['NoSegment', 'NoMatch', 'NoRoute', 'NoTrips'].includes(err.message)) {
+                    resolve(null);
+                } else {
+
+                reject(err);
+                }
+            } else {
+                const endTime = process.hrtime(startTime);
+                resolve(endTime[0] + endTime[1] / 1e9);
+            }
+        });
+    });
+    return time;
+}
+
+async function nearest(osrm, gpsData) {
+    const times = [];
+    for (let i = 0; i < 1000; i++) {
+        const coord = gpsData.getRandomCoordinate();
+        times.push(await runOSRMMethod(osrm, 'nearest', [coord]));
+    }
+    return times;
+}
+
+async function route(osrm, gpsData) {
+    const times = [];
+    for (let i = 0; i < 1000; i++) {
+        const from = gpsData.getRandomCoordinate();
+        const to = gpsData.getRandomCoordinate();
+
+        
+        times.push(await runOSRMMethod(osrm, 'route', [from, to]));
+    }
+    return times;
+}
+
+async function table(osrm, gpsData) {
+    const times = [];
+    for (let i = 0; i < 250; i++) {
+        const numPoints = Math.floor(RNG() * 3) + 15;
+        const coordinates = [];
+        for (let i = 0; i < numPoints; i++) {
+            coordinates.push(gpsData.getRandomCoordinate());
+        }
+
+        
+        times.push(await runOSRMMethod(osrm, 'table', coordinates));
+    }
+    return times;
+}
+
+async function match(osrm, gpsData) {
+    const times = [];
+    for (let i = 0; i < 1000; i++) {
+        const numPoints = Math.floor(RNG() * 50) + 50;
+        const coordinates = gpsData.getRandomTrack().slice(0, numPoints);
+
+        
+        times.push(await runOSRMMethod(osrm, 'match', coordinates));
+    }
+    return times;
+}
+
+async function trip(osrm, gpsData) {
+    const times = [];
+    for (let i = 0; i < 250; i++) {
+        const numPoints = Math.floor(RNG() * 2) + 5;
+        const coordinates = [];
+        for (let i = 0; i < numPoints; i++) {
+            coordinates.push(gpsData.getRandomCoordinate());
+        }
+
+        
+        times.push(await runOSRMMethod(osrm, 'trip', coordinates));
+    }
+    return times;
+}
+
+function bootstrapConfidenceInterval(data, numSamples = 1000, confidenceLevel = 0.95) {
+    let means = [];
+    let dataLength = data.length;
+
+    for (let i = 0; i < numSamples; i++) {
+        let sample = [];
+        for (let j = 0; j < dataLength; j++) {
+            let randomIndex = Math.floor(RNG() * dataLength);
+            sample.push(data[randomIndex]);
+        }
+        let sampleMean = sample.reduce((a, b) => a + b, 0) / sample.length;
+        means.push(sampleMean);
+    }
+
+    means.sort((a, b) => a - b);
+    let lowerBoundIndex = Math.floor((1 - confidenceLevel) / 2 * numSamples);
+    let upperBoundIndex = Math.floor((1 + confidenceLevel) / 2 * numSamples);
+    let mean = means.reduce((a, b) => a + b, 0) / means.length;
+    let lowerBound = means[lowerBoundIndex];
+    let upperBound = means[upperBoundIndex];
+
+    return { mean: mean, lowerBound: lowerBound, upperBound: upperBound };
+}
+
+function calculateConfidenceInterval(data) {
+    let { mean, lowerBound, upperBound } = bootstrapConfidenceInterval(data);
+    let bestValue = Math.max(...data);
+    let errorMargin = (upperBound - lowerBound) / 2;
+
+    return { mean, errorMargin, bestValue };
+}
+
+async function main() {
+    const args = process.argv.slice(2);
+
+    const {OSRM} = require(args[0]);
+    const path = args[1];
+    const algorithm = args[2].toUpperCase();
+    const method = args[3];
+    const gpsTracesFilePath = args[4];
+    const iterations = parseInt(args[5]);
+
+    const gpsData = new GPSData(gpsTracesFilePath);
+    const osrm = new OSRM({path, algorithm});
+
+
+    const functions = {
+        route: route,
+        table: table,
+        nearest: nearest,
+        match: match,
+        trip: trip
+    };
+    const func = functions[method];
+    if (!func) {
+        throw new Error('Unknown method');
+    }
+    const allTimes = [];
+    for (let i = 0; i < iterations; i++) {
+        RNG = seedrandom(42);
+        allTimes.push((await func(osrm, gpsData)).filter(t => t !== null));
+    }
+
+    const opsPerSec = allTimes.map(times => times.length / times.reduce((a, b) => a + b, 0));
+    const { mean, errorMargin, bestValue } = calculateConfidenceInterval(opsPerSec);
+    console.log(`Ops: ${mean.toFixed(1)} ± ${errorMargin.toFixed(1)} ops/s. Best: ${bestValue.toFixed(1)} ops/s`);
+
+}
+
+main();

scripts/ci/run_benchmarks.sh

@@ -50,25 +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"
-    $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/match_mld.bench"
-    echo "Running match-bench CH"
-    $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/match_ch.bench"
-    echo "Running route-bench MLD"
-    $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/route_mld.bench"
-    echo "Running route-bench CH"
-    $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/route_ch.bench"
-    echo "Running alias"
-    $BENCHMARKS_FOLDER/alias-bench > "$RESULTS_FOLDER/alias.bench"
-    echo "Running json-render-bench"
-    $BENCHMARKS_FOLDER/json-render-bench  "$FOLDER/test/data/portugal_to_korea.json" > "$RESULTS_FOLDER/json-render.bench"
-    echo "Running packedvector-bench"
-    $BENCHMARKS_FOLDER/packedvector-bench > "$RESULTS_FOLDER/packedvector.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"
+
+    # echo "Running match-bench MLD"
+    # $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/match_mld.bench"
+    # echo "Running match-bench CH"
+    # $BENCHMARKS_FOLDER/match-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/match_ch.bench"
+    # echo "Running route-bench MLD"
+    # $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/mld/monaco.osrm" mld > "$RESULTS_FOLDER/route_mld.bench"
+    # echo "Running route-bench CH"
+    # $BENCHMARKS_FOLDER/route-bench "$FOLDER/test/data/ch/monaco.osrm" ch > "$RESULTS_FOLDER/route_ch.bench"
+    # echo "Running alias"
+    # $BENCHMARKS_FOLDER/alias-bench > "$RESULTS_FOLDER/alias.bench"
+    # echo "Running json-render-bench"
+    # $BENCHMARKS_FOLDER/json-render-bench  "$FOLDER/test/data/portugal_to_korea.json" > "$RESULTS_FOLDER/json-render.bench"
+    # echo "Running packedvector-bench"
+    # $BENCHMARKS_FOLDER/packedvector-bench > "$RESULTS_FOLDER/packedvector.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"
 
     cp -rf $OSM_PBF $FOLDER/data.osm.pbf
 
@@ -81,8 +83,20 @@ function run_benchmarks_for_folder {
     echo "Running osrm-contract"
     measure_peak_ram_and_time "$BINARIES_FOLDER/osrm-contract $FOLDER/data.osrm" "$RESULTS_FOLDER/osrm_contract.bench"
 
-    for ALGORITHM in ch mld; do
-        for BENCH in nearest table trip route match; do
+
+    # for ALGORITHM in ch mld; do
+    #     for BENCH in nearest table trip route match; do
+    #         echo "Running node $BENCH $ALGORITHM"
+    #         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
+    #         END=$(date +%s.%N)
+    #         DIFF=$(echo "$END - $START" | bc)
+    #         echo "Took: ${DIFF}s"
+    #     done
+    # done
+
+    for ALGORITHM in mld; 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
@@ -93,28 +107,28 @@ function run_benchmarks_for_folder {
     done
 
 
-    for ALGORITHM in ch mld; do
-        $BINARIES_FOLDER/osrm-routed --algorithm $ALGORITHM $FOLDER/data.osrm > /dev/null 2>&1 &
-        OSRM_ROUTED_PID=$!
+    # for ALGORITHM in ch mld; do
+    #     $BINARIES_FOLDER/osrm-routed --algorithm $ALGORITHM $FOLDER/data.osrm > /dev/null 2>&1 &
+    #     OSRM_ROUTED_PID=$!
 
-        # 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
-            echo "osrm-routed failed to start for algorithm $ALGORITHM"
-            kill -9 $OSRM_ROUTED_PID
-            continue
-        fi
+    #     # 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
+    #         echo "osrm-routed failed to start for algorithm $ALGORITHM"
+    #         kill -9 $OSRM_ROUTED_PID
+    #         continue
+    #     fi
 
-        for METHOD in route nearest trip table match; do
-            echo "Running e2e benchmark for $METHOD $ALGORITHM"
-            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
-            END=$(date +%s.%N)
-            DIFF=$(echo "$END - $START" | bc)
-            echo "Took: ${DIFF}s"
-        done
+    #     for METHOD in route nearest trip table match; do
+    #         echo "Running e2e benchmark for $METHOD $ALGORITHM"
+    #         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
+    #         END=$(date +%s.%N)
+    #         DIFF=$(echo "$END - $START" | bc)
+    #         echo "Took: ${DIFF}s"
+    #     done
 
-        kill -9 $OSRM_ROUTED_PID
-    done
+    #     kill -9 $OSRM_ROUTED_PID
+    # done
 }
 
 run_benchmarks_for_folder

src/benchmarks/bench.cpp

@@ -16,8 +16,8 @@
 #include "osrm/osrm.hpp"
 #include "osrm/status.hpp"
 
+#include "util/meminfo.hpp"
 #include <boost/assert.hpp>
-
 #include <boost/optional/optional.hpp>
 #include <cstdlib>
 #include <exception>
@@ -655,6 +655,12 @@ try
         std::cerr << "Unknown benchmark: " << benchmarkToRun << std::endl;
         return EXIT_FAILURE;
     }
+
+    std::cout << "Peak RAM: " << std::setprecision(3)
+              << static_cast<double>(osrm::util::PeakRAMUsedInBytes()) /
+                     static_cast<double>((1024 * 1024))
+              << "MB" << std::endl;
+
     return EXIT_SUCCESS;
 }
 catch (const std::exception &e)

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,
-                                                                       border_nodes_number);
+    engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(
+        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 =
-                        getNetworkDistance(engine_working_data,
-                                           facade,
-                                           forward_heap,
-                                           reverse_heap,
-                                           prev_unbroken_timestamps_list[s].phantom_node,
-                                           current_timestamps_list[s_prime].phantom_node,
-                                           weight_upper_bound);
+                    double network_distance = distances[distance_index];
+                    ++distance_index;
+                    // double network_distance =
+                    //     getNetworkDistance(engine_working_data,
+                    //                        facade,
+                    //                        forward_heap,
+                    //                        reverse_heap,
+                    //                        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(