feat: complete implementation for the radar-lidar, partial implementa…

…tion for the rdv, and others

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

.cspell.json

@@ -13,6 +13,7 @@
     "discretization",
     "distro",
     "eigen",
+    "figsize",
     "gicp",
     "homography",
     "hsize",
@@ -23,6 +24,7 @@
     "lidars",
     "lidartag",
     "lidartags",
+    "matplotlib",
     "matx",
     "ncols",
     "nrows",
@@ -32,12 +34,14 @@
     "pointclouds",
     "prerejective",
     "pydot",
+    "pyplot",
     "quaterniond",
     "ransac",
     "rclcpp",
     "rclpy",
     "registrator",
     "registrators",
+    "remappings",
     "representer",
     "reprojection",
     "rosbag",
@@ -54,6 +58,12 @@
     "undistortion",
     "uniformingly",
     "vectord",
-    "voxel"
+    "voxel",
+    "xaxis",
+    "xlabel",
+    "xlim",
+    "yaxis",
+    "ylabel",
+    "ylim"
   ]
 }

sensor/extrinsic_mapping_based_calibrator/launch/calibrator.launch.xml

@@ -21,6 +21,7 @@
   <arg name="local_map_num_keyframes" default="15"/>
   <arg name="dense_pointcloud_num_keyframes" default="10"/>
   <arg name="mapping_max_frames" default="5000"/>
+  <arg name="mapping_min_range" default="1.0"/>
   <arg name="mapping_max_range" default="100.0"/>
   <arg name="marker_size" default="10.0"/>
   <arg name="mapper_resolution" default="1.0"/>
@@ -86,6 +87,7 @@
       <param name="local_map_num_keyframes" value="$(var local_map_num_keyframes)"/>
       <param name="dense_pointcloud_num_keyframes" value="$(var dense_pointcloud_num_keyframes)"/>
       <param name="mapping_max_frames" value="$(var mapping_max_frames)"/>
+      <param name="mapping_min_range" value="$(var mapping_min_range)"/>
       <param name="mapping_max_range" value="$(var mapping_max_range)"/>
       <param name="marker_size" value="$(var marker_size)"/>
       <param name="mapper_resolution" value="$(var mapper_resolution)"/>

sensor/extrinsic_marker_radar_lidar_calibrator/extrinsic_marker_radar_lidar_calibrator/ros_interface.py

@@ -59,7 +59,7 @@ def __call__(self):
 
 class RosInterface(Node):
     def __init__(self):
-        super().__init__("extrinsic_reflector_based_calibrator_ui")
+        super().__init__("extrinsic_marker_radar_lidar_calibrator_ui")
 
         self.ros_context = None
         self.ros_executor = None

sensor/extrinsic_marker_radar_lidar_calibrator/launch/calibrator.launch.xml

@@ -55,7 +55,7 @@
     </node>
 
     <node pkg="extrinsic_marker_radar_lidar_calibrator" exec="calibrator_ui_node.py" name="calibrator_ui" output="screen"/>
-    <node pkg="extrinsic_reflector_based_calibrator" exec="metrics_plotter_node.py" name="metrics_plotter_node" output="screen"/>
+    <node pkg="extrinsic_marker_radar_lidar_calibrator" exec="metrics_plotter_node.py" name="metrics_plotter_node" output="screen"/>
 
     <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)" if="$(var rviz)">
       <remap from="/pointcloud_topic" to="$(var input_lidar_pointcloud)"/>

sensor/extrinsic_marker_radar_lidar_calibrator/rviz/default.rviz

@@ -8,7 +8,6 @@ Panels:
         - /lidar1/Topic1
         - /lidar_background_pointcloud1/Topic1
         - /lidar_colored_clusters1/Topic1
-        - /DetectedObjects1/Topic1
       Splitter Ratio: 0.5
     Tree Height: 1106
   - Class: rviz_common/Selection
@@ -179,69 +178,20 @@ Visualization Manager:
       Enabled: true
       Name: lidar_detections
       Namespaces:
-        {}
+        "": true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /lidar_detection_markers
       Value: true
-    - BUS:
-        Alpha: 0.9990000128746033
-        Color: 30; 144; 255
-      CAR:
-        Alpha: 0.9990000128746033
-        Color: 30; 144; 255
-      CYCLIST:
-        Alpha: 0.9990000128746033
-        Color: 119; 11; 32
-      Class: autoware_auto_perception_rviz_plugin/DetectedObjects
-      Display Acceleration: true
-      Display Label: true
-      Display PoseWithCovariance: true
-      Display Predicted Path Confidence: true
-      Display Predicted Paths: true
-      Display Twist: true
-      Display UUID: true
-      Display Velocity: true
-      Enabled: true
-      Line Width: 0.029999999329447746
-      MOTORCYCLE:
-        Alpha: 0.9990000128746033
-        Color: 119; 11; 32
-      Name: DetectedObjects
-      Namespaces:
-        label: true
-        shape: true
-        twist: true
-        velocity: true
-      PEDESTRIAN:
-        Alpha: 0.9990000128746033
-        Color: 255; 192; 203
-      Polygon Type: 3d
-      TRAILER:
-        Alpha: 0.9990000128746033
-        Color: 30; 144; 255
-      TRUCK:
-        Alpha: 0.9990000128746033
-        Color: 30; 144; 255
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Best Effort
-        Value: /sensing/radar/front_center/detected_objects
-      UNKNOWN:
-        Alpha: 0.9990000128746033
-        Color: 255; 255; 255
-      Value: true
-      Visualization Type: Normal
     - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Name: radar_detections
       Namespaces:
-        {}
+        center: true
+        line: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -287,7 +237,8 @@ Visualization Manager:
       Enabled: true
       Name: tracking_markers
       Namespaces:
-        {}
+        calibrated: true
+        initial: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -379,14 +330,14 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3347971737384796
+      Pitch: 0.23479722440242767
       Position:
-        X: -5.563827037811279
-        Y: 0.02594861388206482
-        Z: 3.217146873474121
+        X: -6.539778709411621
+        Y: 0.01612010970711708
+        Z: 3.799168348312378
       Target Frame: <Fixed Frame>
       Value: FPS (rviz_default_plugins)
-      Yaw: 6.261345386505127
+      Yaw: 0.0031585693359375
     Saved: ~
 Window Geometry:
   Displays:

sensor/extrinsic_marker_radar_lidar_calibrator/src/extrinsic_marker_radar_lidar_calibrator.cpp

@@ -34,6 +34,7 @@
 #include <iostream>
 #include <limits>
 #include <numeric>
+#include <sstream>
 
 #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME)
 
@@ -49,7 +50,7 @@ void update_param(
   if (it != parameters.cend()) {
     value = it->template get_value<T>();
     RCLCPP_INFO_STREAM(
-      rclcpp::get_logger("extrinsic_reflector_based_calibrator"),
+      rclcpp::get_logger("extrinsic_marker_radar_lidar_calibrator"),
       "Setting parameter [" << name << "] to " << value);
   }
 }
@@ -117,7 +118,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para
 
 ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
   const rclcpp::NodeOptions & options)
-: Node("extrinsic_reflector_based_calibrator_node", options), tf_broadcaster_(this)
+: Node("extrinsic_marker_radar_lidar_calibrator_node", options), tf_broadcaster_(this)
 {
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -301,9 +302,13 @@ void ExtrinsicReflectorBasedCalibrator::requestReceivedCallback(
 
   std::unique_lock<std::mutex> lock(mutex_);
 
+  std::stringstream ss;
+  ss << "Calibration successful. distance_score=" << calibration_distance_score_
+     << " yaw_score=" << calibration_yaw_score_;
+
   tier4_calibration_msgs::msg::CalibrationResult result;
-  result.message.data = "Calibration successful";
-  result.score = 0.f;
+  result.message.data = ss.str();
+  result.score = calibration_distance_score_;
   result.success = true;
   result.transform_stamped = tf2::eigenToTransform(calibrated_radar_to_lidar_eigen_);
   result.transform_stamped.header.frame_id = radar_frame_;
@@ -1013,6 +1018,17 @@ ExtrinsicReflectorBasedCalibrator::matchDetections(
       return transformed_point;
     });
 
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Lidar reflectors in radar coordinate system (using the initial transformation)");
+  for (std::size_t lidar_index = 0; lidar_index < lidar_detections.size(); lidar_index++) {
+    const auto & lidar_detection = lidar_detections_transformed[lidar_index];
+    RCLCPP_INFO(
+      this->get_logger(), "\t Lidar reflector (rcs) id=%lu size=%lu center: x=%.2f y=%.2f z=%.2f",
+      lidar_index, lidar_detections.size(), lidar_detection.x(), lidar_detection.y(),
+      lidar_detection.z());
+  }
+
   std::vector<std::size_t> lidar_to_radar_closest_idx, radar_to_lidar_closest_idx;
   lidar_to_radar_closest_idx.resize(lidar_detections.size());
   radar_to_lidar_closest_idx.resize(radar_detections.size());
@@ -1754,7 +1770,7 @@ void ExtrinsicReflectorBasedCalibrator::drawCalibrationStatusText()
   // show the latest cross validation results which is located in the last two elements of the
   // metrics vector show the latest calibration result, which is located in the 2nd and 3rd index of
   // the metrics vector
-  double m_to_cm = 100.0;
+  constexpr double m_to_cm = 100.0;
 
   if (converged_tracks_.size() == 0) {
     text_marker.text = " pairs=" + std::to_string(converged_tracks_.size());