feat(marker_radar_lidar_calibrator): marker_radar_lidar_calibrator support for different radar msgs and transformation algorithms

calibrators/marker_radar_lidar_calibrator/CMakeLists.txt

@@ -3,32 +3,25 @@ cmake_minimum_required(VERSION 3.5)
 project(marker_radar_lidar_calibrator)
 
 find_package(autoware_cmake REQUIRED)
-
 autoware_package()
 
-ament_python_install_package(${PROJECT_NAME})
+find_package(Ceres REQUIRED)
 
-ament_export_include_directories(
-  include
-  ${OpenCV_INCLUDE_DIRS}
-)
+ament_python_install_package(${PROJECT_NAME})
 
 ament_auto_add_executable(marker_radar_lidar_calibrator
   src/marker_radar_lidar_calibrator.cpp
+  src/transformation_estimator.cpp
   src/track.cpp
   src/main.cpp
 )
 
 target_link_libraries(marker_radar_lidar_calibrator
-  ${OpenCV_LIBS}
+  ${CERES_LIBRARIES}
 )
 
 install(PROGRAMS
   scripts/calibrator_ui_node.py
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-install(PROGRAMS
   scripts/metrics_plotter_node.py
   DESTINATION lib/${PROJECT_NAME}
 )

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp

@@ -24,6 +24,7 @@
 #include <std_srvs/srv/empty.hpp>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <radar_msgs/msg/radar_scan.hpp>
 #include <radar_msgs/msg/radar_tracks.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
@@ -40,7 +41,6 @@
 
 #include <algorithm>
 #include <cstdint>
-#include <iostream>
 #include <limits>
 #include <memory>
 #include <mutex>
@@ -53,11 +53,20 @@
 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 };
+
+  enum class MsgType { radar_tracks, radar_scan, radar_cloud };
 
   explicit ExtrinsicReflectorBasedCalibrator(const rclcpp::NodeOptions & options);
 
@@ -85,28 +94,39 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     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);
 
-  std::vector<Eigen::Vector3d> extractReflectors(
+  void radarTracksCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg);
+
+  void radarScanCallback(const radar_msgs::msg::RadarScan::SharedPtr msg);
+
+  void radarCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+
+  template <typename RadarMsgType>
+  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> extractReflectors(const radar_msgs::msg::RadarTracks::SharedPtr msg);
+  std::vector<Eigen::Vector3d> extractRadarReflectors(
+    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::Vector4f & ground_model);
+    pcl::PointCloud<common_types::PointType>::Ptr & foreground_points,
+    Eigen::Vector4f & 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::Vector4f & ground_model);
 
   bool checkInitialTransforms();
@@ -119,19 +139,27 @@ 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, double, double>
-  getPointsSetAndDelta();
+  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(
-    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-    pcl::PointCloud<PointType>::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum);
+  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);
-  void crossValEvaluation(
-    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-    pcl::PointCloud<PointType>::Ptr radar_points_rcs);
+    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(
+    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();
   void visualizationMarkers(
@@ -142,15 +170,18 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   void deleteTrackMarkers();
   void drawCalibrationStatusText();
   geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen);
-  double getYawError(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2);
+  double getDistanceError(const Eigen::Vector3d v1, const Eigen::Vector3d v2);
+  double getYawError(const Eigen::Vector3d v1, const Eigen::Vector3d v2);
 
   rcl_interfaces::msg::SetParametersResult paramCallback(
     const std::vector<rclcpp::Parameter> & parameters);
 
   struct Parameters
   {
-    std::string radar_parallel_frame;  // frame that is assumed to be parallel to the radar (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;
@@ -185,7 +216,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
@@ -212,7 +243,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   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_;
+  rclcpp::Subscription<radar_msgs::msg::RadarTracks>::SharedPtr radar_tracks_sub_;
+  rclcpp::Subscription<radar_msgs::msg::RadarScan>::SharedPtr radar_scan_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr radar_cloud_sub_;
 
   rclcpp::Service<tier4_calibration_msgs::srv::ExtrinsicCalibrator>::SharedPtr
     calibration_request_server_;
@@ -233,8 +266,12 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   Eigen::Isometry3d initial_radar_to_lidar_eigen_;
   Eigen::Isometry3d calibrated_radar_to_lidar_eigen_;
 
-  geometry_msgs::msg::Transform radar_parallel_to_lidar_msg_;
-  Eigen::Isometry3d radar_parallel_to_lidar_eigen_;
+  // radar optimization is the frame that radar optimize the transformation to.
+  geometry_msgs::msg::Transform radar_optimization_to_lidar_msg_;
+  Eigen::Isometry3d radar_optimization_to_lidar_eigen_;
+
+  geometry_msgs::msg::Transform initial_radar_optimization_to_radar_msg_;
+  Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_;
 
   bool got_initial_transform_{false};
   bool broadcast_tf_{false};
@@ -254,7 +291,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   BackgroundModel lidar_background_model_;
   BackgroundModel radar_background_model_;
 
-  radar_msgs::msg::RadarTracks::SharedPtr latest_radar_msgs_;
+  radar_msgs::msg::RadarTracks::SharedPtr latest_radar_tracks_msgs_;
+  radar_msgs::msg::RadarScan::SharedPtr latest_radar_scan_msgs_;
+  sensor_msgs::msg::PointCloud2::SharedPtr latest_radar_cloud_msgs_;
 
   // Tracking
   bool tracking_active_{false};
@@ -266,6 +305,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   // Metrics
   std::vector<float> output_metrics_;
 
+  MsgType msg_type_;
+  TransformationType transformation_type_;
   static constexpr int MARKER_SIZE_PER_TRACK = 8;
 };
 

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp

@@ -0,0 +1,64 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_
+#define MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_
+
+#include <Eigen/Dense>
+
+namespace marker_radar_lidar_calibrator
+{
+
+struct SensorResidual
+{
+  SensorResidual(const Eigen::Vector4d & radar_point, const Eigen::Vector4d & lidar_point)
+  : radar_point_(radar_point), lidar_point_(lidar_point)
+  {
+  }
+
+  template <class T>
+  bool operator()(T const * const params, T * s_residuals) const
+  {
+    // parameters: x, y, z, pitch, yaw.
+    Eigen::Matrix<T, 4, 4> transformation_matrix = Eigen::Matrix<T, 4, 4>::Identity(4, 4);
+    Eigen::Matrix<T, 3, 3> rotation_matrix;
+
+    transformation_matrix(0, 3) = T(params[0]);
+    transformation_matrix(1, 3) = T(params[1]);
+    transformation_matrix(2, 3) = T(params[2]);
+
+    // This rotation matrix is rotate from radar to optimization frames (usually base_link).
+    // To avoid make sure that the Y axis does not approaches 90 degrees to avoid gimbal lock.
+    rotation_matrix = (Eigen::AngleAxis<T>(T(params[4]), Eigen::Vector3<T>::UnitZ()) *
+                       Eigen::AngleAxis<T>(T(params[3]), Eigen::Vector3<T>::UnitY()) *
+                       Eigen::AngleAxis<T>(T(0), Eigen::Vector3<T>::UnitX()))
+                        .matrix();
+
+    transformation_matrix.block(0, 0, 3, 3) = rotation_matrix;
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals(s_residuals);
+    Eigen::Matrix<T, 4, 1> residuals4d =
+      lidar_point_.cast<T>() - transformation_matrix * radar_point_.cast<T>();
+    residuals = residuals4d.block(0, 0, 3, 1);
+
+    return true;
+  }
+
+  Eigen::Vector4d radar_point_;
+  Eigen::Vector4d lidar_point_;
+};
+
+}  // namespace marker_radar_lidar_calibrator
+
+#endif  // MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp

@@ -0,0 +1,66 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_
+#define MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_
+
+#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>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <ceres/ceres.h>
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <tf2/convert.h>
+
+namespace marker_radar_lidar_calibrator
+{
+
+class TransformationEstimator
+{
+public:
+  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<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);
+  void estimateZeroRollTransformation();
+  Eigen::Isometry3d getTransformation();
+
+  double delta_cos_;
+  double delta_sin_;
+  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_;
+  Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_;
+  Eigen::Isometry3d radar_optimization_to_lidar_eigen_;
+};
+
+}  // namespace marker_radar_lidar_calibrator
+
+#endif  // MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_