From 7be4ad863bc676cd2b6b3102cf738bc84558ed59 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Tue, 4 Jun 2024 10:23:03 +0900 Subject: [PATCH] perf(lanelet2_extension): more efficient getLinkedParkingLot Signed-off-by: Maxime CLEMENT --- .../lanelet2_extension/utility/query.hpp | 2 +- tmp/lanelet2_extension/lib/query.cpp | 19 ++++++++++++------- tmp/lanelet2_extension_python/src/utility.cpp | 6 +++--- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index b094a2b5..4d9a2998 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -189,7 +189,7 @@ bool getLinkedParkingLot( lanelet::ConstPolygon3d * linked_parking_lot); // get linked parking lot from current pose of ego car bool getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstPolygon3d * linked_parking_lot); // get linked parking lot from parking space bool getLinkedParkingLot( diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index a505c12e..f4ba7cbe 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -591,15 +591,20 @@ bool query::getLinkedParkingLot( // get overlapping parking lot bool query::getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, + const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::ConstPolygon3d * linked_parking_lot) { - for (const auto & parking_lot : all_parking_lots) { - const double distance = - boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon()); - if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + const auto candidates = + lanelet_map_ptr->polygonLayer.search(lanelet::geometry::boundingBox2d(current_position)); + for (const auto & candidate : candidates) { + const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == "parking_lot") { + const double distance = + boost::geometry::distance(current_position, to2D(candidate).basicPolygon()); + if (distance < std::numeric_limits::epsilon()) { + *linked_parking_lot = candidate; + return true; + } } } return false; diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp index fdd612f1..469631da 100644 --- a/tmp/lanelet2_extension_python/src/utility.cpp +++ b/tmp/lanelet2_extension_python/src/utility.cpp @@ -209,11 +209,11 @@ lanelet::Optional getLinkedParkingLot( } lanelet::Optional getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots) + const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr lanelet_map_ptr) { lanelet::ConstPolygon3d linked_parking_lot; if (lanelet::utils::query::getLinkedParkingLot( - current_position, all_parking_lots, &linked_parking_lot)) { + current_position, lanelet_map_ptr, &linked_parking_lot)) { return linked_parking_lot; } else { return {}; @@ -506,7 +506,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility) const lanelet::ConstLanelet &, const lanelet::ConstPolygons3d &)>( "getLinkedParkingLot", ::getLinkedParkingLot); bp::def( - const lanelet::BasicPoint2d &, const lanelet::ConstPolygons3d &)>( + const lanelet::BasicPoint2d &, const lanelet::LaneletMapPtr)>( "getLinkedParkingLot", ::getLinkedParkingLot); bp::def( const lanelet::ConstLineString3d &, const lanelet::ConstPolygons3d &)>(