fix: account for road width in map matching

Author: Komzpa

features/testbot/matching.feature

@@ -159,6 +159,35 @@ Feature: Basic Map Matching
             | trace  | matchings |
             | afcde  | abcde     |
 
+    Scenario Outline: Testbot - Map matching accounts for road surface width from <source>
+        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   | width   | width:carriageway   | width:forward   | width:backward   | width:lanes   | width:lanes:forward   | width:lanes:backward   |
+            | abcd  | no     | <lanes> | <width> | <width_carriageway> | <width_forward> | <width_backward> | <width_lanes> | <width_lanes_forward> | <width_lanes_backward> |
+            | efgh  | no     |         |         |                     |                 |                  |               |                       |                        |
+
+        When I match I should get
+            | trace | matchings |
+            | 1234  | abcd      |
+
+        Examples:
+            | source                | lanes | width | width_carriageway | width_forward | width_backward | width_lanes         | width_lanes_forward | width_lanes_backward |
+            | width                 |       | 26    |                   |               |                |                     |                     |                      |
+            | width:carriageway     |       |       | 26                |               |                |                     |                     |                      |
+            | width:forward/backward |       |       |                   | 13            | 13             |                     |                     |                      |
+            | width:lanes           |       |       |                   |               |                | 6.5\|6.5\|6.5\|6.5 |                     |                      |
+            | width:lanes directions |       |       |                   |               |                |                     | 13                  | 13                   |
+            | lanes fallback        | 24    |       |                   |               |                |                     |                     |                      |
+
     Scenario: Testbot - Map matching with oneways
         Given a grid size of 10 meters
         Given the node map
@@ -826,4 +855,3 @@ Feature: Basic Map Matching
         When I match I should get
             | trace | matchings | confidence |
             | abcd  | abcd      | 1 ~5%      |
-

include/engine/datafacade/contiguous_internalmem_datafacade.hpp

@@ -167,6 +167,7 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade
     util::vector_view<TurnPenalty> m_turn_duration_penalties;
     extractor::SegmentDataView segment_data;
     extractor::EdgeBasedNodeDataView edge_based_node_data;
+    double m_max_road_half_width = 0.;
     std::optional<osrm::guidance::TurnDataView> turn_data;
 
     std::optional<util::vector_view<util::guidance::LaneTupleIdPair>> m_lane_tuple_id_pairs;
@@ -230,6 +231,7 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade
             new SharedGeospatialQuery(m_static_rtree, m_coordinate_list, *this));
 
         edge_based_node_data = make_ebn_data_view(index, "/common/ebg_node_data");
+        m_max_road_half_width = edge_based_node_data.ComputeMaxRoadHalfWidth();
 
         if (isIndexed(index, "/common/turn_data"))
         {
@@ -431,6 +433,18 @@ 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);
+    }
+
+    double GetRoadWidth(const NodeID edge_based_node_id) const override final
+    {
+        return edge_based_node_data.GetRoadWidth(edge_based_node_id);
+    }
+
+    double GetMaxRoadHalfWidth() const override final { return m_max_road_half_width; }
+
     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,12 @@ 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 { return 0; }
+
+    virtual double GetRoadWidth(const NodeID /*edge_based_node_id*/) const { return 0.; }
+
+    virtual double GetMaxRoadHalfWidth() const { return 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"
@@ -57,12 +58,12 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
     {
         auto results = rtree.SearchInRange(
             input_coordinate,
-            max_distance,
+            max_distance + datafacade.GetMaxRoadHalfWidth(),
             [this, approach, &input_coordinate, &bearing_with_range, &use_all_edges, max_distance](
                 const CandidateSegment &segment)
             {
                 auto invalidDistance =
-                    CheckSegmentDistance(input_coordinate, segment, max_distance);
+                    CheckSegmentDistance(input_coordinate, segment, max_distance, true);
                 if (invalidDistance)
                 {
                     return std::make_pair(false, false);
@@ -106,7 +107,7 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
             {
                 return (num_results >= max_results) ||
                        (max_distance && max_distance != -1.0 &&
-                        CheckSegmentDistance(input_coordinate, segment, *max_distance));
+                        CheckSegmentDistance(input_coordinate, segment, *max_distance, false));
             });
 
         return MakePhantomNodes(input_coordinate, results);
@@ -495,11 +496,44 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                                                                  wsg84_coordinate);
     }
 
+    double GetRoadHalfWidth(const NodeID node_id) const
+    {
+        const double road_width = std::min(datafacade.GetRoadWidth(node_id),
+                                           extractor::intersection::MAX_ROAD_SURFACE_WIDTH);
+        if (road_width > 0.)
+        {
+            return 0.5 * road_width;
+        }
+
+        return 0.5 * std::min<double>(datafacade.GetNumberOfLanes(node_id) *
+                                          extractor::intersection::ASSUMED_LANE_WIDTH,
+                                      extractor::intersection::MAX_ROAD_SURFACE_WIDTH);
+    }
+
+    double GetRoadHalfWidth(const CandidateSegment &segment) const
+    {
+        if (segment.data.forward_segment_id.enabled &&
+            segment.data.forward_segment_id.id != SPECIAL_SEGMENTID)
+        {
+            return GetRoadHalfWidth(segment.data.forward_segment_id.id);
+        }
+
+        if (segment.data.reverse_segment_id.enabled &&
+            segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID)
+        {
+            return GetRoadHalfWidth(segment.data.reverse_segment_id.id);
+        }
+
+        return 0.;
+    }
+
     bool CheckSegmentDistance(const Coordinate input_coordinate,
                               const CandidateSegment &segment,
-                              const double max_distance) const
+                              const double max_distance,
+                              const bool account_road_surface) const
     {
-        return GetSegmentDistance(input_coordinate, segment) > max_distance;
+        return GetSegmentDistance(input_coordinate, segment) >
+               max_distance + (account_road_surface ? GetRoadHalfWidth(segment) : 0.);
     }
 
     std::pair<bool, bool> CheckSegmentExclude(const CandidateSegment &segment) const

include/extractor/extraction_way.hpp

@@ -51,6 +51,7 @@ struct ExtractionWay
         exits.clear();
         turn_lanes_forward.clear();
         turn_lanes_backward.clear();
+        road_width = 0.;
         road_classification = RoadClassification();
         forward_travel_mode = TRAVEL_MODE_INACCESSIBLE;
         backward_travel_mode = TRAVEL_MODE_INACCESSIBLE;
@@ -110,6 +111,7 @@ struct ExtractionWay
     std::string exits;
     std::string turn_lanes_forward;
     std::string turn_lanes_backward;
+    double road_width;
     RoadClassification road_classification;
     TravelMode forward_travel_mode : 4;
     TravelMode backward_travel_mode : 4;

include/extractor/graph_compressor.hpp

@@ -24,7 +24,7 @@ class GraphCompressor
                   std::vector<TurnRestriction> &turn_restrictions,
                   std::vector<UnresolvedManeuverOverride> &maneuver_overrides,
                   util::NodeBasedDynamicGraph &graph,
-                  const std::vector<NodeBasedEdgeAnnotation> &node_data_container,
+                  std::vector<NodeBasedEdgeAnnotation> &node_data_container,
                   CompressedEdgeContainer &geometry_compressor);
 
   private:

include/extractor/intersection/constants.hpp

@@ -22,6 +22,8 @@ const double constexpr PRIORITY_DISTINCTION_FACTOR = 1.75;
 
 // the lane width we assume for a single lane
 const auto constexpr ASSUMED_LANE_WIDTH = 3.25;
+// cap pathological width tags and lane-derived widths when approximating a road surface
+const auto constexpr MAX_ROAD_SURFACE_WIDTH = 80.;
 
 // how far apart can roads be at the most, when thinking about merging them?
 const auto constexpr MERGABLE_ANGLE_DIFFERENCE = 95.0;

include/extractor/node_based_edge.hpp

@@ -68,6 +68,8 @@ 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
+    std::uint16_t road_width;              // 16 2, centimeters
     TravelMode travel_mode : 4;            // 4
     bool is_left_hand_driving : 1;         // 1
 
@@ -80,16 +82,26 @@ struct NodeBasedEdgeAnnotation
                          other.is_left_hand_driving));
     }
 
+    bool CanCompressWith(const NodeBasedEdgeAnnotation &other) const
+    {
+        return CanCombineWith(other) && road_width == other.road_width;
+    }
+
     bool operator<(const NodeBasedEdgeAnnotation &other) const
     {
-        return (
-            std::tie(
-                string_view_id, lane_description_id, classes, travel_mode, is_left_hand_driving) <
-            std::tie(other.string_view_id,
-                     other.lane_description_id,
-                     other.classes,
-                     other.travel_mode,
-                     other.is_left_hand_driving));
+        return (std::tie(string_view_id,
+                         lane_description_id,
+                         classes,
+                         number_of_lanes,
+                         road_width,
+                         travel_mode,
+                         is_left_hand_driving) < std::tie(other.string_view_id,
+                                                          other.lane_description_id,
+                                                          other.classes,
+                                                          other.number_of_lanes,
+                                                          other.road_width,
+                                                          other.travel_mode,
+                                                          other.is_left_hand_driving));
     }
 };
 

include/extractor/node_data_container.hpp

@@ -3,6 +3,7 @@
 
 #include "extractor/class_data.hpp"
 #include "extractor/edge_based_node.hpp"
+#include "extractor/intersection/constants.hpp"
 #include "extractor/node_based_edge.hpp"
 #include "extractor/travel_mode.hpp"
 
@@ -13,6 +14,8 @@
 #include "util/typedefs.hpp"
 #include "util/vector_view.hpp"
 
+#include <algorithm>
+
 namespace osrm::extractor
 {
 
@@ -89,6 +92,32 @@ 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;
+    }
+
+    double GetRoadWidth(const NodeID node_id) const
+    {
+        return annotation_data[nodes[node_id].annotation_id].road_width / 100.;
+    }
+
+    double ComputeMaxRoadHalfWidth() const
+    {
+        double max_road_half_width = 0.;
+        for (const auto &annotation : annotation_data)
+        {
+            const double road_width = std::min<double>(annotation.road_width / 100.,
+                                                       intersection::MAX_ROAD_SURFACE_WIDTH);
+            const double fallback_width =
+                std::min<double>(annotation.number_of_lanes * intersection::ASSUMED_LANE_WIDTH,
+                                 intersection::MAX_ROAD_SURFACE_WIDTH);
+            max_road_half_width =
+                std::max(max_road_half_width, 0.5 * std::max(road_width, fallback_width));
+        }
+        return max_road_half_width;
+    }
+
     friend void serialization::read<Ownership>(storage::tar::FileReader &reader,
                                                const std::string &name,
                                                EdgeBasedNodeDataContainerImpl &ebn_data_container);

profiles/lib/guidance.lua

@@ -1,5 +1,6 @@
 local Tags = require('lib/tags')
 local Set = require('lib/set')
+local Measure = require('lib/measure')
 
 local Guidance = {}
 
@@ -77,6 +78,70 @@ local function to_number_uint(s)
   return nil
 end
 
+local function first_width(way, keys)
+  for _, key in ipairs(keys) do
+    local width = Measure.get_max_width(way:get_value_by_key(key))
+    if width and width > 0 then
+      return width
+    end
+  end
+end
+
+local function sum_width_lanes(value)
+  if not value then
+    return nil
+  end
+
+  local total = 0
+  local found = false
+  for lane_width in (value .. '|'):gmatch("([^|]*)|") do
+    local width = Measure.get_max_width(lane_width)
+    if width and width > 0 then
+      total = total + width
+      found = true
+    end
+  end
+
+  if found then
+    return total
+  end
+end
+
+local function first_width_lanes(way, keys)
+  for _, key in ipairs(keys) do
+    local width = sum_width_lanes(way:get_value_by_key(key))
+    if width and width > 0 then
+      return width
+    end
+  end
+end
+
+function Guidance.get_road_width(way)
+  local width = first_width(way, { 'width:carriageway', 'width', 'est_width' })
+  if width then
+    return width
+  end
+
+  local forward_width = first_width(way, { 'width:forward' })
+  local backward_width = first_width(way, { 'width:backward' })
+
+  if forward_width or backward_width then
+    return (forward_width or 0) + (backward_width or 0)
+  end
+
+  forward_width = first_width_lanes(way, { 'width:lanes:forward' })
+  backward_width = first_width_lanes(way, { 'width:lanes:backward' })
+
+  if forward_width or backward_width then
+    return (forward_width or 0) + (backward_width or 0)
+  end
+
+  width = first_width_lanes(way, { 'width:lanes' })
+  if width then
+    return width
+  end
+end
+
 function Guidance.set_classification (highway, result, input_way)
   if motorway_types[highway] then
     result.road_classification.motorway_class = true
@@ -140,6 +205,11 @@ function Guidance.set_classification (highway, result, input_way)
       result.road_classification.num_lanes = total_count
     end
   end
+
+  local road_width = Guidance.get_road_width(input_way)
+  if road_width then
+    result.road_width = road_width
+  end
 end
 
 -- returns forward,backward psv lane count