First compiling version of map_match plugin

include/engine/api/base_api.hpp

@@ -26,7 +26,7 @@ class BaseAPI
     {
     }
 
-    virtual util::json::Array
+    util::json::Array
     MakeWaypoints(const std::vector<PhantomNodes> &segment_end_coordinates) const
     {
         BOOST_ASSERT(parameters.coordinates.size() > 0);
@@ -47,7 +47,7 @@ class BaseAPI
     }
 
   protected:
-    virtual util::json::Object MakeWaypoint(const util::FixedPointCoordinate input_coordinate,
+    util::json::Object MakeWaypoint(const util::FixedPointCoordinate input_coordinate,
                                             const PhantomNode &phantom) const
     {
         return json::makeWaypoint(phantom.location, facade.get_name_for_id(phantom.name_id),

include/engine/api/match_api.hpp

@@ -0,0 +1,116 @@
+#ifndef ENGINE_API_MATCH_HPP
+#define ENGINE_API_MATCH_HPP
+
+#include "engine/api/route_api.hpp"
+#include "engine/api/match_parameters.hpp"
+
+#include "engine/datafacade/datafacade_base.hpp"
+
+#include "engine/internal_route_result.hpp"
+#include "engine/map_matching/sub_matching.hpp"
+
+#include "util/integer_range.hpp"
+
+namespace osrm
+{
+namespace engine
+{
+namespace api
+{
+
+class MatchAPI final : public RouteAPI
+{
+  public:
+    MatchAPI(const datafacade::BaseDataFacade &facade_, const MatchParameters &parameters_)
+        : RouteAPI(facade_, parameters_), parameters(parameters_)
+    {
+    }
+
+    void MakeResponse(const std::vector<map_matching::SubMatching> &sub_matchings,
+                      const std::vector<InternalRouteResult> &sub_routes,
+                      util::json::Object &response) const
+    {
+        auto number_of_routes = sub_matchings.size();
+        util::json::Array routes;
+        routes.values.reserve(number_of_routes);
+        BOOST_ASSERT(sub_matchings.size() == sub_routes.size());
+        for (auto index : util::irange<std::size_t>(0UL, sub_matchings.size()))
+        {
+            auto route = MakeRoute(sub_routes[index].segment_end_coordinates,
+                                   sub_routes[index].unpacked_path_segments,
+                                   sub_routes[index].source_traversed_in_reverse,
+                                   sub_routes[index].target_traversed_in_reverse);
+            route.values["confidence"] = sub_matchings[index].confidence;
+        }
+        response.values["tracepoints"] = MakeTracepoints(sub_matchings);
+        response.values["routes"] = std::move(routes);
+        response.values["code"] = "ok";
+    }
+
+  protected:
+    // FIXME this logic is a little backwards. We should change the output format of the
+    // map_matching
+    // routing algorithm to be easier to consume here.
+    util::json::Array
+    MakeTracepoints(const std::vector<map_matching::SubMatching> &sub_matchings) const
+    {
+        util::json::Array waypoints;
+        waypoints.values.reserve(parameters.coordinates.size());
+
+        struct MatchingIndex
+        {
+            MatchingIndex() = default;
+            MatchingIndex(unsigned sub_matching_index_, unsigned point_index_)
+              : sub_matching_index(sub_matching_index_), point_index(point_index_)
+            {
+            }
+
+            unsigned sub_matching_index = std::numeric_limits<unsigned>::max();
+            unsigned point_index = std::numeric_limits<unsigned>::max();
+
+            bool NotMatched()
+            {
+                return sub_matching_index == std::numeric_limits<unsigned>::max() &&
+                       point_index == std::numeric_limits<unsigned>::max();
+            }
+        };
+
+        std::vector<MatchingIndex> trace_idx_to_matching_idx(parameters.coordinates.size());
+        for (auto sub_matching_index :
+             util::irange(0u, static_cast<unsigned>(sub_matchings.size())))
+        {
+            for (auto point_index : util::irange(
+                     0u, static_cast<unsigned>(sub_matchings[sub_matching_index].indices.size())))
+            {
+                trace_idx_to_matching_idx[sub_matchings[sub_matching_index].indices[point_index]] =
+                    MatchingIndex{sub_matching_index, point_index};
+            }
+        }
+
+        for (auto trace_index : util::irange(0UL, parameters.coordinates.size()))
+        {
+            auto matching_index = trace_idx_to_matching_idx[trace_index];
+            if (matching_index.NotMatched())
+            {
+                waypoints.values.push_back(util::json::Null());
+                continue;
+            }
+            const auto &phantom =
+                sub_matchings[matching_index.sub_matching_index].nodes[matching_index.point_index];
+            auto waypoint = BaseAPI::MakeWaypoint(parameters.coordinates[trace_index], phantom);
+            waypoint.values["matchings_index"] = matching_index.sub_matching_index;
+            waypoint.values["waypoint_index"] = matching_index.point_index;
+            waypoints.values.push_back(std::move(waypoint));
+        }
+
+        return waypoints;
+    }
+
+    const MatchParameters &parameters;
+};
+
+} // ns api
+} // ns engine
+} // ns osrm
+
+#endif

include/engine/api/match_parameters.hpp

@@ -14,10 +14,10 @@ namespace api
 
 struct MatchParameters : public RouteParameters
 {
-    std::vector<boost::optional<unsigned>> timestamps;
+    std::vector<unsigned> timestamps;
     bool IsValid() const
     {
-        return timestamps.empty() || timestamps.size() == coordinates.size();
+        return RouteParameters::IsValid() && (timestamps.empty() || timestamps.size() == coordinates.size());
     }
 };
 

include/engine/api/route_api.hpp

@@ -24,7 +24,7 @@ namespace engine
 namespace api
 {
 
-class RouteAPI final : public BaseAPI
+class RouteAPI : public BaseAPI
 {
   public:
     RouteAPI(const datafacade::BaseDataFacade &facade_, const RouteParameters &parameters_)
@@ -32,8 +32,7 @@ class RouteAPI final : public BaseAPI
     {
     }
 
-    virtual void MakeResponse(const InternalRouteResult &raw_route,
-                              util::json::Object &response) const
+    void MakeResponse(const InternalRouteResult &raw_route, util::json::Object &response) const
     {
         auto number_of_routes = raw_route.has_alternative() ? 2UL : 1UL;
         util::json::Array routes;
@@ -67,11 +66,10 @@ class RouteAPI final : public BaseAPI
         return json::makeGeoJSONLineString(begin, end);
     }
 
-    virtual util::json::Object
-    MakeRoute(const std::vector<PhantomNodes> &segment_end_coordinates,
-              const std::vector<std::vector<PathData>> &unpacked_path_segments,
-              const std::vector<bool> &source_traversed_in_reverse,
-              const std::vector<bool> &target_traversed_in_reverse) const
+    util::json::Object MakeRoute(const std::vector<PhantomNodes> &segment_end_coordinates,
+                                 const std::vector<std::vector<PathData>> &unpacked_path_segments,
+                                 const std::vector<bool> &source_traversed_in_reverse,
+                                 const std::vector<bool> &target_traversed_in_reverse) const
     {
         std::vector<guidance::RouteLeg> legs;
         std::vector<guidance::LegGeometry> leg_geometries;

include/engine/datafacade/datafacade_base.hpp

@@ -86,8 +86,12 @@ class BaseDataFacade
     virtual std::vector<PhantomNodeWithDistance>
     NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
                                const float max_distance,
-                               const int bearing = 0,
-                               const int bearing_range = 180) = 0;
+                               const int bearing,
+                               const int bearing_range) = 0;
+
+    virtual std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
+                               const float max_distance) = 0;
 
     virtual std::vector<PhantomNodeWithDistance>
     NearestPhantomNodes(const util::FixedPointCoordinate input_coordinate,

include/engine/datafacade/internal_datafacade.hpp

@@ -354,11 +354,24 @@ class InternalDataFacade final : public BaseDataFacade
         return m_geospatial_query->Search(bbox);
     }
 
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
+                               const float max_distance) override final
+    {
+        if (!m_static_rtree.get())
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance);
+    }
+
     std::vector<PhantomNodeWithDistance>
     NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
                                const float max_distance,
-                               const int bearing = 0,
-                               const int bearing_range = 180) override final
+                               const int bearing,
+                               const int bearing_range) override final
     {
         if (!m_static_rtree.get())
         {

include/engine/datafacade/shared_datafacade.hpp

@@ -421,11 +421,24 @@ class SharedDataFacade final : public BaseDataFacade
         return m_geospatial_query->Search(bbox);
     }
 
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
+                               const float max_distance) override final
+    {
+        if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
+        {
+            LoadRTree();
+            BOOST_ASSERT(m_geospatial_query.get());
+        }
+
+        return m_geospatial_query->NearestPhantomNodesInRange(input_coordinate, max_distance);
+    }
+
     std::vector<PhantomNodeWithDistance>
     NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
                                const float max_distance,
-                               const int bearing = 0,
-                               const int bearing_range = 180) override final
+                               const int bearing,
+                               const int bearing_range) override final
     {
         if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
         {

include/engine/engine.hpp

@@ -74,7 +74,7 @@ class Engine final
     std::unique_ptr<plugins::TablePlugin> table_plugin;
     std::unique_ptr<plugins::NearestPlugin> nearest_plugin;
     // std::unique_ptr<plugins::TripPlugin> trip_plugin;
-    // std::unique_ptr<plugins::MatchPlugin> match_plugin;
+    std::unique_ptr<plugins::MatchPlugin> match_plugin;
 
     std::unique_ptr<datafacade::BaseDataFacade> query_data_facade;
 };

include/engine/geospatial_query.hpp

@@ -38,13 +38,30 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
         return rtree.SearchInBox(bbox);
     }
 
+    // Returns nearest PhantomNodes in the given bearing range within max_distance.
+    // Does not filter by small/big component!
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
+                               const double max_distance)
+    {
+        auto results =
+            rtree.Nearest(input_coordinate,
+                          [] (const EdgeData &) { return std::make_pair(true, true); },
+                          [max_distance](const std::size_t, const double min_dist)
+                          {
+                              return min_dist > max_distance;
+                          });
+
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
     // Returns nearest PhantomNodes in the given bearing range within max_distance.
     // Does not filter by small/big component!
     std::vector<PhantomNodeWithDistance>
     NearestPhantomNodesInRange(const util::FixedPointCoordinate input_coordinate,
                                const double max_distance,
-                               const int bearing = 0,
-                               const int bearing_range = 180)
+                               const int bearing,
+                               const int bearing_range)
     {
         auto results =
             rtree.Nearest(input_coordinate,

include/engine/map_matching/hidden_markov_model.hpp

@@ -52,23 +52,21 @@ template <class CandidateLists> struct HiddenMarkovModel
 {
     std::vector<std::vector<double>> viterbi;
     std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
-    std::vector<std::vector<float>> path_lengths;
+    std::vector<std::vector<float>> path_distances;
     std::vector<std::vector<bool>> pruned;
-    std::vector<std::vector<bool>> suspicious;
     std::vector<bool> breakage;
 
     const CandidateLists &candidates_list;
-    const EmissionLogProbability &emission_log_probability;
+    const std::vector<std::vector<double>> &emission_log_probabilities;
 
     HiddenMarkovModel(const CandidateLists &candidates_list,
-                      const EmissionLogProbability &emission_log_probability)
+                      const std::vector<std::vector<double>> &emission_log_probabilities)
         : breakage(candidates_list.size()), candidates_list(candidates_list),
-          emission_log_probability(emission_log_probability)
+          emission_log_probabilities(emission_log_probabilities)
     {
         viterbi.resize(candidates_list.size());
         parents.resize(candidates_list.size());
-        path_lengths.resize(candidates_list.size());
-        suspicious.resize(candidates_list.size());
+        path_distances.resize(candidates_list.size());
         pruned.resize(candidates_list.size());
         breakage.resize(candidates_list.size());
         for (const auto i : util::irange<std::size_t>(0u, candidates_list.size()))
@@ -79,8 +77,7 @@ template <class CandidateLists> struct HiddenMarkovModel
             {
                 viterbi[i].resize(num_candidates);
                 parents[i].resize(num_candidates);
-                path_lengths[i].resize(num_candidates);
-                suspicious[i].resize(num_candidates);
+                path_distances[i].resize(num_candidates);
                 pruned[i].resize(num_candidates);
             }
         }
@@ -90,15 +87,14 @@ template <class CandidateLists> struct HiddenMarkovModel
 
     void clear(std::size_t initial_timestamp)
     {
-        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_lengths.size() &&
-                     path_lengths.size() == pruned.size() && pruned.size() == breakage.size());
+        BOOST_ASSERT(viterbi.size() == parents.size() && parents.size() == path_distances.size() &&
+                     path_distances.size() == pruned.size() && pruned.size() == breakage.size());
 
         for (const auto t : util::irange(initial_timestamp, viterbi.size()))
         {
             std::fill(viterbi[t].begin(), viterbi[t].end(), IMPOSSIBLE_LOG_PROB);
             std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0u, 0u));
-            std::fill(path_lengths[t].begin(), path_lengths[t].end(), 0);
-            std::fill(suspicious[t].begin(), suspicious[t].end(), true);
+            std::fill(path_distances[t].begin(), path_distances[t].end(), 0);
             std::fill(pruned[t].begin(), pruned[t].end(), true);
         }
         std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
@@ -113,11 +109,9 @@ template <class CandidateLists> struct HiddenMarkovModel
 
             for (const auto s : util::irange<std::size_t>(0u, viterbi[initial_timestamp].size()))
             {
-                viterbi[initial_timestamp][s] =
-                    emission_log_probability(candidates_list[initial_timestamp][s].distance);
+                viterbi[initial_timestamp][s] = emission_log_probabilities[initial_timestamp][s];
                 parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
                 pruned[initial_timestamp][s] = viterbi[initial_timestamp][s] < MINIMAL_LOG_PROB;
-                suspicious[initial_timestamp][s] = false;
 
                 breakage[initial_timestamp] =
                     breakage[initial_timestamp] && pruned[initial_timestamp][s];

include/engine/map_matching/matching_confidence.hpp

@@ -0,0 +1,58 @@
+#ifndef ENGINE_MAP_MATCHING_CONFIDENCE_HPP
+#define ENGINE_MAP_MATCHING_CONFIDENCE_HPP
+
+#include "engine/map_matching/bayes_classifier.hpp"
+
+#include <cmath>
+
+namespace osrm
+{
+namespace engine
+{
+namespace map_matching
+{
+
+struct MatchingConfidence
+{
+private:
+    using ClassifierT = BayesClassifier<LaplaceDistribution, LaplaceDistribution, double>;
+    using TraceClassification = ClassifierT::ClassificationT;
+
+public:
+    MatchingConfidence()
+        : // the values were derived from fitting a laplace distribution
+          // to the values of manually classified traces
+          classifier(map_matching::LaplaceDistribution(0.005986, 0.016646),
+                     map_matching::LaplaceDistribution(0.054385, 0.458432),
+                     0.696774) // valid apriori probability
+    {
+    }
+
+    double operator()(const float trace_length, const float matched_length) const
+    {
+        const double distance_feature = -std::log(trace_length) + std::log(matched_length);
+
+        // matched to the same point
+        if (!std::isfinite(distance_feature))
+        {
+            return 0;
+        }
+
+        const auto label_with_confidence = classifier.classify(distance_feature);
+        if (label_with_confidence.first == ClassifierT::ClassLabel::POSITIVE)
+        {
+            return label_with_confidence.second;
+        }
+
+        BOOST_ASSERT(label_with_confidence.first == ClassifierT::ClassLabel::NEGATIVE);
+        return 1 - label_with_confidence.second;
+    }
+private:
+
+    ClassifierT classifier;
+};
+}
+}
+}
+
+#endif

include/engine/map_matching/sub_matching.hpp

@@ -0,0 +1,26 @@
+#ifndef MAP_MATCHING_SUB_MATCHING_HPP
+#define MAP_MATCHING_SUB_MATCHING_HPP
+
+#include "engine/phantom_node.hpp"
+
+#include <vector>
+
+namespace osrm
+{
+namespace engine
+{
+namespace map_matching
+{
+
+struct SubMatching
+{
+    std::vector<PhantomNode> nodes;
+    std::vector<unsigned> indices;
+    double confidence;
+};
+
+}
+}
+}
+
+#endif

include/engine/plugins/match.hpp

@@ -6,6 +6,7 @@
 
 #include "engine/map_matching/bayes_classifier.hpp"
 #include "engine/routing_algorithms/map_matching.hpp"
+#include "engine/routing_algorithms/shortest_path.hpp"
 #include "util/json_util.hpp"
 
 #include <vector>
@@ -17,33 +18,28 @@ namespace engine
 namespace plugins
 {
 
-class MatchingPlugin : public BasePlugin
+class MatchPlugin : public BasePlugin
 {
-    using SubMatching = routing_algorithms::SubMatching;
+  public:
+    using SubMatching = map_matching::SubMatching;
     using SubMatchingList = routing_algorithms::SubMatchingList;
     using CandidateLists = routing_algorithms::CandidateLists;
-    using ClassifierT = map_matching::BayesClassifier<map_matching::LaplaceDistribution,
-                                                      map_matching::LaplaceDistribution,
-                                                      double>;
-    using TraceClassification = ClassifierT::ClassificationT;
+    static const constexpr double DEFAULT_GPS_PRECISION = 5;
+    static const constexpr double RADIUS_MULTIPLIER = 3;
 
-  public:
-    MatchingPlugin(datafacade::BaseDataFacade &facade_, const int max_locations_map_matching)
-        : BasePlugin(facade_),
-          max_locations_map_matching(max_locations_map_matching),
-          // the values were derived from fitting a laplace distribution
-          // to the values of manually classified traces
-          classifier(map_matching::LaplaceDistribution(0.005986, 0.016646),
-                     map_matching::LaplaceDistribution(0.054385, 0.458432),
-                     0.696774) // valid apriori probability
+    MatchPlugin(datafacade::BaseDataFacade &facade_, const int max_locations_map_matching)
+        : BasePlugin(facade_), map_matching(&facade_, heaps, DEFAULT_GPS_PRECISION),
+          shortest_path(&facade_, heaps), max_locations_map_matching(max_locations_map_matching)
     {
     }
 
-    Status HandleRequest(const MatchParameters &parameters, util::json::Object &json_result);
+    Status HandleRequest(const api::MatchParameters &parameters, util::json::Object &json_result);
 
   private:
+    SearchEngineData heaps;
+    routing_algorithms::MapMatching<datafacade::BaseDataFacade> map_matching;
+    routing_algorithms::ShortestPathRouting<datafacade::BaseDataFacade> shortest_path;
     int max_locations_map_matching;
-    ClassifierT classifier;
 };
 }
 }

include/engine/plugins/plugin_base.hpp

@@ -7,6 +7,7 @@
 #include "engine/status.hpp"
 
 #include "util/coordinate.hpp"
+#include "util/coordinate_calculation.hpp"
 #include "util/json_container.hpp"
 #include "util/integer_range.hpp"
 
@@ -30,10 +31,10 @@ class BasePlugin
     bool CheckAllCoordinates(const std::vector<util::FixedPointCoordinate> &coordinates)
     {
         return !std::any_of(std::begin(coordinates), std::end(coordinates),
-                           [](const util::FixedPointCoordinate &coordinate)
-                           {
-                               return !coordinate.IsValid();
-                           });
+                            [](const util::FixedPointCoordinate &coordinate)
+                            {
+                                return !coordinate.IsValid();
+                            });
     }
 
     Status Error(const std::string &code,
@@ -109,6 +110,46 @@ class BasePlugin
         return snapped_phantoms;
     }
 
+    // Falls back to default_radius for non-set radii
+    std::vector<std::vector<PhantomNodeWithDistance>>
+    GetPhantomNodesInRange(const api::BaseParameters &parameters,
+                           const std::vector<double> radiuses) const
+    {
+        std::vector<std::vector<PhantomNodeWithDistance>> phantom_nodes(
+            parameters.coordinates.size());
+        BOOST_ASSERT(parameters.radiuses.size() == parameters.coordinates.size());
+
+        const bool use_hints = !parameters.hints.empty();
+        const bool use_bearings = !parameters.bearings.empty();
+
+        for (const auto i : util::irange<std::size_t>(0, parameters.coordinates.size()))
+        {
+            if (use_hints && parameters.hints[i] &&
+                parameters.hints[i]->IsValid(parameters.coordinates[i], facade))
+            {
+                phantom_nodes[i].push_back(PhantomNodeWithDistance{
+                    parameters.hints[i]->phantom,
+                    util::coordinate_calculation::haversineDistance(
+                        parameters.coordinates[i], parameters.hints[i]->phantom.location),
+                });
+                continue;
+            }
+            if (use_bearings && parameters.bearings[i])
+            {
+                phantom_nodes[i] = facade.NearestPhantomNodesInRange(
+                    parameters.coordinates[i], radiuses[i], parameters.bearings[i]->bearing,
+                    parameters.bearings[i]->range);
+            }
+            else
+            {
+                phantom_nodes[i] =
+                    facade.NearestPhantomNodesInRange(parameters.coordinates[i], radiuses[i]);
+            }
+        }
+
+        return phantom_nodes;
+    }
+
     std::vector<PhantomNodePair> GetPhantomNodes(const api::BaseParameters &parameters)
     {
         std::vector<PhantomNodePair> phantom_node_pairs(parameters.coordinates.size());
@@ -138,11 +179,11 @@ class BasePlugin
                             parameters.bearings[i]->bearing, parameters.bearings[i]->range);
                 }
                 else
+                {
                     phantom_node_pairs[i] =
                         facade.NearestPhantomNodeWithAlternativeFromBigComponent(
                             parameters.coordinates[i], parameters.bearings[i]->bearing,
                             parameters.bearings[i]->range);
-                {
                 }
             }
             else
@@ -154,10 +195,10 @@ class BasePlugin
                             parameters.coordinates[i], *parameters.radiuses[i]);
                 }
                 else
+                {
                     phantom_node_pairs[i] =
                         facade.NearestPhantomNodeWithAlternativeFromBigComponent(
                             parameters.coordinates[i]);
-                {
                 }
             }
 

include/engine/routing_algorithms/map_matching.hpp

@@ -3,9 +3,13 @@
 
 #include "engine/routing_algorithms/routing_base.hpp"
 
-#include "util/coordinate_calculation.hpp"
 #include "engine/map_matching/hidden_markov_model.hpp"
+#include "engine/map_matching/sub_matching.hpp"
+#include "engine/map_matching/matching_confidence.hpp"
+
+#include "util/coordinate_calculation.hpp"
 #include "util/json_logger.hpp"
+#include "util/for_each_pair.hpp"
 #include "util/matching_debug_info.hpp"
 
 #include <cstddef>
@@ -24,22 +28,14 @@ namespace engine
 namespace routing_algorithms
 {
 
-struct SubMatching
-{
-    std::vector<PhantomNode> nodes;
-    std::vector<unsigned> indices;
-    double length;
-    double confidence;
-};
-
 using CandidateList = std::vector<PhantomNodeWithDistance>;
 using CandidateLists = std::vector<CandidateList>;
 using HMM = map_matching::HiddenMarkovModel<CandidateLists>;
-using SubMatchingList = std::vector<SubMatching>;
+using SubMatchingList = std::vector<map_matching::SubMatching>;
 
 constexpr static const unsigned MAX_BROKEN_STATES = 10;
 constexpr static const double MAX_SPEED = 180 / 3.6; // 180km -> m/s
-constexpr static const unsigned SUSPICIOUS_DISTANCE_DELTA = 100;
+static const constexpr double MATCHING_BETA = 10;
 constexpr static const double MAX_DISTANCE_DELTA = 2000.;
 
 // implements a hidden markov model map matching algorithm
@@ -49,6 +45,9 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
     using super = BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>;
     using QueryHeap = SearchEngineData::QueryHeap;
     SearchEngineData &engine_working_data;
+    map_matching::EmissionLogProbability default_emission_log_probability;
+    map_matching::TransitionLogProbability transition_log_probability;
+    map_matching::MatchingConfidence confidence;
 
     unsigned GetMedianSampleTime(const std::vector<unsigned> &timestamps) const
     {
@@ -66,18 +65,23 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
     }
 
   public:
-    MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data)
-        : super(facade), engine_working_data(engine_working_data)
+    MapMatching(DataFacadeT *facade,
+                SearchEngineData &engine_working_data,
+                const double default_gps_precision)
+        : super(facade), engine_working_data(engine_working_data),
+          default_emission_log_probability(default_gps_precision),
+          transition_log_probability(MATCHING_BETA)
     {
     }
 
-    void operator()(const CandidateLists &candidates_list,
-                    const std::vector<util::FixedPointCoordinate> &trace_coordinates,
-                    const std::vector<unsigned> &trace_timestamps,
-                    const double matching_beta,
-                    const double gps_precision,
-                    SubMatchingList &sub_matchings) const
+    SubMatchingList
+    operator()(const CandidateLists &candidates_list,
+               const std::vector<util::FixedPointCoordinate> &trace_coordinates,
+               const std::vector<unsigned> &trace_timestamps,
+               const std::vector<boost::optional<double>> &trace_gps_precision) const
     {
+        SubMatchingList sub_matchings;
+
         BOOST_ASSERT(candidates_list.size() == trace_coordinates.size());
         BOOST_ASSERT(candidates_list.size() > 1);
 
@@ -107,16 +111,55 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
             }
         }();
 
-        // TODO replace default values with table lookup based on sampling frequency
-        map_matching::EmissionLogProbability emission_log_probability(gps_precision);
-        map_matching::TransitionLogProbability transition_log_probability(matching_beta);
+        std::vector<std::vector<double>> emission_log_probabilities(trace_coordinates.size());
+        if (trace_gps_precision.empty())
+        {
+            for (auto t = 0UL; t < candidates_list.size(); ++t)
+            {
+                emission_log_probabilities[t].resize(candidates_list.size());
+                std::transform(candidates_list[t].begin(), candidates_list[t].end(),
+                               emission_log_probabilities[t].begin(),
+                               [this](const PhantomNodeWithDistance &candidate)
+                               {
+                                   return default_emission_log_probability(candidate.distance);
+                               });
+            }
+        }
+        else
+        {
+            for (auto t = 0UL; t < candidates_list.size(); ++t)
+            {
+                emission_log_probabilities[t].resize(candidates_list.size());
+                if (trace_gps_precision[t])
+                {
+                    map_matching::EmissionLogProbability emission_log_probability(
+                        *trace_gps_precision[t]);
+                    std::transform(
+                        candidates_list[t].begin(), candidates_list[t].end(),
+                        emission_log_probabilities[t].begin(),
+                        [&emission_log_probability](const PhantomNodeWithDistance &candidate)
+                        {
+                            return emission_log_probability(candidate.distance);
+                        });
+                }
+                else
+                {
+                    std::transform(candidates_list[t].begin(), candidates_list[t].end(),
+                                   emission_log_probabilities[t].begin(),
+                                   [this](const PhantomNodeWithDistance &candidate)
+                                   {
+                                       return default_emission_log_probability(candidate.distance);
+                                   });
+                }
+            }
+        }
 
-        HMM model(candidates_list, emission_log_probability);
+        HMM model(candidates_list, emission_log_probabilities);
 
         std::size_t initial_timestamp = model.initialize(0);
         if (initial_timestamp == map_matching::INVALID_STATE)
         {
-            return;
+            return sub_matchings;
         }
 
         util::MatchingDebugInfo matching_debug(util::json::Logger::get());
@@ -193,9 +236,8 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
             auto &current_viterbi = model.viterbi[t];
             auto &current_pruned = model.pruned[t];
-            auto &current_suspicious = model.suspicious[t];
             auto &current_parents = model.parents[t];
-            auto &current_lengths = model.path_lengths[t];
+            auto &current_lengths = model.path_distances[t];
             const auto &current_timestamps_list = candidates_list[t];
             const auto &current_coordinate = trace_coordinates[t];
 
@@ -214,10 +256,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
 
                 for (const auto s_prime : util::irange<std::size_t>(0u, current_viterbi.size()))
                 {
-                    // how likely is candidate s_prime at time t to be emitted?
-                    // FIXME this can be pre-computed
-                    const double emission_pr =
-                        emission_log_probability(candidates_list[t][s_prime].distance);
+                    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)
                     {
@@ -268,7 +307,6 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                         current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s);
                         current_lengths[s_prime] = network_distance;
                         current_pruned[s_prime] = false;
-                        current_suspicious[s_prime] = d_t > SUSPICIOUS_DISTANCE_DELTA;
                         model.breakage[t] = false;
                     }
                 }
@@ -292,7 +330,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
             }
         }
 
-        matching_debug.set_viterbi(model.viterbi, model.pruned, model.suspicious);
+        matching_debug.set_viterbi(model.viterbi, model.pruned);
 
         if (!prev_unbroken_timestamps.empty())
         {
@@ -302,7 +340,7 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
         std::size_t sub_matching_begin = initial_timestamp;
         for (const auto sub_matching_end : split_points)
         {
-            SubMatching matching;
+            map_matching::SubMatching matching;
 
             std::size_t parent_timestamp_index = sub_matching_end - 1;
             while (parent_timestamp_index >= sub_matching_begin &&
@@ -355,25 +393,39 @@ class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<
                 continue;
             }
 
-            matching.length = 0.0;
-            matching.nodes.resize(reconstructed_indices.size());
-            matching.indices.resize(reconstructed_indices.size());
-            for (const auto i : util::irange<std::size_t>(0u, reconstructed_indices.size()))
+            auto matching_distance = 0.0;
+            auto trace_distance = 0.0;
+            matching.nodes.reserve(reconstructed_indices.size());
+            matching.indices.reserve(reconstructed_indices.size());
+            for (const auto idx : reconstructed_indices)
             {
-                const auto timestamp_index = reconstructed_indices[i].first;
-                const auto location_index = reconstructed_indices[i].second;
+                const auto timestamp_index = idx.first;
+                const auto location_index = idx.second;
 
-                matching.indices[i] = timestamp_index;
-                matching.nodes[i] = candidates_list[timestamp_index][location_index].phantom_node;
-                matching.length += model.path_lengths[timestamp_index][location_index];
+                matching.indices.push_back(timestamp_index);
+                matching.nodes.push_back(
+                    candidates_list[timestamp_index][location_index].phantom_node);
+                matching_distance += model.path_distances[timestamp_index][location_index];
 
                 matching_debug.add_chosen(timestamp_index, location_index);
             }
+            util::for_each_pair(
+                reconstructed_indices, [&trace_distance, &trace_coordinates](
+                                           const std::pair<std::size_t, std::size_t> &prev,
+                                           const std::pair<std::size_t, std::size_t> &curr)
+                {
+                    trace_distance += util::coordinate_calculation::haversineDistance(
+                        trace_coordinates[prev.first], trace_coordinates[curr.first]);
+                });
+
+            matching.confidence = confidence(trace_distance, matching_distance);
 
             sub_matchings.push_back(matching);
             sub_matching_begin = sub_matching_end;
         }
         matching_debug.add_breakage(model.breakage);
+
+        return sub_matchings;
     }
 };
 }