chore: remove method type to number of points

Signed-off-by: vividf <[email protected]>
vividf · Nov 18, 2024 · 62fcf1e · 62fcf1e
1 parent 3143178
commit 62fcf1e
Show file tree

Hide file tree

Showing 9 changed files with 6 additions and 41 deletions.
diff --git a/calibrators/marker_radar_lidar_calibrator/README.md b/calibrators/marker_radar_lidar_calibrator/README.md
@@ -137,7 +137,6 @@ Below, you can see how the algorithm is implemented in the `marker_radar_lidar_c
 | `radar_parallel_frame`                      | `std::string` | `base_link`                                                   | Auxiliar frame used in the 2d SVD-based method.                                                                                                                    |
 | `msg_type`                                  | `std::string` | `radar_tracks` / `radar_scan` / `radar_cloud `                                 | The type of input radar objects.                                                                         |
 | `transformation_type`                       | `std::string` | `yaw_only_rotation_2d` / `svd_2d` / `svd_3d` / `roll_zero_3d` | Specifies the algorithm used to optimize the transformation between the radar frame and the radar parallel frame.                             |
-| `corner_reflector_estimation_method`                                  | `std::string` | `longest_distance_point` / `average_points`                                 | The method for estimating the center of a corner reflector from LiDAR point clouds.                                                                         |
 | `use_lidar_initial_crop_box_filter`         | `bool`        | `true`                                                        | Enables or disables the initial cropping filter for lidar data processing.                                                                                         |
 | `lidar_initial_crop_box_min_x`              | `double`      | `-50.0`                                                       | Minimum x-coordinate in meters for the initial lidar calibration area.                                                                                             |
 | `lidar_initial_crop_box_min_y`              | `double`      | `-50.0`                                                       | Minimum y-coordinate in meters for the initial lidar calibration area.                                                                                             |

diff --git a/..._lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/..._lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp
@@ -222,6 +222,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     double max_initial_calibration_rotation_error;
     std::size_t max_number_of_combination_samples;
     int match_count_for_convergence;
+    std::size_t reflector_points_threshold;
   } parameters_;
 
   // ROS Interface
@@ -313,7 +314,6 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 
   MsgType msg_type_;
   TransformationType transformation_type_;
-  CornerReflectorEstimationMethod corner_reflector_estimation_method_;
   static constexpr int MARKER_SIZE_PER_TRACK = 8;
 };
 

diff --git a/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml b/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml
@@ -9,7 +9,6 @@
   <arg name="input_radar_msg" default="/sensing/radar/front_center/objects_raw"/>
   <arg name="msg_type" default="objects_raw"/>
   <arg name="transformation_type" default="svd_2d"/>
-  <arg name="corner_reflector_estimation_method" default="longest_distance_point"/>
 
   <arg name="use_lidar_initial_crop_box_filter" default="false"/>
   <arg name="lidar_initial_crop_box_min_x" default="-50.0"/>
@@ -35,7 +34,6 @@
       <param name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
       <param name="msg_type" value="$(var msg_type)"/>
       <param name="transformation_type" value="$(var transformation_type)"/>
-      <param name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
 
       <remap from="extrinsic_calibration" to="$(var calibration_service_name)"/>
       <remap from="input_lidar_pointcloud" to="$(var input_lidar_pointcloud)"/>

diff --git a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp
@@ -199,11 +199,11 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
     this->declare_parameter<int>("max_number_of_combination_samples", 2000));
   parameters_.match_count_for_convergence = static_cast<std::size_t>(
     this->declare_parameter<int>("match_count_for_convergence", 10));
+  parameters_.reflector_points_threshold = static_cast<int>(
+    this->declare_parameter<int>("reflector_points_threshold", 10));
 
   auto msg_type = this->declare_parameter<std::string>("msg_type");
   auto transformation_type = this->declare_parameter<std::string>("transformation_type");
-  auto corner_reflector_estimation_method = this->declare_parameter<std::string>("corner_reflector_estimation_method");
-
   if (msg_type == "radar_tracks") {
     msg_type_ = MsgType::radar_tracks;
   } else if (msg_type == "radar_scan") {
@@ -226,14 +226,6 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
     throw std::runtime_error("Invalid param value: " + transformation_type);
   }
 
-  if (corner_reflector_estimation_method == "average_points") {
-    corner_reflector_estimation_method_ = CornerReflectorEstimationMethod::average_points;
-  } else if (corner_reflector_estimation_method == "longest_distance_point") {
-    corner_reflector_estimation_method_ = CornerReflectorEstimationMethod::longest_distance_point;
-  } else {
-    throw std::runtime_error("Invalid param value: " + corner_reflector_estimation_method);
-  }
-
   parameters_.max_initial_calibration_translation_error =
     this->declare_parameter<double>("max_initial_calibration_translation_error", 1.0);
   parameters_.max_initial_calibration_rotation_error =
@@ -1059,7 +1051,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFr
 
       Eigen::Vector3d center = Eigen::Vector3d::Zero();
 
-      if(corner_reflector_estimation_method_ == CornerReflectorEstimationMethod::longest_distance_point) {
+      if(cluster_pointcloud_ptr->points.size() > parameters_.reflector_points_threshold) {
         //  Locate the center of the reflector at the maximum distance (This works better for high resolution LiDARs)
         double max_distance = -std::numeric_limits<double>::infinity();
         for (const auto & index : indexes) {
@@ -1074,7 +1066,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFr
           }
         }
       }
-      else if(corner_reflector_estimation_method_ == CornerReflectorEstimationMethod::average_points) {
+      else {
         // Locate the center of the reflector by averaging all of the points in the cluster
         for (const auto & index : indexes) {
           const auto & p = cluster_pointcloud_ptr->points[index];

diff --git a/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml
@@ -7,7 +7,6 @@
   <arg name="lidar_frame"/>
   <arg name="msg_type"/>
   <arg name="transformation_type"/>
-  <arg name="corner_reflector_estimation_method"/>
 
   <arg name="input_radar_msg"/>
   <arg name="input_lidar_pointcloud"/>
@@ -23,6 +22,5 @@
     <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
     <arg name="msg_type" value="$(var msg_type)"/>
     <arg name="transformation_type" value="$(var transformation_type)"/>
-    <arg name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml
@@ -22,10 +22,6 @@
     <choice value="zero_roll_3d"/>
   </arg>
 
-  <arg name="corner_reflector_estimation_method" default="longest_distance_point">
-    <choice value="longest_distance_point"/>
-    <choice value="average_points"/>
-  </arg>
 
   <arg name="rviz" default="true"/>
 
@@ -49,6 +45,5 @@
     <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
     <arg name="msg_type" value="$(var msg_type)"/>
     <arg name="transformation_type" value="$(var transformation_type)"/>
-    <arg name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml
@@ -22,11 +22,6 @@
     <choice value="zero_roll_3d"/>
   </arg>
 
-  <arg name="corner_reflector_estimation_method" default="longest_distance_point">
-    <choice value="longest_distance_point"/>
-    <choice value="average_points"/>
-  </arg>
-
   <arg name="rviz" default="true"/>
 
   <!-- For x2 project, do not change radar_optimization_frame to base_link, if you are using radar that doesn't support elevation or z axis-->
@@ -69,6 +64,6 @@
     <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
     <arg name="msg_type" value="$(var msg_type)"/>
     <arg name="transformation_type" value="$(var transformation_type)"/>
-    <arg name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
+
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/x2_gen2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/x2_gen2/marker_radar_lidar_calibrator.launch.xml
@@ -22,11 +22,6 @@
     <choice value="zero_roll_3d"/>
   </arg>
 
-  <arg name="corner_reflector_estimation_method" default="longest_distance_point">
-    <choice value="longest_distance_point"/>
-    <choice value="average_points"/>
-  </arg>
-
   <arg name="rviz" default="true"/>
 
   <!-- For x2 project, do not change radar_optimization_frame to base_link, if you are using radar that doesn't support elevation or z axis-->
@@ -64,6 +59,5 @@
     <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
     <arg name="msg_type" value="$(var msg_type)"/>
     <arg name="transformation_type" value="$(var transformation_type)"/>
-    <arg name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml
@@ -22,11 +22,6 @@
     <choice value="zero_roll_3d"/>
   </arg>
 
-  <arg name="corner_reflector_estimation_method" default="longest_distance_point">
-    <choice value="longest_distance_point"/>
-    <choice value="average_points"/>
-  </arg>
-
   <arg name="rviz" default="true"/>
 
   <let name="radar_optimization_frame" value="base_link"/>
@@ -52,6 +47,5 @@
     <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
     <arg name="msg_type" value="$(var msg_type)"/>
     <arg name="transformation_type" value="$(var transformation_type)"/>
-    <arg name="corner_reflector_estimation_method" value="$(var corner_reflector_estimation_method)"/>
   </include>
 </launch>