Fix map matching lane-width tolerance

Author: Komzpa

features/testbot/matching.feature

@@ -159,6 +159,26 @@ Feature: Basic Map Matching
             | trace  | matchings |
             | afcde  | abcde     |
 
+    Scenario: Testbot - Map matching accounts for lane width
+        Given a grid size of 4 meters
+        Given the node map
+            """
+            a b c d
+
+
+            1 2 3 4
+            e f g h
+            """
+
+        And the ways
+            | nodes | oneway | lanes |
+            | abcd  | no     | 8     |
+            | efgh  | no     |       |
+
+        When I match I should get
+            | trace | matchings |
+            | 1234  | abcd      |
+
     Scenario: Testbot - Map matching with oneways
         Given a grid size of 10 meters
         Given the node map

include/engine/datafacade/contiguous_internalmem_datafacade.hpp

@@ -431,6 +431,11 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade
         return edge_based_node_data.GetClassData(edge_based_node_id);
     }
 
+    std::uint8_t GetNumberOfLanes(const NodeID edge_based_node_id) const override final
+    {
+        return edge_based_node_data.GetNumberOfLanes(edge_based_node_id);
+    }
+
     bool ExcludeNode(const NodeID edge_based_node_id) const override final
     {
         return (edge_based_node_data.GetClassData(edge_based_node_id) & exclude_mask) > 0;

include/engine/datafacade/datafacade_base.hpp

@@ -114,6 +114,8 @@ class BaseDataFacade
 
     virtual extractor::ClassData GetClassData(const NodeID edge_based_node_id) const = 0;
 
+    virtual std::uint8_t GetNumberOfLanes(const NodeID edge_based_node_id) const = 0;
+
     virtual bool ExcludeNode(const NodeID edge_based_node_id) const = 0;
 
     virtual std::vector<std::string> GetClasses(const extractor::ClassData class_data) const = 0;

include/engine/geospatial_query.hpp

@@ -1,6 +1,7 @@
 #ifndef GEOSPATIAL_QUERY_HPP
 #define GEOSPATIAL_QUERY_HPP
 
+#include "extractor/intersection/constants.hpp"
 #include "engine/approach.hpp"
 #include "engine/bearing.hpp"
 #include "engine/phantom_node.hpp"
@@ -17,6 +18,7 @@
 #include <algorithm>
 #include <cmath>
 #include <iterator>
+#include <limits>
 #include <vector>
 
 namespace osrm::engine
@@ -57,7 +59,7 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
     {
         auto results = rtree.SearchInRange(
             input_coordinate,
-            max_distance,
+            max_distance + GetMaximumRoadHalfWidth(),
             [this, approach, &input_coordinate, &bearing_with_range, &use_all_edges, max_distance](
                 const CandidateSegment &segment)
             {
@@ -495,11 +497,37 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                                                                  wsg84_coordinate);
     }
 
+    static constexpr double GetMaximumRoadHalfWidth()
+    {
+        return 0.5 * std::numeric_limits<std::uint8_t>::max() *
+               extractor::intersection::ASSUMED_LANE_WIDTH;
+    }
+
+    double GetRoadHalfWidth(const CandidateSegment &segment) const
+    {
+        if (segment.data.forward_segment_id.enabled &&
+            segment.data.forward_segment_id.id != SPECIAL_SEGMENTID)
+        {
+            return 0.5 * datafacade.GetNumberOfLanes(segment.data.forward_segment_id.id) *
+                   extractor::intersection::ASSUMED_LANE_WIDTH;
+        }
+
+        if (segment.data.reverse_segment_id.enabled &&
+            segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID)
+        {
+            return 0.5 * datafacade.GetNumberOfLanes(segment.data.reverse_segment_id.id) *
+                   extractor::intersection::ASSUMED_LANE_WIDTH;
+        }
+
+        return 0.;
+    }
+
     bool CheckSegmentDistance(const Coordinate input_coordinate,
                               const CandidateSegment &segment,
                               const double max_distance) const
     {
-        return GetSegmentDistance(input_coordinate, segment) > max_distance;
+        return GetSegmentDistance(input_coordinate, segment) >
+               max_distance + GetRoadHalfWidth(segment);
     }
 
     std::pair<bool, bool> CheckSegmentExclude(const CandidateSegment &segment) const

include/extractor/node_based_edge.hpp

@@ -68,29 +68,35 @@ struct NodeBasedEdgeAnnotation
     StringViewID string_view_id;           // 32 4
     LaneDescriptionID lane_description_id; // 16 2
     ClassData classes;                     // 8  1
+    std::uint8_t number_of_lanes;          // 8  1
     TravelMode travel_mode : 4;            // 4
     bool is_left_hand_driving : 1;         // 1
 
     bool CanCombineWith(const NodeBasedEdgeAnnotation &other) const
-    {
-        return (std::tie(string_view_id, classes, travel_mode, is_left_hand_driving) ==
-                std::tie(other.string_view_id,
-                         other.classes,
-                         other.travel_mode,
-                         other.is_left_hand_driving));
-    }
-
-    bool operator<(const NodeBasedEdgeAnnotation &other) const
     {
         return (
-            std::tie(
-                string_view_id, lane_description_id, classes, travel_mode, is_left_hand_driving) <
+            std::tie(string_view_id, classes, number_of_lanes, travel_mode, is_left_hand_driving) ==
             std::tie(other.string_view_id,
-                     other.lane_description_id,
                      other.classes,
+                     other.number_of_lanes,
                      other.travel_mode,
                      other.is_left_hand_driving));
     }
+
+    bool operator<(const NodeBasedEdgeAnnotation &other) const
+    {
+        return (std::tie(string_view_id,
+                         lane_description_id,
+                         classes,
+                         number_of_lanes,
+                         travel_mode,
+                         is_left_hand_driving) < std::tie(other.string_view_id,
+                                                          other.lane_description_id,
+                                                          other.classes,
+                                                          other.number_of_lanes,
+                                                          other.travel_mode,
+                                                          other.is_left_hand_driving));
+    }
 };
 
 struct NodeBasedEdge

include/extractor/node_data_container.hpp

@@ -89,6 +89,11 @@ template <storage::Ownership Ownership> class EdgeBasedNodeDataContainerImpl
         return annotation_data[nodes[node_id].annotation_id].classes;
     }
 
+    std::uint8_t GetNumberOfLanes(const NodeID node_id) const
+    {
+        return annotation_data[nodes[node_id].annotation_id].number_of_lanes;
+    }
+
     friend void serialization::read<Ownership>(storage::tar::FileReader &reader,
                                                const std::string &name,
                                                EdgeBasedNodeDataContainerImpl &ebn_data_container);

profiles/testbot.lua

@@ -55,6 +55,7 @@ function process_way (profile, way, result)
   local maxspeed_forward = tonumber(way:get_value_by_key( "maxspeed:forward"))
   local maxspeed_backward = tonumber(way:get_value_by_key( "maxspeed:backward"))
   local junction = way:get_value_by_key("junction")
+  local lanes = tonumber(way:get_value_by_key("lanes"))
 
   if name then
     result.name = name
@@ -120,6 +121,10 @@ function process_way (profile, way, result)
       result.backward_classes["toll"] = true
   end
 
+  if lanes and lanes > 0 then
+    result.road_classification.num_lanes = lanes
+  end
+
   if junction == 'roundabout' then
     result.roundabout = true
   end

src/engine/routing_algorithms/map_matching.cpp

@@ -6,6 +6,8 @@
 #include "engine/map_matching/matching_confidence.hpp"
 #include "engine/map_matching/sub_matching.hpp"
 
+#include "extractor/intersection/constants.hpp"
+
 #include "util/coordinate_calculation.hpp"
 #include "util/for_each_pair.hpp"
 
@@ -61,6 +63,20 @@ inline void initializeHeap<mld::Algorithm>(SearchEngineData<mld::Algorithm> &eng
     engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number,
                                                                        border_nodes_number);
 }
+
+template <typename Algorithm>
+double getDistanceOutsideRoadSurface(const DataFacade<Algorithm> &facade,
+                                     const PhantomNodeWithDistance &candidate)
+{
+    const auto &phantom = candidate.phantom_node;
+    const auto number_of_lanes = phantom.forward_segment_id.enabled
+                                     ? facade.GetNumberOfLanes(phantom.forward_segment_id.id)
+                                     : facade.GetNumberOfLanes(phantom.reverse_segment_id.id);
+    const double road_half_width =
+        0.5 * number_of_lanes * extractor::intersection::ASSUMED_LANE_WIDTH;
+
+    return std::max(0., candidate.distance - road_half_width);
+}
 } // namespace
 
 template <typename Algorithm>
@@ -106,7 +122,10 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                            candidates_list[t].end(),
                            emission_log_probabilities[t].begin(),
                            [&](const PhantomNodeWithDistance &candidate)
-                           { return default_emission_log_probability(candidate.distance); });
+                           {
+                               return default_emission_log_probability(
+                                   getDistanceOutsideRoadSurface(facade, candidate));
+                           });
         }
     }
     else
@@ -121,16 +140,22 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
                 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); });
+                               [&](const PhantomNodeWithDistance &candidate)
+                               {
+                                   return emission_log_probability(
+                                       getDistanceOutsideRoadSurface(facade, candidate));
+                               });
             }
             else
             {
                 std::transform(candidates_list[t].begin(),
                                candidates_list[t].end(),
                                emission_log_probabilities[t].begin(),
                                [&](const PhantomNodeWithDistance &candidate)
-                               { return default_emission_log_probability(candidate.distance); });
+                               {
+                                   return default_emission_log_probability(
+                                       getDistanceOutsideRoadSurface(facade, candidate));
+                               });
             }
         }
     }

src/extractor/extractor_callbacks.cpp

@@ -400,11 +400,13 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
     if (in_forward_direction)
     { // add (forward) segments or (forward,backward) for non-split edges in backward direction
         const auto annotation_data_id = external_memory.all_edges_annotation_data_list.size();
-        external_memory.all_edges_annotation_data_list.push_back({forward_name_id,
-                                                                  turn_lane_id_forward,
-                                                                  forward_classes,
-                                                                  parsed_way.forward_travel_mode,
-                                                                  parsed_way.is_left_hand_driving});
+        external_memory.all_edges_annotation_data_list.push_back(
+            {forward_name_id,
+             turn_lane_id_forward,
+             forward_classes,
+             road_classification.GetNumberOfLanes(),
+             parsed_way.forward_travel_mode,
+             parsed_way.is_left_hand_driving});
         util::for_each_pair(nodes,
                             [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
                             {
@@ -435,11 +437,13 @@ void ExtractorCallbacks::ProcessWay(const osmium::Way &input_way, const Extracti
     if (in_backward_direction && (!in_forward_direction || split_edge))
     { // add (backward) segments for split edges or not in forward direction
         const auto annotation_data_id = external_memory.all_edges_annotation_data_list.size();
-        external_memory.all_edges_annotation_data_list.push_back({backward_name_id,
-                                                                  turn_lane_id_backward,
-                                                                  backward_classes,
-                                                                  parsed_way.backward_travel_mode,
-                                                                  parsed_way.is_left_hand_driving});
+        external_memory.all_edges_annotation_data_list.push_back(
+            {backward_name_id,
+             turn_lane_id_backward,
+             backward_classes,
+             road_classification.GetNumberOfLanes(),
+             parsed_way.backward_travel_mode,
+             parsed_way.is_left_hand_driving});
         util::for_each_pair(nodes,
                             [&](const osmium::NodeRef &first_node, const osmium::NodeRef &last_node)
                             {

unit_tests/engine/offline_facade.cpp

@@ -319,6 +319,7 @@ class ContiguousInternalMemoryDataFacade<routing_algorithms::offline::Algorithm>
     }
 
     osrm::extractor::ClassData GetClassData(const NodeID /*id*/) const override { return 0; }
+    std::uint8_t GetNumberOfLanes(const NodeID /*id*/) const override { return 0; }
     std::vector<std::string> GetClasses(const extractor::ClassData /*class_data*/) const override
     {
         return {};