chore: add clustering estimation algorithm

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

Hide file tree

Showing 9 changed files with 70 additions and 16 deletions.
diff --git a/calibrators/marker_radar_lidar_calibrator/README.md b/calibrators/marker_radar_lidar_calibrator/README.md
@@ -135,8 +135,9 @@ Below, you can see how the algorithm is implemented in the `marker_radar_lidar_c
 | Name                                        | Type          | Default Value                                                 | Description                                                                                                                                                        |
 | ------------------------------------------- | ------------- | ------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
 | `radar_parallel_frame`                      | `std::string` | `base_link`                                                   | Auxiliar frame used in the 2d SVD-based method.                                                                                                                    |
-| `msg_type`                                  | `std::string` | `radar tracks` / `radar scan`                                 | The type of input radar objects. (Not available yet, currently only support radar tracks.)                                                                         |
-| `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. (Not available yet.)                             |
+| `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
@@ -38,12 +38,10 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <algorithm>
 #include <cstdint>
 #include <limits>
 #include <memory>
 #include <mutex>
-#include <random>
 #include <string>
 #include <tuple>
 #include <utility>
@@ -73,6 +71,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 
   enum class MsgType { radar_tracks, radar_scan, radar_cloud };
 
+  enum class CornerReflectorEstimationMethod { average_points, longest_distance_point };
+
   explicit ExtrinsicReflectorBasedCalibrator(const rclcpp::NodeOptions & options);
 
 protected:
@@ -313,6 +313,7 @@ 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,6 +9,7 @@
   <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"/>
@@ -34,6 +35,7 @@
       <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
@@ -30,6 +30,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2/utils.h>
 
+#include <random>
 #include <chrono>
 #include <cstddef>
 #include <iostream>
@@ -197,10 +198,11 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
   parameters_.max_number_of_combination_samples = static_cast<std::size_t>(
     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", 20));
+    this->declare_parameter<int>("match_count_for_convergence", 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;
@@ -224,6 +226,14 @@ 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 =
@@ -1047,16 +1057,31 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFr
       tree_ptr->radiusSearch(
         highest_point, parameters_.reflector_radius, indexes, squared_distances) > 0) {
 
-      //  Locate the center of the reflector at the maximum distance
       Eigen::Vector3d center = Eigen::Vector3d::Zero();
-      double max_distance = -std::numeric_limits<double>::infinity();
-      for (const auto & index : indexes) {
-        const auto & point = cluster_pointcloud_ptr->points[index];
-        const auto point_xy_distance = std::hypot(point.x, point.y);
-        if (point_xy_distance > max_distance) {
-          max_distance = point_xy_distance;        
-          center = Eigen::Vector3d(point.x, point.y, point.z);
+
+      if(corner_reflector_estimation_method_ == CornerReflectorEstimationMethod::longest_distance_point) {
+        //  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) {
+          const auto & point = cluster_pointcloud_ptr->points[index];
+          const Eigen::Vector3d point_in_lidar_frame = Eigen::Vector3d(point.x, point.y, point.z);
+          const Eigen::Vector3d point_in_radar_frame = initial_radar_to_lidar_eigen_ * point_in_lidar_frame;
+
+          const auto point_xy_distance = std::hypot(point_in_radar_frame.x(), point_in_radar_frame.y());
+          if (point_xy_distance > max_distance) {
+            max_distance = point_xy_distance;
+            center = initial_radar_to_lidar_eigen_.inverse() * point_in_radar_frame;        
+          }
+        }
+      }
+      else if(corner_reflector_estimation_method_ == CornerReflectorEstimationMethod::average_points) {
+        // 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];
+          center += Eigen::Vector3d(p.x, p.y, p.z);
         }
+
+        center /= indexes.size();
       }
 
       RCLCPP_INFO(
@@ -1295,10 +1320,11 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches(
   current_new_tracks_ = converging_tracks_.size();
   tracking_active_ = false;
   count_ = 0;
+  converging_tracks_.clear();
+
   if(!current_new_tracks_) {
     return false;
   }
-  converging_tracks_.clear();
   return true;
 }
 

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,6 +7,7 @@
   <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"/>
@@ -22,5 +23,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/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml
@@ -22,13 +22,16 @@
     <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"/>
   <let name="radar_frame" value="$(var radar_name)/radar_link"/>
   <let name="lidar_frame" value="pandar_top"/>
-  <let name="msg_type" value="$(var msg_type)"/>
-  <let name="transformation_type" value="$(var transformation_type)"/>
 
   <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/objects_raw" if="$(eval &quot;'$(var msg_type)'=='radar_tracks'&quot;)"/>
   <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/scan_raw" if="$(eval &quot;'$(var msg_type)'=='radar_scan'&quot;)"/>
@@ -46,5 +49,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/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml
@@ -22,6 +22,11 @@
     <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,5 +69,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,6 +22,11 @@
     <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-->
@@ -59,5 +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/xx1_gen2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml
@@ -22,6 +22,11 @@
     <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"/>
@@ -47,5 +52,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>