Optimise R-tree queries in the case of map matching (#6881)

CHANGELOG.md

@@ -49,6 +49,7 @@
       - FIXED: Correctly check runtime search conditions for forcing routing steps [#6866](https://github.com/Project-OSRM/osrm-backend/pull/6866)
     - Map Matching:
       - CHANGED: Optimise path distance calculation in MLD map matching. [#6876](https://github.com/Project-OSRM/osrm-backend/pull/6876)
+      - CHANGED: Optimise R-tree queries in the case of map matching. [#6881](https://github.com/Project-OSRM/osrm-backend/pull/6876)
     - Debug tiles:
       - FIXED: Ensure speed layer features have unique ids. [#6726](https://github.com/Project-OSRM/osrm-backend/pull/6726)
 

include/engine/datafacade/contiguous_internalmem_datafacade.hpp

@@ -375,7 +375,7 @@ class ContiguousInternalMemoryDataFacadeBase : public BaseDataFacade
         BOOST_ASSERT(m_geospatial_query.get());
 
         return m_geospatial_query->NearestPhantomNodes(
-            input_coordinate, approach, boost::none, max_distance, bearing, use_all_edges);
+            input_coordinate, approach, max_distance, bearing, use_all_edges);
     }
 
     std::vector<PhantomNodeWithDistance>

include/engine/geospatial_query.hpp

@@ -47,12 +47,42 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
         return rtree.SearchInBox(bbox);
     }
 
+    std::vector<PhantomNodeWithDistance>
+    NearestPhantomNodes(const util::Coordinate input_coordinate,
+                        const Approach approach,
+                        const double max_distance,
+                        const boost::optional<Bearing> bearing_with_range,
+                        const boost::optional<bool> use_all_edges) const
+    {
+        auto results = rtree.SearchInRange(
+            input_coordinate,
+            max_distance,
+            [this, approach, &input_coordinate, &bearing_with_range, &use_all_edges, max_distance](
+                const CandidateSegment &segment)
+            {
+                auto invalidDistance =
+                    CheckSegmentDistance(input_coordinate, segment, max_distance);
+                if (invalidDistance)
+                {
+                    return std::make_pair(false, false);
+                }
+                auto valid = CheckSegmentExclude(segment) &&
+                             CheckApproach(input_coordinate, segment, approach) &&
+                             (use_all_edges ? HasValidEdge(segment, *use_all_edges)
+                                            : HasValidEdge(segment)) &&
+                             (bearing_with_range ? CheckSegmentBearing(segment, *bearing_with_range)
+                                                 : std::make_pair(true, true));
+                return valid;
+            });
+        return MakePhantomNodes(input_coordinate, results);
+    }
+
     // Returns max_results nearest PhantomNodes that are valid within the provided parameters.
     // Does not filter by small/big component!
     std::vector<PhantomNodeWithDistance>
     NearestPhantomNodes(const util::Coordinate input_coordinate,
                         const Approach approach,
-                        const boost::optional<size_t> max_results,
+                        const size_t max_results,
                         const boost::optional<double> max_distance,
                         const boost::optional<Bearing> bearing_with_range,
                         const boost::optional<bool> use_all_edges) const
@@ -70,10 +100,10 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
                                                  : std::make_pair(true, true));
                 return valid;
             },
-            [this, &max_distance, &max_results, input_coordinate](const std::size_t num_results,
+            [this, &max_distance, max_results, input_coordinate](const std::size_t num_results,
                                                                  const CandidateSegment &segment)
             {
-                return (max_results && num_results >= *max_results) ||
+                return (num_results >= max_results) ||
                        (max_distance && max_distance != -1.0 &&
                         CheckSegmentDistance(input_coordinate, segment, *max_distance));
             });

include/util/coordinate_calculation.hpp

@@ -36,6 +36,13 @@ inline double radToDeg(const double radian)
 }
 } // namespace detail
 
+const constexpr static double METERS_PER_DEGREE_LAT = 110567.0;
+
+inline double metersPerLngDegree(const FixedLatitude lat)
+{
+    return std::cos(detail::degToRad(static_cast<double>(toFloating(lat)))) * METERS_PER_DEGREE_LAT;
+}
+
 //! Takes the squared euclidean distance of the input coordinates. Does not return meters!
 std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);
 

include/util/rectangle.hpp

@@ -3,7 +3,6 @@
 
 #include "util/coordinate.hpp"
 #include "util/coordinate_calculation.hpp"
-
 #include <boost/assert.hpp>
 
 #include <limits>
@@ -168,6 +167,18 @@ struct RectangleInt2D
                min_lat != FixedLatitude{std::numeric_limits<std::int32_t>::max()} &&
                max_lat != FixedLatitude{std::numeric_limits<std::int32_t>::min()};
     }
+
+    static RectangleInt2D ExpandMeters(const Coordinate &coordinate, const double meters)
+    {
+        const double lat_offset = meters / coordinate_calculation::METERS_PER_DEGREE_LAT;
+        const double lon_offset =
+            meters / coordinate_calculation::metersPerLngDegree(coordinate.lat);
+
+        return RectangleInt2D{coordinate.lon - toFixed(FloatLongitude{lon_offset}),
+                              coordinate.lon + toFixed(FloatLongitude{lon_offset}),
+                              coordinate.lat - toFixed(FloatLatitude{lat_offset}),
+                              coordinate.lat + toFixed(FloatLatitude{lat_offset})};
+    }
 };
 } // namespace osrm::util
 

include/util/static_rtree.hpp

@@ -2,7 +2,6 @@
 #define STATIC_RTREE_HPP
 
 #include "storage/tar_fwd.hpp"
-
 #include "util/bearing.hpp"
 #include "util/coordinate_calculation.hpp"
 #include "util/deallocating_vector.hpp"
@@ -11,6 +10,7 @@
 #include "util/integer_range.hpp"
 #include "util/mmap_file.hpp"
 #include "util/rectangle.hpp"
+#include "util/timing_util.hpp"
 #include "util/typedefs.hpp"
 #include "util/vector_view.hpp"
 #include "util/web_mercator.hpp"
@@ -487,70 +487,9 @@ class StaticRTree
        Rectangle needs to be projected!*/
     std::vector<EdgeDataT> SearchInBox(const Rectangle &search_rectangle) const
     {
-        const Rectangle projected_rectangle{
-            search_rectangle.min_lon,
-            search_rectangle.max_lon,
-            toFixed(FloatLatitude{
-                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.min_lat)))}),
-            toFixed(FloatLatitude{
-                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.max_lat)))})};
         std::vector<EdgeDataT> results;
-
+        SearchInBox(search_rectangle,
-        std::queue<TreeIndex> traversal_queue;
+                    [&results](const auto &edge_data) { results.push_back(edge_data); });
-        traversal_queue.push(TreeIndex{});
-
-        while (!traversal_queue.empty())
-        {
-            auto const current_tree_index = traversal_queue.front();
-            traversal_queue.pop();
-
-            // If we're at the bottom of the tree, we need to explore the
-            // element array
-            if (is_leaf(current_tree_index))
-            {
-
-                // Note: irange is [start,finish), so we need to +1 to make sure we visit the
-                // last
-                for (const auto current_child_index : child_indexes(current_tree_index))
-                {
-                    const auto &current_edge = m_objects[current_child_index];
-
-                    // we don't need to project the coordinates here,
-                    // because we use the unprojected rectangle to test against
-                    const Rectangle bbox{std::min(m_coordinate_list[current_edge.u].lon,
-                                                  m_coordinate_list[current_edge.v].lon),
-                                         std::max(m_coordinate_list[current_edge.u].lon,
-                                                  m_coordinate_list[current_edge.v].lon),
-                                         std::min(m_coordinate_list[current_edge.u].lat,
-                                                  m_coordinate_list[current_edge.v].lat),
-                                         std::max(m_coordinate_list[current_edge.u].lat,
-                                                  m_coordinate_list[current_edge.v].lat)};
-
-                    // use the _unprojected_ input rectangle here
-                    if (bbox.Intersects(search_rectangle))
-                    {
-                        results.push_back(current_edge);
-                    }
-                }
-            }
-            else
-            {
-                BOOST_ASSERT(current_tree_index.level + 1 < m_tree_level_starts.size());
-
-                for (const auto child_index : child_indexes(current_tree_index))
-                {
-                    const auto &child_rectangle =
-                        m_search_tree[child_index].minimum_bounding_rectangle;
-
-                    if (child_rectangle.Intersects(projected_rectangle))
-                    {
-                        traversal_queue.push(TreeIndex(
-                            current_tree_index.level + 1,
-                            child_index - m_tree_level_starts[current_tree_index.level + 1]));
-                    }
-                }
-            }
-        }
         return results;
     }
 
@@ -565,6 +504,45 @@ class StaticRTree
             { return num_results >= max_results; });
     }
 
+    // NB 1: results are not guaranteed to be sorted by distance
+    // NB 2: maxDistanceMeters is not a hard limit, it's just a way to reduce the number of edges
+    // returned
+    template <typename FilterT>
+    std::vector<CandidateSegment> SearchInRange(const Coordinate input_coordinate,
+                                                double maxDistanceMeters,
+                                                const FilterT filter) const
+    {
+        auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
+        Coordinate fixed_projected_coordinate{projected_coordinate};
+
+        auto bbox = Rectangle::ExpandMeters(input_coordinate, maxDistanceMeters);
+        std::vector<CandidateSegment> results;
+
+        SearchInBox(
+            bbox,
+            [&results, &filter, fixed_projected_coordinate, this](const EdgeDataT &current_edge)
+            {
+                const auto projected_u = web_mercator::fromWGS84(m_coordinate_list[current_edge.u]);
+                const auto projected_v = web_mercator::fromWGS84(m_coordinate_list[current_edge.v]);
+
+                auto [_, projected_nearest] = coordinate_calculation::projectPointOnSegment(
+                    projected_u, projected_v, fixed_projected_coordinate);
+
+                CandidateSegment current_candidate{projected_nearest, current_edge};
+                auto use_segment = filter(current_candidate);
+                if (!use_segment.first && !use_segment.second)
+                {
+                    return;
+                }
+                current_candidate.data.forward_segment_id.enabled &= use_segment.first;
+                current_candidate.data.reverse_segment_id.enabled &= use_segment.second;
+
+                results.push_back(current_candidate);
+            });
+
+        return results;
+    }
+
     // Return edges in distance order with the coordinate of the closest point on the edge.
     template <typename FilterT, typename TerminationT>
     std::vector<CandidateSegment> Nearest(const Coordinate input_coordinate,
@@ -572,8 +550,10 @@ class StaticRTree
                                           const TerminationT terminate) const
     {
         std::vector<CandidateSegment> results;
+
         auto projected_coordinate = web_mercator::fromWGS84(input_coordinate);
         Coordinate fixed_projected_coordinate{projected_coordinate};
+
         // initialize queue with root element
         std::priority_queue<QueryCandidate> traversal_queue;
         traversal_queue.push(QueryCandidate{0, TreeIndex{}});
@@ -631,6 +611,73 @@ class StaticRTree
     }
 
   private:
+    template <typename Callback>
+    void SearchInBox(const Rectangle &search_rectangle, Callback &&callback) const
+    {
+        const Rectangle projected_rectangle{
+            search_rectangle.min_lon,
+            search_rectangle.max_lon,
+            toFixed(FloatLatitude{
+                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.min_lat)))}),
+            toFixed(FloatLatitude{
+                web_mercator::latToY(toFloating(FixedLatitude(search_rectangle.max_lat)))})};
+        std::queue<TreeIndex> traversal_queue;
+        traversal_queue.push(TreeIndex{});
+
+        while (!traversal_queue.empty())
+        {
+            auto const current_tree_index = traversal_queue.front();
+            traversal_queue.pop();
+
+            // If we're at the bottom of the tree, we need to explore the
+            // element array
+            if (is_leaf(current_tree_index))
+            {
+
+                // Note: irange is [start,finish), so we need to +1 to make sure we visit the
+                // last
+                for (const auto current_child_index : child_indexes(current_tree_index))
+                {
+                    const auto &current_edge = m_objects[current_child_index];
+
+                    // we don't need to project the coordinates here,
+                    // because we use the unprojected rectangle to test against
+                    const Rectangle bbox{std::min(m_coordinate_list[current_edge.u].lon,
+                                                  m_coordinate_list[current_edge.v].lon),
+                                         std::max(m_coordinate_list[current_edge.u].lon,
+                                                  m_coordinate_list[current_edge.v].lon),
+                                         std::min(m_coordinate_list[current_edge.u].lat,
+                                                  m_coordinate_list[current_edge.v].lat),
+                                         std::max(m_coordinate_list[current_edge.u].lat,
+                                                  m_coordinate_list[current_edge.v].lat)};
+
+                    // use the _unprojected_ input rectangle here
+                    if (bbox.Intersects(search_rectangle))
+                    {
+                        callback(current_edge);
+                    }
+                }
+            }
+            else
+            {
+                BOOST_ASSERT(current_tree_index.level + 1 < m_tree_level_starts.size());
+
+                for (const auto child_index : child_indexes(current_tree_index))
+                {
+                    const auto &child_rectangle =
+                        m_search_tree[child_index].minimum_bounding_rectangle;
+
+                    if (child_rectangle.Intersects(projected_rectangle))
+                    {
+                        traversal_queue.push(TreeIndex(
+                            current_tree_index.level + 1,
+                            child_index - m_tree_level_starts[current_tree_index.level + 1]));
+                    }
+                }
+            }
+        }
+    }
+
     /**
      * Iterates over all the objects in a leaf node and inserts them into our
      * search priority queue.  The speed of this function is very much governed

src/benchmarks/match.cpp

@@ -11,6 +11,7 @@
 #include "osrm/status.hpp"
 
 #include <boost/assert.hpp>
+
 #include <cstdlib>
 #include <exception>
 #include <iostream>

unit_tests/util/static_rtree.cpp

@@ -348,7 +348,13 @@ BOOST_AUTO_TEST_CASE(radius_regression_test)
 
     {
         auto results = query.NearestPhantomNodes(
-            input, osrm::engine::Approach::UNRESTRICTED, boost::none, 0.01, boost::none, true);
+            input, osrm::engine::Approach::UNRESTRICTED, 0.01, boost::none, true);
+        BOOST_CHECK_EQUAL(results.size(), 0);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 1, 0.01, boost::none, true);
         BOOST_CHECK_EQUAL(results.size(), 0);
     }
 }
@@ -374,13 +380,25 @@ BOOST_AUTO_TEST_CASE(permissive_edge_snapping)
 
     {
         auto results = query.NearestPhantomNodes(
-            input, osrm::engine::Approach::UNRESTRICTED, boost::none, 1000, boost::none, false);
+            input, osrm::engine::Approach::UNRESTRICTED, 1000, boost::none, false);
         BOOST_CHECK_EQUAL(results.size(), 1);
     }
 
     {
         auto results = query.NearestPhantomNodes(
-            input, osrm::engine::Approach::UNRESTRICTED, boost::none, 1000, boost::none, true);
+            input, osrm::engine::Approach::UNRESTRICTED, 1000, boost::none, true);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 10, 1000, boost::none, false);
+        BOOST_CHECK_EQUAL(results.size(), 1);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 10, 1000, boost::none, true);
         BOOST_CHECK_EQUAL(results.size(), 2);
     }
 }
@@ -442,27 +460,45 @@ BOOST_AUTO_TEST_CASE(bearing_tests)
 
     {
         auto results = query.NearestPhantomNodes(
-            input, osrm::engine::Approach::UNRESTRICTED, boost::none, 11000, boost::none, true);
+            input, osrm::engine::Approach::UNRESTRICTED, 11000, boost::none, true);
         BOOST_CHECK_EQUAL(results.size(), 2);
     }
 
     {
-        auto results = query.NearestPhantomNodes(input,
+        auto results = query.NearestPhantomNodes(
-                                                 osrm::engine::Approach::UNRESTRICTED,
+            input, osrm::engine::Approach::UNRESTRICTED, 10, 11000, boost::none, true);
-                                                 boost::none,
+        BOOST_CHECK_EQUAL(results.size(), 2);
-                                                 11000,
+    }
-                                                 engine::Bearing{270, 10},
+
-                                                 true);
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 11000, engine::Bearing{270, 10}, true);
         BOOST_CHECK_EQUAL(results.size(), 0);
     }
 
     {
-        auto results = query.NearestPhantomNodes(input,
+        auto results = query.NearestPhantomNodes(
-                                                 osrm::engine::Approach::UNRESTRICTED,
+            input, osrm::engine::Approach::UNRESTRICTED, 10, 11000, engine::Bearing{270, 10}, true);
-                                                 boost::none,
+        BOOST_CHECK_EQUAL(results.size(), 0);
-                                                 11000,
+    }
-                                                 engine::Bearing{45, 10},
+
-                                                 true);
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 11000, engine::Bearing{45, 10}, true);
+        BOOST_CHECK_EQUAL(results.size(), 2);
+
+        BOOST_CHECK(results[0].phantom_node.forward_segment_id.enabled);
+        BOOST_CHECK(!results[0].phantom_node.reverse_segment_id.enabled);
+        BOOST_CHECK_EQUAL(results[0].phantom_node.forward_segment_id.id, 1);
+
+        BOOST_CHECK(!results[1].phantom_node.forward_segment_id.enabled);
+        BOOST_CHECK(results[1].phantom_node.reverse_segment_id.enabled);
+        BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_segment_id.id, 1);
+    }
+
+    {
+        auto results = query.NearestPhantomNodes(
+            input, osrm::engine::Approach::UNRESTRICTED, 10, 11000, engine::Bearing{45, 10}, true);
         BOOST_CHECK_EQUAL(results.size(), 2);
 
         BOOST_CHECK(results[0].phantom_node.forward_segment_id.enabled);