From d259848456c8c84df0422f7389ef034e63e6462d Mon Sep 17 00:00:00 2001 From: Siarhei Fedartsou Date: Mon, 20 May 2024 12:32:40 +0200 Subject: [PATCH] Optimise R-tree queries in the case of map matching (#6881) --- CHANGELOG.md | 1 + .../contiguous_internalmem_datafacade.hpp | 2 +- include/engine/geospatial_query.hpp | 38 +++- include/util/coordinate_calculation.hpp | 7 + include/util/rectangle.hpp | 13 +- include/util/static_rtree.hpp | 175 +++++++++++------- src/benchmarks/match.cpp | 1 + unit_tests/util/static_rtree.cpp | 68 +++++-- 8 files changed, 219 insertions(+), 86 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a736d3040..72cea29b9 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/include/engine/datafacade/contiguous_internalmem_datafacade.hpp b/include/engine/datafacade/contiguous_internalmem_datafacade.hpp index 9b35989e1..ac9497eb0 100644 --- a/include/engine/datafacade/contiguous_internalmem_datafacade.hpp +++ b/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 diff --git a/include/engine/geospatial_query.hpp b/include/engine/geospatial_query.hpp index b8e43b637..2fce9c0c9 100644 --- a/include/engine/geospatial_query.hpp +++ b/include/engine/geospatial_query.hpp @@ -47,12 +47,42 @@ template class GeospatialQuery return rtree.SearchInBox(bbox); } + std::vector + NearestPhantomNodes(const util::Coordinate input_coordinate, + const Approach approach, + const double max_distance, + const boost::optional bearing_with_range, + const boost::optional 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 NearestPhantomNodes(const util::Coordinate input_coordinate, const Approach approach, - const boost::optional max_results, + const size_t max_results, const boost::optional max_distance, const boost::optional bearing_with_range, const boost::optional use_all_edges) const @@ -70,10 +100,10 @@ template class GeospatialQuery : std::make_pair(true, true)); return valid; }, - [this, &max_distance, &max_results, input_coordinate](const std::size_t num_results, - const CandidateSegment &segment) + [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)); }); diff --git a/include/util/coordinate_calculation.hpp b/include/util/coordinate_calculation.hpp index 3ab8db076..6a716ffaf 100644 --- a/include/util/coordinate_calculation.hpp +++ b/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(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); diff --git a/include/util/rectangle.hpp b/include/util/rectangle.hpp index 65050f200..aca192446 100644 --- a/include/util/rectangle.hpp +++ b/include/util/rectangle.hpp @@ -3,7 +3,6 @@ #include "util/coordinate.hpp" #include "util/coordinate_calculation.hpp" - #include #include @@ -168,6 +167,18 @@ struct RectangleInt2D min_lat != FixedLatitude{std::numeric_limits::max()} && max_lat != FixedLatitude{std::numeric_limits::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 diff --git a/include/util/static_rtree.hpp b/include/util/static_rtree.hpp index d39d8454d..dc88da233 100644 --- a/include/util/static_rtree.hpp +++ b/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 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 results; - - std::queue 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 ¤t_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])); - } - } - } - } + SearchInBox(search_rectangle, + [&results](const auto &edge_data) { results.push_back(edge_data); }); 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 + std::vector 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 results; + + SearchInBox( + bbox, + [&results, &filter, fixed_projected_coordinate, this](const EdgeDataT ¤t_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 std::vector Nearest(const Coordinate input_coordinate, @@ -572,8 +550,10 @@ class StaticRTree const TerminationT terminate) const { std::vector results; + auto projected_coordinate = web_mercator::fromWGS84(input_coordinate); Coordinate fixed_projected_coordinate{projected_coordinate}; + // initialize queue with root element std::priority_queue traversal_queue; traversal_queue.push(QueryCandidate{0, TreeIndex{}}); @@ -631,6 +611,73 @@ class StaticRTree } private: + template + 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 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 ¤t_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 diff --git a/src/benchmarks/match.cpp b/src/benchmarks/match.cpp index 83ef7ef1b..cece7960b 100644 --- a/src/benchmarks/match.cpp +++ b/src/benchmarks/match.cpp @@ -11,6 +11,7 @@ #include "osrm/status.hpp" #include + #include #include #include diff --git a/unit_tests/util/static_rtree.cpp b/unit_tests/util/static_rtree.cpp index f46aa7387..812202d38 100644 --- a/unit_tests/util/static_rtree.cpp +++ b/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, - osrm::engine::Approach::UNRESTRICTED, - boost::none, - 11000, - engine::Bearing{270, 10}, - true); + auto results = query.NearestPhantomNodes( + input, osrm::engine::Approach::UNRESTRICTED, 10, 11000, boost::none, true); + BOOST_CHECK_EQUAL(results.size(), 2); + } + + { + 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, - osrm::engine::Approach::UNRESTRICTED, - boost::none, - 11000, - engine::Bearing{45, 10}, - true); + auto results = query.NearestPhantomNodes( + input, osrm::engine::Approach::UNRESTRICTED, 10, 11000, engine::Bearing{270, 10}, true); + BOOST_CHECK_EQUAL(results.size(), 0); + } + + { + 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);