feat(extrinsic_reflector_based_calibrator): add metric plotter for cross validation and add deletion button

.cspell.json

@@ -9,6 +9,9 @@
     "rvec",
     "tvec",
     "rvecs",
-    "tvecs"
+    "tvecs",
+    "nrows",
+    "ncols",
+    "crossval"
   ]
 }

sensor/extrinsic_reflector_based_calibrator/CMakeLists.txt

@@ -28,6 +28,11 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+install(PROGRAMS
+  scripts/metrics_plotter_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_export_dependencies(ament_cmake_python)
 
 ament_auto_package(

sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/calibrator_ui.py

@@ -34,17 +34,24 @@ def __init__(self, ros_interface):
 
         self.extract_background_model_status = False
         self.add_lidar_radar_pair_status = False
+        self.delete_lidar_radar_pair_status = False
         self.send_calibration_status = False
 
         self.ros_interface.set_extract_background_model_callback(
             self.extract_background_model_result_callback,
             self.extract_background_model_status_callback,
         )
+
         self.ros_interface.set_add_lidar_radar_pair_callback(
             self.add_lidar_radar_pair_result_callback,
             self.add_lidar_radar_pair_status_callback,
         )
 
+        self.ros_interface.set_delete_lidar_radar_pair_callback(
+            self.delete_lidar_radar_pair_result_callback,
+            self.delete_lidar_radar_pair_status_callback,
+        )
+
         self.ros_interface.set_send_calibration_callback(
             self.send_calibration_result_callback,
             self.send_calibration_status_callback,
@@ -68,6 +75,13 @@ def __init__(self, ros_interface):
         self.add_lidar_radar_pair_button.clicked.connect(self.add_lidar_radar_pair_button_callback)
         self.layout.addWidget(self.add_lidar_radar_pair_button)
 
+        self.delete_lidar_radar_pair_button = QPushButton("Delete previous lidar-radar pair")
+        self.delete_lidar_radar_pair_button.setEnabled(False)
+        self.delete_lidar_radar_pair_button.clicked.connect(
+            self.delete_lidar_radar_pair_button_callback
+        )
+        self.layout.addWidget(self.delete_lidar_radar_pair_button)
+
         self.send_calibration_button = QPushButton("Send calibration")
         self.send_calibration_button.setEnabled(False)
         self.send_calibration_button.clicked.connect(self.send_calibration_button_callback)
@@ -85,6 +99,9 @@ def check_status(self):
         self.add_lidar_radar_pair_button.setEnabled(
             self.add_lidar_radar_pair_status and not disable_buttons
         )
+        self.delete_lidar_radar_pair_button.setEnabled(
+            self.delete_lidar_radar_pair_status and not disable_buttons
+        )
         self.send_calibration_button.setEnabled(
             self.send_calibration_status and not disable_buttons
         )
@@ -106,6 +123,14 @@ def add_lidar_radar_pair_status_callback(self, status):
         self.add_lidar_radar_pair_status = status
         self.check_status()
 
+    def delete_lidar_radar_pair_result_callback(self, result):
+        self.pending_service = False
+        self.check_status()
+
+    def delete_lidar_radar_pair_status_callback(self, status):
+        self.delete_lidar_radar_pair_status = status
+        self.check_status()
+
     def send_calibration_result_callback(self, result):
         self.pending_service = False
         self.calibration_sent = True
@@ -125,6 +150,11 @@ def add_lidar_radar_pair_button_callback(self):
         self.ros_interface.add_lidar_radar_pair()
         self.check_status()
 
+    def delete_lidar_radar_pair_button_callback(self):
+        self.pending_service = True
+        self.ros_interface.delete_lidar_radar_pair()
+        self.check_status()
+
     def send_calibration_button_callback(self):
         self.pending_service = True
         self.ros_interface.send_calibration()

sensor/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator/ros_interface.py

@@ -67,11 +67,13 @@ def __init__(self):
 
         self.extract_background_model_client = EmptyServiceWrapper(self, "extract_background_model")
         self.add_lidar_radar_pair_client = EmptyServiceWrapper(self, "add_lidar_radar_pair")
+        self.delete_lidar_radar_pair_client = EmptyServiceWrapper(self, "delete_lidar_radar_pair")
         self.send_calibration_client = EmptyServiceWrapper(self, "send_calibration")
 
         self.client_list = [
             self.extract_background_model_client,
             self.add_lidar_radar_pair_client,
+            self.delete_lidar_radar_pair_client,
             self.send_calibration_client,
         ]
 
@@ -87,6 +89,11 @@ def set_add_lidar_radar_pair_callback(self, result_callback, status_callback):
             self.add_lidar_radar_pair_client.set_result_callback(result_callback)
             self.add_lidar_radar_pair_client.set_status_callback(status_callback)
 
+    def set_delete_lidar_radar_pair_callback(self, result_callback, status_callback):
+        with self.lock:
+            self.delete_lidar_radar_pair_client.set_result_callback(result_callback)
+            self.delete_lidar_radar_pair_client.set_status_callback(status_callback)
+
     def set_send_calibration_callback(self, result_callback, status_callback):
         with self.lock:
             self.send_calibration_client.set_result_callback(result_callback)
@@ -98,6 +105,9 @@ def extract_background_model(self):
     def add_lidar_radar_pair(self):
         self.add_lidar_radar_pair_client()
 
+    def delete_lidar_radar_pair(self):
+        self.delete_lidar_radar_pair_client()
+
     def send_calibration(self):
         self.send_calibration_client()
 

sensor/extrinsic_reflector_based_calibrator/include/extrinsic_reflector_based_calibrator/extrinsic_reflector_based_calibrator.hpp

@@ -25,6 +25,7 @@
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <radar_msgs/msg/radar_tracks.hpp>
+#include <std_msgs/msg/float32_multi_array.hpp>
 #include <tier4_calibration_msgs/srv/extrinsic_calibrator.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -41,11 +42,14 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <algorithm>
 #include <cstdint>
 #include <iostream>
 #include <memory>
 #include <mutex>
+#include <random>
 #include <string>
+#include <tuple>
 #include <utility>
 #include <vector>
 
@@ -76,6 +80,10 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     const std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  void deleteTrackRequestCallback(
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    const std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
   void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
   void radarCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg);
 
@@ -107,14 +115,34 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     const std::vector<Eigen::Vector3d> & lidar_detections,
     const std::vector<Eigen::Vector3d> & radar_detections);
 
-  void trackMatches(
+  bool trackMatches(
     const std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> & matches,
     builtin_interfaces::msg::Time & time);
+
+  std::tuple<pcl::PointCloud<PointType>::Ptr, pcl::PointCloud<PointType>::Ptr, double, double>
+  getPointsSetAndDelta();
+  std::pair<double, double> computeCalibrationError(
+    const Eigen::Isometry3d & radar_to_lidar_isometry);
+  void estimateTransformation(
+    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
+    pcl::PointCloud<PointType>::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum);
+  void findCombinations(
+    int n, int k, std::vector<int> & curr, int first_num,
+    std::vector<std::vector<int>> & combinations);
+  void crossValEvaluation(
+    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
+    pcl::PointCloud<PointType>::Ptr radar_points_rcs);
+  void publishMetrics();
   void calibrateSensors();
   void visualizationMarkers(
     const std::vector<Eigen::Vector3d> & lidar_detections,
     const std::vector<Eigen::Vector3d> & radar_detections,
     const std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> & matched_detections);
+  void visualizeTrackMarkers();
+  void deleteTrackMarkers();
+  void drawCalibrationStatusText();
+  geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen);
+  double getYawError(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2);
 
   rcl_interfaces::msg::SetParametersResult paramCallback(
     const std::vector<rclcpp::Parameter> & parameters);
@@ -157,6 +185,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     double max_matching_distance;
     double max_initial_calibration_translation_error;
     double max_initial_calibration_rotation_error;
+    int max_number_of_combination_samples;
   } parameters_;
 
   // ROS Interface
@@ -179,6 +208,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr radar_detections_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr matches_markers_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr tracking_markers_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr text_markers_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr metrics_pub_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub_;
   rclcpp::Subscription<radar_msgs::msg::RadarTracks>::SharedPtr radar_sub_;
@@ -188,6 +219,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr background_model_service_server_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr tracking_service_server_;
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr send_calibration_service_server_;
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr delete_track_service_server_;
 
   // Threading, sync, and result
   std::mutex mutex_;
@@ -230,6 +262,11 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   TrackFactory::Ptr factory_ptr_;
   std::vector<Track> active_tracks_;
   std::vector<Track> converged_tracks_;
+
+  // Metrics
+  std::vector<float> output_metrics_;
+
+  static constexpr int MARKER_SIZE_PER_TRACK = 8;
 };
 
 #endif  // EXTRINSIC_REFLECTOR_BASED_CALIBRATOR__EXTRINSIC_REFLECTOR_BASED_CALIBRATOR_HPP_

sensor/extrinsic_reflector_based_calibrator/launch/calibrator.launch.xml

@@ -50,5 +50,6 @@
     </node>
 
     <node pkg="extrinsic_reflector_based_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"/>
   </group>
 </launch>

sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_center.rviz

@@ -9,8 +9,9 @@ Panels:
         - /lidar_background_pointcloud1/Topic1
         - /lidar_colored_clusters1/Topic1
         - /DetectedObjects1/Topic1
+        - /text_markers1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 1106
+    Tree Height: 752
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -125,7 +126,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -302,7 +303,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: " /matches_markers"
+        Value: /front_unit/front_unit_base_link/front_center/radar_link/matches_markers
       Value: true
     - Alpha: 0.5
       Cell Size: 1
@@ -322,6 +323,19 @@ Visualization Manager:
       Plane Cell Count: 40
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: text_markers
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /front_unit/front_unit_base_link/front_center/radar_link/text_markers
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -378,20 +392,20 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.2697972357273102
       Position:
-        X: 0.19182920455932617
-        Y: 5.7288312911987305
-        Z: 3.2564549446105957
+        X: 0.5868673324584961
+        Y: 18.44519805908203
+        Z: 6.774729251861572
       Target Frame: <Fixed Frame>
       Value: FPS (rviz_default_plugins)
       Yaw: 4.681333541870117
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1403
+  Height: 1043
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000021600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005640000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -400,6 +414,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2486
-  X: 1994
+  Width: 1920
+  X: 2560
   Y: 0

sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_left.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3

sensor/extrinsic_reflector_based_calibrator/rviz/x2_front_right.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3

sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_center.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3

sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_left.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3

sensor/extrinsic_reflector_based_calibrator/rviz/x2_rear_right.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3

sensor/extrinsic_reflector_based_calibrator/rviz/xx1.rviz

@@ -125,7 +125,7 @@ Visualization Manager:
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: lidar_forground_pointcloud
+      Name: lidar_foreground_pointcloud
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3