From 41c35b0d4f9a9eef798a5c3d8be030502f3c44c0 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:06:18 +0900
Subject: [PATCH] perf PR 8471

---
 .../src/map_based_prediction_node.cpp         | 45 +++++++------------
 1 file changed, 16 insertions(+), 29 deletions(-)

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 6fc9f479c3d03..f740469a50f12 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -359,28 +359,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
                              back_center_point,  r_p_back,  l_p_back};
 }
 
-bool withinLanelet(
-  const TrackedObject & object, const lanelet::ConstLanelet & lanelet,
-  const bool use_yaw_information = false, const float yaw_threshold = 0.6)
-{
-  using Point = boost::geometry::model::d2::point_xy<double>;
-
-  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  const Point p_object{obj_pos.x, obj_pos.y};
-
-  auto polygon = lanelet.polygon2d().basicPolygon();
-  boost::geometry::correct(polygon);
-  bool with_in_polygon = boost::geometry::within(p_object, polygon);
-
-  if (!use_yaw_information) return with_in_polygon;
-
-  // use yaw angle to compare
-  const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet);
-  if (abs_yaw_diff < yaw_threshold) return with_in_polygon;
-
-  return false;
-}
-
 bool withinRoadLanelet(
   const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const bool use_yaw_information = false)
@@ -396,9 +374,12 @@ bool withinRoadLanelet(
   const auto surrounding_lanelets =
     lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
 
-  for (const auto & lanelet : surrounding_lanelets) {
-    if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) {
-      lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
+  for (const auto & lanelet_with_dist : surrounding_lanelets) {
+    const auto & dist = lanelet_with_dist.first;
+    const auto & lanelet = lanelet_with_dist.second;
+
+    if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
+      lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (
         attr.value() == lanelet::AttributeValueString::Crosswalk ||
         attr.value() == lanelet::AttributeValueString::Walkway) {
@@ -406,7 +387,13 @@ bool withinRoadLanelet(
       }
     }
 
-    if (withinLanelet(object, lanelet.second, use_yaw_information)) {
+    constexpr float yaw_threshold = 0.6;
+    bool within_lanelet = std::abs(dist) < 1e-5;
+    if (use_yaw_information) {
+      within_lanelet =
+        within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold;
+    }
+    if (within_lanelet) {
       return true;
     }
   }
@@ -1098,7 +1085,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       attr.value() == lanelet::AttributeValueString::Walkway) {
       const auto & crosswalk = lanelet;
       surrounding_crosswalks.push_back(crosswalk);
-      if (withinLanelet(object, crosswalk)) {
+      if (std::abs(dist) < 1e-5) {
         crossing_crosswalk = crosswalk;
       }
     }
@@ -1184,8 +1171,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, surrounding_lanelets, surrounding_crosswalks, edge_points,
-      prediction_time_horizon_, min_crosswalk_user_velocity_);
+      object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_,
+      min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;