chore: optimize object distance validation in obstacle_pointcloud_val…

…idator

Signed-off-by: Taekjin LEE <[email protected]>

perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp

@@ -295,7 +295,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
     declare_parameter<std::vector<int64_t>>("max_points_num");
   points_num_threshold_param_.min_points_and_distance_ratio =
     declare_parameter<std::vector<double>>("min_points_and_distance_ratio");
-  validate_max_distance_ = declare_parameter<double>("validate_max_distance_m");
+  const double validate_max_distance = declare_parameter<double>("validate_max_distance_m");
+  validate_max_distance_sq_ = validate_max_distance * validate_max_distance;
 
   using_2d_validator_ = declare_parameter<bool>("using_2d_validator");
 
@@ -348,15 +349,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     const auto & transformed_object = transformed_objects.objects.at(i);
     const auto & object = input_objects->objects.at(i);
     // check object distance
-    const double distance = std::hypot(
-      transformed_object.kinematics.pose_with_covariance.pose.position.x,
-      transformed_object.kinematics.pose_with_covariance.pose.position.y);
-    if (distance > validate_max_distance_) {
+    const double distance_sq =
+      transformed_object.kinematics.pose_with_covariance.pose.position.x *
+        transformed_object.kinematics.pose_with_covariance.pose.position.x +
+      transformed_object.kinematics.pose_with_covariance.pose.position.y *
+        transformed_object.kinematics.pose_with_covariance.pose.position.y;
+    if (distance_sq > validate_max_distance_sq_) {
       // pass to output
       output.objects.push_back(object);
       continue;
     }
-      
+
     const auto validated =
       validation_is_ready ? validator_->validate_object(transformed_object) : false;
     if (debugger_) {

perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp

@@ -152,7 +152,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   typedef message_filters::Synchronizer<SyncPolicy> Sync;
   Sync sync_;
   PointsNumThresholdParam points_num_threshold_param_;
-  double validate_max_distance_; // maximum object distance to validate [m]
+  double validate_max_distance_sq_;  // maximum object distance to validate, squared [m^2]
 
   std::shared_ptr<Debugger> debugger_;
   bool using_2d_validator_;