T4 garage xx1 experiment

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp

@@ -41,7 +41,6 @@
 
 #include <algorithm>
 #include <cstdint>
-#include <iostream>
 #include <limits>
 #include <memory>
 #include <mutex>
@@ -54,10 +53,16 @@
 namespace marker_radar_lidar_calibrator
 {
 
+struct TransformationResult
+{
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs;
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs;
+  Eigen::Isometry3d calibrated_radar_to_lidar_transformation;
+};
+
 class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 {
 public:
-  using PointType = pcl::PointXYZ;
   using index_t = std::uint32_t;
   enum class TransformationType { svd_2d, yaw_only_rotation_2d, svd_3d, zero_roll_3d };
 
@@ -97,29 +102,31 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   void radarCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
 
   template <typename RadarMsgType>
-  pcl::PointCloud<PointType>::Ptr extractRadarPointcloud(const std::shared_ptr<RadarMsgType> & msg);
+  pcl::PointCloud<common_types::PointType>::Ptr extractRadarPointcloud(
+    const std::shared_ptr<RadarMsgType> & msg);
 
   std::vector<Eigen::Vector3d> extractLidarReflectors(
     const sensor_msgs::msg::PointCloud2::SharedPtr msg);
   std::vector<Eigen::Vector3d> extractRadarReflectors(
-    pcl::PointCloud<PointType>::Ptr radar_pointcloud_ptr);
+    pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr);
 
   void extractBackgroundModel(
-    const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud,
+    const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud,
     const std_msgs::msg::Header & current_header, std_msgs::msg::Header & last_updated_header,
     std_msgs::msg::Header & first_header, BackgroundModel & background_model);
 
   void extractForegroundPoints(
-    const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud,
+    const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud,
     const BackgroundModel & background_model, bool use_ransac,
-    pcl::PointCloud<PointType>::Ptr & foreground_points, Eigen::Vector4d & ground_model);
+    pcl::PointCloud<common_types::PointType>::Ptr & foreground_points,
+    Eigen::Vector4d & ground_model);
 
-  std::vector<pcl::PointCloud<PointType>::Ptr> extractClusters(
-    const pcl::PointCloud<PointType>::Ptr & foreground_pointcloud,
+  std::vector<pcl::PointCloud<common_types::PointType>::Ptr> extractClusters(
+    const pcl::PointCloud<common_types::PointType>::Ptr & foreground_pointcloud,
     const double cluster_max_tolerance, const int cluster_min_points, const int cluster_max_points);
 
   std::vector<Eigen::Vector3d> findReflectorsFromClusters(
-    const std::vector<pcl::PointCloud<PointType>::Ptr> & clusters,
+    const std::vector<pcl::PointCloud<common_types::PointType>::Ptr> & clusters,
     const Eigen::Vector4d & ground_model);
 
   bool checkInitialTransforms();
@@ -132,20 +139,26 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     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> getPointsSet();
-  std::tuple<double, double> getDelta(std::vector<Track> converged_tracks, bool is_crossval);
+  std::tuple<
+    pcl::PointCloud<common_types::PointType>::Ptr, pcl::PointCloud<common_types::PointType>::Ptr>
+  getPointsSet();
+  std::tuple<double, double> get2DRotationDelta(
+    std::vector<Track> converged_tracks, bool is_crossval);
 
   std::pair<double, double> computeCalibrationError(
     const Eigen::Isometry3d & radar_to_lidar_isometry);
-  void estimateTransformation();
-  void calculateCalibrationError(Eigen::Isometry3d calibrated_radar_to_lidar_transformation);
-  void crossValEvaluation();
+  TransformationResult estimateTransformation();
+  void evaluateTransformation(Eigen::Isometry3d calibrated_radar_to_lidar_transformation);
+  void crossValEvaluation(TransformationResult transformation_result);
   void findCombinations(
-    int n, int k, std::vector<int> & curr, int first_num,
-    std::vector<std::vector<int>> & combinations);
+    std::size_t n, std::size_t k, std::vector<std::size_t> & curr, std::size_t first_num,
+    std::vector<std::vector<std::size_t>> & combinations);
   void selectCombinations(
-    int tracks_size, int num_of_samples, std::vector<std::vector<int>> & combinations);
-  void doEvaluation(std::vector<std::vector<int>> & combinations, int num_of_samples);
+    std::size_t tracks_size, std::size_t num_of_samples,
+    std::vector<std::vector<std::size_t>> & combinations);
+  void evaluateCombinations(
+    std::vector<std::vector<std::size_t>> & combinations, std::size_t num_of_samples,
+    TransformationResult transformation_result);
 
   void publishMetrics();
   void calibrateSensors();
@@ -164,9 +177,10 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 
   struct Parameters
   {
-    std::string radar_optimization_frame;  // frame that is assumed to be parallel to the radar
-                                           // if estimating the transformation by 2d algorithms
-                                           // (needed for radars that do not provide elevation)
+    std::string radar_optimization_frame;  // If the radar does not provide elevation,
+                                           // this frame needs to be parallel to the radar
+                                           // and should only use the 2D transformation.
+
     bool use_lidar_initial_crop_box_filter;
     double lidar_initial_crop_box_min_x;
     double lidar_initial_crop_box_min_y;
@@ -201,7 +215,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;
+    std::size_t max_number_of_combination_samples;
   } parameters_;
 
   // ROS Interface
@@ -256,7 +270,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   geometry_msgs::msg::Transform radar_optimization_to_lidar_msg_;
   Eigen::Isometry3d radar_optimization_to_lidar_eigen_;
 
-  geometry_msgs::msg::Transform init_radar_optimization_to_radar_msg_;
+  geometry_msgs::msg::Transform initial_radar_optimization_to_radar_msg_;
   Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_;
 
   bool got_initial_transform_{false};
@@ -288,10 +302,6 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   std::vector<Track> active_tracks_;
   std::vector<Track> converged_tracks_;
 
-  // Converged points
-  pcl::PointCloud<PointType>::Ptr lidar_points_ocs_;
-  pcl::PointCloud<PointType>::Ptr radar_points_rcs_;
-
   // Metrics
   std::vector<float> output_metrics_;
 

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp

@@ -18,6 +18,7 @@
 #include <Eigen/Dense>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp>
+#include <marker_radar_lidar_calibrator/types.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
@@ -35,15 +36,14 @@ namespace marker_radar_lidar_calibrator
 class TransformationEstimator
 {
 public:
-  using PointType = pcl::PointXYZ;
   TransformationEstimator(
     Eigen::Isometry3d initial_radar_to_lidar_eigen,
     Eigen::Isometry3d initial_radar_to_radar_optimization_eigen,
     Eigen::Isometry3d radar_optimization_to_lidar_eigen);
   void setPoints(
-    pcl::PointCloud<PointType>::Ptr lidar_points_ocs,
-    pcl::PointCloud<PointType>::Ptr radar_points_rcs);
-  void setDelta(double delta_cos, double delta_sin);
+    pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs,
+    pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs);
+  void set2DRotationDelta(double delta_cos, double delta_sin);
   void estimateYawOnlyTransformation();
   void estimateSVDTransformation(
     ExtrinsicReflectorBasedCalibrator::TransformationType transformation_type);
@@ -52,8 +52,8 @@ class TransformationEstimator
 
   double delta_cos_;
   double delta_sin_;
-  pcl::PointCloud<PointType>::Ptr lidar_points_ocs_;
-  pcl::PointCloud<PointType>::Ptr radar_points_rcs_;
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs_;
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs_;
   Eigen::Isometry3d calibrated_radar_to_lidar_transformation_;
 
   Eigen::Isometry3d initial_radar_to_lidar_eigen_;

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp

@@ -28,11 +28,15 @@
 namespace marker_radar_lidar_calibrator
 {
 
+namespace common_types
+{
+using PointType = pcl::PointXYZ;
+}
+
 struct BackgroundModel
 {
 public:
-  using PointType = pcl::PointXYZ;
-  using TreeType = pcl::KdTreeFLANN<PointType>;  // cSpell:ignore FLANN
+  using TreeType = pcl::KdTreeFLANN<common_types::PointType>;  // cSpell:ignore FLANN
   using index_t = std::uint32_t;
 
   BackgroundModel()
@@ -44,7 +48,7 @@ struct BackgroundModel
     max_point_(
       -std::numeric_limits<float>::max(), -std::numeric_limits<float>::max(),
       -std::numeric_limits<float>::max(), 1.f),
-    pointcloud_(new pcl::PointCloud<PointType>)
+    pointcloud_(new pcl::PointCloud<common_types::PointType>)
   {
   }
 
@@ -53,7 +57,7 @@ struct BackgroundModel
   Eigen::Vector4f min_point_;
   Eigen::Vector4f max_point_;
   std::unordered_set<index_t> set_;
-  pcl::PointCloud<PointType>::Ptr pointcloud_;
+  pcl::PointCloud<common_types::PointType>::Ptr pointcloud_;
   TreeType tree_;
 };
 

calibrators/marker_radar_lidar_calibrator/rviz/default.rviz

@@ -9,7 +9,7 @@ Panels:
         - /lidar_background_pointcloud1/Topic1
         - /lidar_colored_clusters1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 1106
+    Tree Height: 725
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -27,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: lidar
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -178,7 +178,7 @@ Visualization Manager:
       Enabled: true
       Name: lidar_detections
       Namespaces:
-        "": true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -190,8 +190,7 @@ Visualization Manager:
       Enabled: true
       Name: radar_detections
       Namespaces:
-        center: true
-        line: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -237,8 +236,7 @@ Visualization Manager:
       Enabled: true
       Name: tracking_markers
       Namespaces:
-        calibrated: true
-        initial: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -258,14 +256,15 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /matches_markers
       Value: true
-    - Class: rviz_default_plugins/MarkerArray
+    - 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: Reliable
         Value: /text_markers
@@ -344,20 +343,20 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.23479722440242767
       Position:
-        X: -6.539778709411621
-        Y: 0.01612010970711708
-        Z: 3.799168348312378
+        X: -17.043386459350586
+        Y: -0.017056670039892197
+        Z: 6.311741828918457
       Target Frame: <Fixed Frame>
       Value: FPS (rviz_default_plugins)
       Yaw: 0.0031585693359375
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1403
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002160000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000051e0000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -366,6 +365,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2486
-  X: 1994
-  Y: 0
+  Width: 1850
+  X: 2630
+  Y: 547