Add skeleton code for matching

data_structures/search_engine.hpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "search_engine_data.hpp"
 #include "../routing_algorithms/alternative_path.hpp"
 #include "../routing_algorithms/many_to_many.hpp"
+#include "../routing_algorithms/map_matching.hpp"
 #include "../routing_algorithms/shortest_path.hpp"
 
 #include <type_traits>
@@ -45,10 +46,14 @@ template <class DataFacadeT> class SearchEngine
     ShortestPathRouting<DataFacadeT> shortest_path;
     AlternativeRouting<DataFacadeT> alternative_path;
     ManyToManyRouting<DataFacadeT> distance_table;
+    MapMatching<DataFacadeT> map_matching;
 
     explicit SearchEngine(DataFacadeT *facade)
-        : facade(facade), shortest_path(facade, engine_working_data),
+        : facade(facade),
-          alternative_path(facade, engine_working_data), distance_table(facade, engine_working_data)
+          shortest_path(facade, engine_working_data),
+          alternative_path(facade, engine_working_data),
+          distance_table(facade, engine_working_data),
+          map_matching(facade, engine_working_data)
     {
         static_assert(!std::is_pointer<DataFacadeT>::value, "don't instantiate with ptr type");
         static_assert(std::is_object<DataFacadeT>::value,

data_structures/static_rtree.hpp

@@ -745,6 +745,7 @@ class StaticRTree
                 // check if it is smaller than what we had before
                 float current_ratio = 0.f;
                 FixedPointCoordinate foot_point_coordinate_on_segment;
+
                 // const float current_perpendicular_distance =
                 coordinate_calculation::perpendicular_distance_from_projected_coordinate(
                     m_coordinate_list->at(current_segment.u),
@@ -796,20 +797,24 @@ class StaticRTree
         return !result_phantom_node_vector.empty();
     }
 
-    // implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree
     bool IncrementalFindPhantomNodeForCoordinateWithDistance(
         const FixedPointCoordinate &input_coordinate,
         std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
-        const unsigned number_of_results,
+        const unsigned max_number_of_phantom_nodes,
-        const unsigned max_checked_segments = 4 * LEAF_NODE_SIZE)
+        const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
     {
-        std::vector<float> min_found_distances(number_of_results,
+        unsigned inspected_elements = 0;
-                                               std::numeric_limits<float>::max());
+        unsigned number_of_elements_from_big_cc = 0;
+        unsigned number_of_elements_from_tiny_cc = 0;
 
-        unsigned number_of_results_found_in_big_cc = 0;
+        unsigned pruned_elements = 0;
-        unsigned number_of_results_found_in_tiny_cc = 0;
 
-        unsigned inspected_segments = 0;
+        std::pair<double, double> projected_coordinate = {
+            mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION),
+            input_coordinate.lon / COORDINATE_PRECISION};
+
+        // upper bound pruning technique
+        upper_bound<float> pruning_bound(max_number_of_phantom_nodes);
 
         // initialize queue with root element
         std::priority_queue<IncrementalQueryCandidate> traversal_queue;
@@ -820,42 +825,43 @@ class StaticRTree
             const IncrementalQueryCandidate current_query_node = traversal_queue.top();
             traversal_queue.pop();
 
-            const float current_min_dist = min_found_distances[number_of_results - 1];
+            if (current_query_node.node.template is<TreeNode>())
-
+            { // current object is a tree node
-            if (current_query_node.min_dist > current_min_dist)
-            {
-                continue;
-            }
-
-            if (current_query_node.RepresentsTreeNode())
-            {
                 const TreeNode &current_tree_node =
                     current_query_node.node.template get<TreeNode>();
                 if (current_tree_node.child_is_on_disk)
                 {
                     LeafNode current_leaf_node;
                     LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
-                    // Add all objects from leaf into queue
+
-                    for (uint32_t i = 0; i < current_leaf_node.object_count; ++i)
+                    // current object represents a block on disk
+                    for (const auto i : osrm::irange(0u, current_leaf_node.object_count))
                     {
                         const auto &current_edge = current_leaf_node.objects[i];
-                        const float current_perpendicular_distance =
+                        const float current_perpendicular_distance = coordinate_calculation::
-                            coordinate_calculation::perpendicular_distance(
+                            perpendicular_distance_from_projected_coordinate(
                                 m_coordinate_list->at(current_edge.u),
-                                m_coordinate_list->at(current_edge.v), input_coordinate);
+                                m_coordinate_list->at(current_edge.v), input_coordinate,
+                                projected_coordinate);
                         // distance must be non-negative
-                        BOOST_ASSERT(0. <= current_perpendicular_distance);
+                        BOOST_ASSERT(0.f <= current_perpendicular_distance);
 
-                        if (current_perpendicular_distance < current_min_dist)
+                        if (pruning_bound.get() >= current_perpendicular_distance ||
+                            current_edge.is_in_tiny_cc())
                         {
+                            pruning_bound.insert(current_perpendicular_distance);
                             traversal_queue.emplace(current_perpendicular_distance, current_edge);
                         }
+                        else
+                        {
+                            ++pruned_elements;
+                        }
                     }
                 }
                 else
                 {
-                    // for each child mbr
+                    // for each child mbr get a lower bound and enqueue it
-                    for (uint32_t i = 0; i < current_tree_node.child_count; ++i)
+                    for (const auto i : osrm::irange(0u, current_tree_node.child_count))
                     {
                         const int32_t child_id = current_tree_node.children[i];
                         const TreeNode &child_tree_node = m_search_tree[child_id];
@@ -863,99 +869,89 @@ class StaticRTree
                             child_tree_node.minimum_bounding_rectangle;
                         const float lower_bound_to_element =
                             child_rectangle.GetMinDist(input_coordinate);
+                        BOOST_ASSERT(0.f <= lower_bound_to_element);
 
-                        // TODO - enough elements found, i.e. nearest distance > maximum distance?
+                        traversal_queue.emplace(lower_bound_to_element, child_tree_node);
-                        //        ie. some measure of 'confidence of accuracy'
-
-                        // check if it needs to be explored by mindist
-                        if (lower_bound_to_element < current_min_dist)
-                        {
-                            traversal_queue.emplace(lower_bound_to_element, child_tree_node);
-                        }
                     }
-                    // SimpleLogger().Write(logDEBUG) << "added " << current_tree_node.child_count
-                    // << " mbrs into queue of " << traversal_queue.size();
                 }
             }
             else
-            {
+            { // current object is a leaf node
-                ++inspected_segments;
+                ++inspected_elements;
                 // inspecting an actual road segment
                 const EdgeDataT &current_segment =
                     current_query_node.node.template get<EdgeDataT>();
 
-                // don't collect too many results from small components
+                // continue searching for the first segment from a big component
-                if (number_of_results_found_in_big_cc == number_of_results &&
+                if (number_of_elements_from_big_cc == 0 &&
-                    !current_segment.is_in_tiny_cc)
+                    number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes &&
-                {
+                    current_segment.is_in_tiny_cc())
-                    continue;
-                }
-
-                // don't collect too many results from big components
-                if (number_of_results_found_in_tiny_cc == number_of_results &&
-                    current_segment.is_in_tiny_cc)
                 {
                     continue;
                 }
 
                 // check if it is smaller than what we had before
-                float current_ratio = 0.;
+                float current_ratio = 0.f;
                 FixedPointCoordinate foot_point_coordinate_on_segment;
+
                 const float current_perpendicular_distance =
-                    coordinate_calculation::perpendicular_distance(
+                coordinate_calculation::perpendicular_distance_from_projected_coordinate(
-                        m_coordinate_list->at(current_segment.u),
+                    m_coordinate_list->at(current_segment.u),
-                        m_coordinate_list->at(current_segment.v), input_coordinate,
+                    m_coordinate_list->at(current_segment.v), input_coordinate,
-                        foot_point_coordinate_on_segment, current_ratio);
+                    projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
 
-                BOOST_ASSERT(0. <= current_perpendicular_distance);
+                // store phantom node in result vector
+                result_phantom_node_vector.emplace_back(
+                    PhantomNode( current_segment.forward_edge_based_node_id,
+                     current_segment.reverse_edge_based_node_id, current_segment.name_id,
+                     current_segment.forward_weight, current_segment.reverse_weight,
+                     current_segment.forward_offset, current_segment.reverse_offset,
+                     current_segment.packed_geometry_id, current_segment.component_id,
+                     foot_point_coordinate_on_segment, current_segment.fwd_segment_position,
+                     current_segment.forward_travel_mode, current_segment.backward_travel_mode),
+                    current_perpendicular_distance);
 
-                if ((current_perpendicular_distance < current_min_dist) &&
+                // Hack to fix rounding errors and wandering via nodes.
-                    !osrm::epsilon_compare(current_perpendicular_distance, current_min_dist))
+                FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first);
-                {
-                    // store phantom node in result vector
-                    result_phantom_node_vector.emplace_back(current_segment,
-                                                            foot_point_coordinate_on_segment,
-                                                            current_perpendicular_distance);
 
-                    // Hack to fix rounding errors and wandering via nodes.
+                // set forward and reverse weights on the phantom node
-                    FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back());
+                SetForwardAndReverseWeightsOnPhantomNode(current_segment,
+                                                         result_phantom_node_vector.back().first);
 
-                    // set forward and reverse weights on the phantom node
+                // update counts on what we found from which result class
-                    SetForwardAndReverseWeightsOnPhantomNode(current_segment,
+                if (current_segment.is_in_tiny_cc())
-                                                             result_phantom_node_vector.back());
+                { // found an element in tiny component
-
+                    ++number_of_elements_from_tiny_cc;
-                    // do we have results only in a small scc
+                }
-                    if (current_segment.is_in_tiny_cc)
+                else
-                    {
+                { // found an element in a big component
-                        ++number_of_results_found_in_tiny_cc;
+                    ++number_of_elements_from_big_cc;
-                    }
-                    else
-                    {
-                        // found an element in a large component
-                        min_found_distances[number_of_results_found_in_big_cc] =
-                            current_perpendicular_distance;
-                        ++number_of_results_found_in_big_cc;
-                        // SimpleLogger().Write(logDEBUG) << std::setprecision(8) <<
-                        // foot_point_coordinate_on_segment << " at " <<
-                        // current_perpendicular_distance;
-                    }
                 }
             }
 
-            // TODO add indicator to prune if maxdist > threshold
+            // stop the search by flushing the queue
-            if (number_of_results == number_of_results_found_in_big_cc ||
+            if ((result_phantom_node_vector.size() >= max_number_of_phantom_nodes &&
-                inspected_segments >= max_checked_segments)
+                 number_of_elements_from_big_cc > 0) ||
+                inspected_elements >= max_checked_elements)
             {
-                // SimpleLogger().Write(logDEBUG) << "flushing queue of " << traversal_queue.size()
-                // << " elements";
-                // work-around for traversal_queue.clear();
                 traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
             }
         }
+        // SimpleLogger().Write() << "result_phantom_node_vector.size(): " <<
+        // result_phantom_node_vector.size();
+        // SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes;
+        // SimpleLogger().Write() << "number_of_elements_from_big_cc: " <<
+        // number_of_elements_from_big_cc;
+        // SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " <<
+        // number_of_elements_from_tiny_cc;
+        // SimpleLogger().Write() << "inspected_elements: " << inspected_elements;
+        // SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements;
+        // SimpleLogger().Write() << "pruned_elements: " << pruned_elements;
 
         return !result_phantom_node_vector.empty();
     }
 
+
     bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                       PhantomNode &result_phantom_node,
                                       const unsigned zoom_level)

library/osrm_impl.cpp

@@ -42,6 +42,7 @@ class named_mutex;
 #include "../plugins/nearest.hpp"
 #include "../plugins/timestamp.hpp"
 #include "../plugins/viaroute.hpp"
+#include "../plugins/map_matching.hpp"
 #include "../server/data_structures/datafacade_base.hpp"
 #include "../server/data_structures/internal_datafacade.hpp"
 #include "../server/data_structures/shared_barriers.hpp"
@@ -81,6 +82,7 @@ OSRM_impl::OSRM_impl(libosrm_config &lib_config)
     RegisterPlugin(new HelloWorldPlugin());
     RegisterPlugin(new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
+    RegisterPlugin(new MapMatchingPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new TimestampPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
     RegisterPlugin(new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
 }

plugins/map_matching.hpp

@@ -0,0 +1,107 @@
+/*
+    open source routing machine
+    Copyright (C) Dennis Luxen, others 2010
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU AFFERO General Public License as published by
+the Free Software Foundation; either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU Affero General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+or see http://www.gnu.org/licenses/agpl.txt.
+ */
+
+#ifndef MAP_MATCHING_PLUGIN_H
+#define MAP_MATCHING_PLUGIN_H
+
+#include "plugin_base.hpp"
+
+#include "../algorithms/object_encoder.hpp"
+#include "../util/integer_range.hpp"
+#include "../data_structures/search_engine.hpp"
+#include "../routing_algorithms/map_matching.hpp"
+#include "../util/simple_logger.hpp"
+#include "../util/string_util.hpp"
+
+#include <cstdlib>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
+{
+  private:
+    std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
+
+  public:
+    MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade)
+    {
+        search_engine_ptr = std::make_shared<SearchEngine<DataFacadeT>>(facade);
+    }
+
+    virtual ~MapMatchingPlugin() { search_engine_ptr.reset(); }
+
+    const std::string GetDescriptor() const final { return descriptor_string; }
+
+    int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final
+    {
+        // check number of parameters
+
+        SimpleLogger().Write() << "1";
+        if (!check_all_coordinates(route_parameters.coordinates))
+        {
+            return 400;
+        }
+
+        SimpleLogger().Write() << "2";
+
+        InternalRouteResult raw_route;
+        Matching::CandidateLists candidate_lists;
+        candidate_lists.resize(route_parameters.coordinates.size());
+
+        SimpleLogger().Write() << "3";
+        // fetch  10 candidates for each given coordinate
+        for (const auto current_coordinate : osrm::irange<std::size_t>(0, candidate_lists.size()))
+        {
+            if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance(
+                    route_parameters.coordinates[current_coordinate],
+                    candidate_lists[current_coordinate],
+                    10))
+            {
+                return 400;
+            }
+
+            while (candidate_lists[current_coordinate].size() < 10)
+            {
+                // TODO: add dummy candidates, if any are missing
+                // TODO: add factory method to get an invalid PhantomNode/Distance pair
+            }
+        }
+        SimpleLogger().Write() << "4";
+
+        // call the actual map matching
+        search_engine_ptr->map_matching(10, candidate_lists, route_parameters.coordinates, raw_route);
+
+        if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length)
+        {
+            SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found";
+        }
+
+        return 200;
+    }
+
+  private:
+    std::string descriptor_string;
+    DataFacadeT *facade;
+};
+
+#endif /* MAP_MATCHING_PLUGIN_H */

routing_algorithms/map_matching.hpp

@@ -0,0 +1,317 @@
+/*
+    open source routing machine
+    Copyright (C) Dennis Luxen, others 2010
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU AFFERO General Public License as published by
+the Free Software Foundation; either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU Affero General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+or see http://www.gnu.org/licenses/agpl.txt.
+ */
+
+#ifndef MAP_MATCHING_H
+#define MAP_MATCHING_H
+
+#include "routing_base.hpp"
+
+#include "../data_structures/coordinate_calculation.hpp"
+#include "../util/simple_logger.hpp"
+#include "../util/container.hpp"
+
+#include <algorithm>
+#include <iomanip>
+#include <numeric>
+
+namespace Matching
+{
+typedef std::vector<std::pair<PhantomNode, double>> CandidateList;
+typedef std::vector<CandidateList> CandidateLists;
+typedef std::pair<PhantomNodes, double> PhantomNodesWithProbability;
+}
+
+// implements a hidden markov model map matching algorithm
+template <class DataFacadeT> class MapMatching final
+    : public BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>
+{
+    using super = BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>;
+    using QueryHeap = SearchEngineData::QueryHeap;
+    SearchEngineData &engine_working_data;
+
+    constexpr static const double sigma_z = 4.07;
+
+    constexpr double emission_probability(const double distance) const
+    {
+        return (1. / (std::sqrt(2. * M_PI) * sigma_z)) *
+               std::exp(-0.5 * std::pow((distance / sigma_z), 2.));
+    }
+
+    constexpr double log_probability(const double probability) const
+    {
+        return std::log2(probability);
+    }
+
+    // TODO: needs to be estimated from the input locations
+    //constexpr static const double beta = 1.;
+    // samples/min and beta
+    // 1 0.49037673
+    // 2 0.82918373
+    // 3 1.24364564
+    // 4 1.67079581
+    // 5 2.00719298
+    // 6 2.42513007
+    // 7 2.81248831
+    // 8 3.15745473
+    // 9 3.52645392
+    // 10 4.09511775
+    // 11 4.67319795
+    // 21 12.55107715
+    // 12 5.41088180
+    // 13 6.47666590
+    // 14 6.29010734
+    // 15 7.80752112
+    // 16 8.09074504
+    // 17 8.08550528
+    // 18 9.09405065
+    // 19 11.09090603
+    // 20 11.87752824
+    // 21 12.55107715
+    // 22 15.82820829
+    // 23 17.69496773
+    // 24 18.07655652
+    // 25 19.63438911
+    // 26 25.40832185
+    // 27 23.76001877
+    // 28 28.43289797
+    // 29 32.21683062
+    // 30 34.56991141
+
+    constexpr double transition_probability(const float d_t, const float beta) const
+    {
+        return (1. / beta) * std::exp(-d_t / beta);
+    }
+
+    // deprecated
+    // translates a distance into how likely it is an input
+    // double DistanceToProbability(const double distance) const
+    // {
+    //     if (0. > distance)
+    //     {
+    //         return 0.;
+    //     }
+    //     return 1. - 1. / (1. + exp((-distance + 35.) / 6.));
+    // }
+
+    double get_beta(const unsigned state_size,
+                    const Matching::CandidateLists &timestamp_list,
+                    const std::vector<FixedPointCoordinate> coordinate_list) const
+    {
+        std::vector<double> d_t_list, median_select_d_t_list;
+        for (auto t = 1; t < timestamp_list.size(); ++t)
+        {
+            for (auto s = 0; s < state_size; ++s)
+            {
+                d_t_list.push_back(get_distance_difference(coordinate_list[t - 1],
+                                                           coordinate_list[t],
+                                                           timestamp_list[t - 1][s].first,
+                                                           timestamp_list[t][s].first));
+                median_select_d_t_list.push_back(d_t_list.back());
+            }
+        }
+
+        std::nth_element(median_select_d_t_list.begin(),
+                         median_select_d_t_list.begin() + median_select_d_t_list.size() / 2,
+                         median_select_d_t_list.end());
+        const auto median_d_t = median_select_d_t_list[median_select_d_t_list.size() / 2];
+
+        return (1. / std::log(2)) * median_d_t;
+    }
+
+
+    double get_distance_difference(const FixedPointCoordinate &location1,
+                                   const FixedPointCoordinate &location2,
+                                   const PhantomNode &source_phantom,
+                                   const PhantomNode &target_phantom) const
+    {
+        // great circle distance of two locations - median/avg dist table(candidate list1/2)
+        const EdgeWeight network_distance = get_network_distance(source_phantom, target_phantom);
+        const auto great_circle_distance =
+            coordinate_calculation::great_circle_distance(location1, location2);
+
+        if (great_circle_distance > network_distance)
+        {
+            return great_circle_distance - network_distance;
+        }
+        return network_distance - great_circle_distance;
+    }
+
+    EdgeWeight get_network_distance(const PhantomNode &source_phantom,
+                                    const PhantomNode &target_phantom) const
+    {
+        EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
+        NodeID middle_node = SPECIAL_NODEID;
+        EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset());
+        edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset());
+
+        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
+        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
+
+        QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
+        QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
+
+        if (source_phantom.forward_node_id != SPECIAL_NODEID)
+        {
+            forward_heap.Insert(source_phantom.forward_node_id,
+                                -source_phantom.GetForwardWeightPlusOffset(),
+                                source_phantom.forward_node_id);
+        }
+        if (source_phantom.reverse_node_id != SPECIAL_NODEID)
+        {
+            forward_heap.Insert(source_phantom.reverse_node_id,
+                                -source_phantom.GetReverseWeightPlusOffset(),
+                                source_phantom.reverse_node_id);
+        }
+
+        if (target_phantom.forward_node_id != SPECIAL_NODEID)
+        {
+            reverse_heap.Insert(target_phantom.forward_node_id,
+                                target_phantom.GetForwardWeightPlusOffset(),
+                                target_phantom.forward_node_id);
+        }
+        if (target_phantom.reverse_node_id != SPECIAL_NODEID)
+        {
+            reverse_heap.Insert(target_phantom.reverse_node_id,
+                                target_phantom.GetReverseWeightPlusOffset(),
+                                target_phantom.reverse_node_id);
+        }
+
+        // search from s and t till new_min/(1+epsilon) > length_of_shortest_path
+        while (0 < (forward_heap.Size() + reverse_heap.Size()))
+        {
+            if (0 < forward_heap.Size())
+            {
+                super::RoutingStep(
+                    forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, true);
+            }
+            if (0 < reverse_heap.Size())
+            {
+                super::RoutingStep(
+                    reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, false);
+            }
+        }
+        return upper_bound;
+    }
+
+  public:
+    MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data)
+        : super(facade), engine_working_data(engine_working_data)
+    {
+    }
+
+    void operator()(const unsigned state_size,
+                    const Matching::CandidateLists &timestamp_list,
+                    const std::vector<FixedPointCoordinate> coordinate_list,
+                    InternalRouteResult &raw_route_data) const
+    {
+        BOOST_ASSERT(state_size != std::numeric_limits<unsigned>::max());
+        BOOST_ASSERT(state_size != 0);
+        SimpleLogger().Write() << "matching starts with " << timestamp_list.size() << " locations";
+
+        SimpleLogger().Write() << "state_size: " << state_size;
+
+        std::vector<std::vector<double>> viterbi(state_size,
+                                                 std::vector<double>(timestamp_list.size() + 1, 0));
+        std::vector<std::vector<std::size_t>> parent(
+            state_size, std::vector<std::size_t>(timestamp_list.size() + 1, 0));
+
+        SimpleLogger().Write() << "a";
+
+        for (auto s = 0; s < state_size; ++s)
+        {
+            SimpleLogger().Write() << "initializing s: " << s << "/" << state_size;
+            SimpleLogger().Write()
+                << " distance: " << timestamp_list[0][s].second << " at "
+                << timestamp_list[0][s].first.location << " prob " << std::setprecision(10)
+                << emission_probability(timestamp_list[0][s].second) << " logprob "
+                << log_probability(emission_probability(timestamp_list[0][s].second));
+            // TODO: implement
+            const double emission_pr = 0.;
+            viterbi[s][0] = emission_pr;
+            parent[s][0] = s;
+        }
+        SimpleLogger().Write() << "b";
+
+        // attention, this call is relatively expensive
+        const auto beta = get_beta(state_size, timestamp_list, coordinate_list);
+
+        for (auto t = 1; t < timestamp_list.size(); ++t)
+        {
+            // compute d_t for this timestamp and the next one
+            for (auto s = 0; s < state_size; ++s)
+            {
+                for (auto s_prime = 0; s_prime < state_size; ++s_prime)
+                {
+                    // how likely is candidate s_prime at time t to be emitted?
+                    const double emission_pr = emission_probability(timestamp_list[t][s_prime].second);
+
+                    // get distance diff between loc1/2 and locs/s_prime
+                    const auto d_t = get_distance_difference(coordinate_list[t-1],
+                                                             coordinate_list[t],
+                                                             timestamp_list[t-1][s].first,
+                                                             timestamp_list[t][s_prime].first);
+
+                    // plug probabilities together. TODO: change to addition for logprobs
+                    const double transition_pr = transition_probability(beta, d_t);
+                    const double new_value = viterbi[s][t] * emission_pr * transition_pr;
+                    if (new_value > viterbi[s_prime][t])
+                    {
+                        viterbi[s_prime][t] = new_value;
+                        parent[s_prime][t] = s;
+                    }
+                }
+            }
+        }
+        SimpleLogger().Write() << "c";
+        SimpleLogger().Write() << "timestamps: " << timestamp_list.size();
+        const auto number_of_timestamps = timestamp_list.size();
+        const auto max_element_iter = std::max_element(viterbi[number_of_timestamps].begin(),
+                                                       viterbi[number_of_timestamps].end());
+        auto parent_index = std::distance(max_element_iter, viterbi[number_of_timestamps].begin());
+        std::deque<std::size_t> reconstructed_indices;
+
+        SimpleLogger().Write() << "d";
+
+        for (auto i = number_of_timestamps - 1; i > 0; --i)
+        {
+            SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ;
+            reconstructed_indices.push_front(parent_index);
+            parent_index = parent[parent_index][i];
+        }
+        SimpleLogger().Write() << "[0] parent: " << parent_index;
+        reconstructed_indices.push_front(parent_index);
+
+        SimpleLogger().Write() << "e";
+
+        for (auto i = 0; i < reconstructed_indices.size(); ++i)
+        {
+            auto location_index = reconstructed_indices[i];
+            SimpleLogger().Write() << std::setprecision(8) << "location " << coordinate_list[i] << " to " << timestamp_list[i][location_index].first.location;
+        }
+
+        SimpleLogger().Write() << "f, done";
+    }
+};
+
+//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM GIS
+
+#endif /* MAP_MATCHING_H */

server/data_structures/datafacade_base.hpp

@@ -105,6 +105,11 @@ template <class EdgeDataT> class BaseDataFacade
     IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
                                             PhantomNode &resulting_phantom_node) = 0;
 
+    virtual bool
+    IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
+                                                        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
+                                                        const unsigned number_of_results) = 0;
+
     virtual unsigned GetCheckSum() const = 0;
 
     virtual unsigned GetNameIndexFromEdgeID(const unsigned id) const = 0;

server/data_structures/internal_datafacade.hpp

@@ -398,6 +398,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
             BOOST_ASSERT(!resulting_phantom_node_vector.empty());
             resulting_phantom_node = resulting_phantom_node_vector.front();
         }
+
         return result;
     }
 
@@ -415,6 +416,20 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
             input_coordinate, resulting_phantom_node_vector, number_of_results);
     }
 
+    bool
+    IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
+                                                        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
+                                                        const unsigned number_of_results) final
+    {
+        if (!m_static_rtree.get())
+        {
+            LoadRTree();
+        }
+
+        return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
+            input_coordinate, resulting_phantom_node_vector, number_of_results);
+    }
+
     unsigned GetCheckSum() const override final { return m_check_sum; }
 
     unsigned GetNameIndexFromEdgeID(const unsigned id) const override final

server/data_structures/shared_datafacade.hpp

@@ -386,6 +386,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
             BOOST_ASSERT(!resulting_phantom_node_vector.empty());
             resulting_phantom_node = resulting_phantom_node_vector.front();
         }
+
         return result;
     }
 
@@ -403,6 +404,20 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
             input_coordinate, resulting_phantom_node_vector, number_of_results);
     }
 
+    bool
+    IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
+                                                        std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
+                                                        const unsigned number_of_results) final
+    {
+        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
+        {
+            LoadRTree();
+        }
+
+        return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
+            input_coordinate, resulting_phantom_node_vector, number_of_results);
+    }
+
     unsigned GetCheckSum() const override final { return m_check_sum; }
 
     unsigned GetNameIndexFromEdgeID(const unsigned id) const override final