feat(marker_radar_lidar_calibrator): 4d radar lidar calibration

calibrators/marker_radar_lidar_calibrator/CMakeLists.txt

@@ -3,32 +3,26 @@ 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/track.cpp
+  src/transformation_estimator.cpp
+  src/visualization.cpp
+  src/utils.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

@@ -12,23 +12,27 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_
-#define MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_
+#pragma once
 
 #include <Eigen/Dense>
-#include <marker_radar_lidar_calibrator/track.hpp>
 #include <marker_radar_lidar_calibrator/types.hpp>
+#include <marker_radar_lidar_calibrator/visualization.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/timer.hpp>
 #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>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tier4_calibration_msgs/msg/calibration_metrics.hpp>
+#include <tier4_calibration_msgs/msg/detail/calibration_metrics__struct.hpp>
+#include <tier4_calibration_msgs/srv/delete_lidar_radar_pair.hpp>
 #include <tier4_calibration_msgs/srv/extrinsic_calibrator.hpp>
+#include <tier4_calibration_msgs/srv/file_srv.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <pcl/pcl_base.h>
@@ -38,13 +42,11 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <algorithm>
+#include <cstddef>
 #include <cstdint>
-#include <iostream>
 #include <limits>
 #include <memory>
 #include <mutex>
-#include <random>
 #include <string>
 #include <tuple>
 #include <utility>
@@ -56,9 +58,12 @@ namespace marker_radar_lidar_calibrator
 class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 {
 public:
-  using PointType = pcl::PointXYZ;
   using index_t = std::uint32_t;
 
+  enum class MsgType { radar_tracks, radar_scan, radar_cloud };
+
+  enum class CornerReflectorEstimationMethod { average_points, longest_distance_point };
+
   explicit ExtrinsicReflectorBasedCalibrator(const rclcpp::NodeOptions & options);
 
 protected:
@@ -81,32 +86,55 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     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);
+    const std::shared_ptr<tier4_calibration_msgs::srv::DeleteLidarRadarPair::Request> request,
+    const std::shared_ptr<tier4_calibration_msgs::srv::DeleteLidarRadarPair::Response> response);
+
+  void loadDatabaseCallback(
+    const std::shared_ptr<tier4_calibration_msgs::srv::FileSrv::Request> request,
+    std::shared_ptr<tier4_calibration_msgs::srv::FileSrv::Response> response);
+
+  void saveDatabaseCallback(
+    const std::shared_ptr<tier4_calibration_msgs::srv::FileSrv::Request> request,
+    std::shared_ptr<tier4_calibration_msgs::srv::FileSrv::Response> response);
+
+  void logErrorAndRespond(
+    std::shared_ptr<tier4_calibration_msgs::srv::FileSrv::Response> & response,
+    const std::string & error_message);
 
   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();
@@ -115,42 +143,33 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     const std::vector<Eigen::Vector3d> & lidar_detections,
     const std::vector<Eigen::Vector3d> & radar_detections);
 
-  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);
+  bool trackMatches(const std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> & matches);
+
+  std::tuple<
+    pcl::PointCloud<common_types::PointType>::Ptr, pcl::PointCloud<common_types::PointType>::Ptr>
+  getPointsSet(std::vector<Track>::iterator & begin, std::vector<Track>::iterator & end);
+  std::tuple<double, double> get2DRotationDelta(
+    std::vector<Track>::iterator & begin, std::vector<Track>::iterator & end, bool is_crossval);
+
+  TransformationResult estimateTransformation(std::size_t track_index);
+  void evaluateTransformation(TransformationResult transformation_result, std::size_t track_index);
+  void crossValEvaluation(TransformationResult transformation_result);
+  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(
-    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);
 
   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 +204,9 @@ 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;
+    int min_frames_for_convergence;
+    std::size_t reflector_points_threshold;
   } parameters_;
 
   // ROS Interface
@@ -209,17 +230,22 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   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::Publisher<tier4_calibration_msgs::msg::CalibrationMetrics>::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_;
+  rclcpp::Service<tier4_calibration_msgs::srv::FileSrv>::SharedPtr load_database_service_server_;
+  rclcpp::Service<tier4_calibration_msgs::srv::FileSrv>::SharedPtr save_database_service_server_;
   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_;
+  rclcpp::Service<tier4_calibration_msgs::srv::DeleteLidarRadarPair>::SharedPtr
+    delete_track_service_server_;
 
   // Threading, sync, and result
   std::mutex mutex_;
@@ -233,8 +259,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,21 +284,25 @@ 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};
   int current_new_tracks_{false};
-  TrackFactory::Ptr factory_ptr_;
-  std::vector<Track> active_tracks_;
+  int num_of_frame_{0};
+  std::vector<std::vector<Track>> converging_tracks_;
   std::vector<Track> converged_tracks_;
 
   // Metrics
-  std::vector<float> output_metrics_;
+  OutputMetrics output_metrics_;
 
-  static constexpr int MARKER_SIZE_PER_TRACK = 8;
+  // Visualization
+  Visualization visualization_;
+
+  MsgType msg_type_;
+  TransformationType transformation_type_;
 };
 
 }  // namespace marker_radar_lidar_calibrator
-
-#endif  // MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_

calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp

@@ -0,0 +1,61 @@
+// 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.
+
+#pragma once
+
+#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