From 49bb788601fea8166c1b799cd917a7be2781b9f0 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 16 Jul 2024 15:05:28 +0900 Subject: [PATCH 1/3] feat: add ground plane Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 8 ++- .../marker_radar_lidar_calibrator/package.xml | 1 + .../src/marker_radar_lidar_calibrator.cpp | 66 ++++++++++++++++--- .../marker_radar_lidar_calibrator.launch.xml | 2 +- 4 files changed, 64 insertions(+), 13 deletions(-) diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index 211ee905..88f5d88b 100644 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -112,7 +112,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node void extractForegroundPoints( const pcl::PointCloud::Ptr & sensor_pointcloud, const BackgroundModel & background_model, bool use_ransac, - pcl::PointCloud::Ptr & foreground_points, Eigen::Vector4f & ground_model); + pcl::PointCloud::Ptr & foreground_points, Eigen::Vector4d & ground_model); std::vector::Ptr> extractClusters( const pcl::PointCloud::Ptr & foreground_pointcloud, @@ -120,7 +120,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node std::vector findReflectorsFromClusters( const std::vector::Ptr> & clusters, - const Eigen::Vector4f & ground_model); + const Eigen::Vector4d & ground_model); bool checkInitialTransforms(); @@ -216,6 +216,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr markers_pub_; rclcpp::Publisher::SharedPtr lidar_background_pub_; + rclcpp::Publisher::SharedPtr lidar_plane_pub_; rclcpp::Publisher::SharedPtr lidar_foreground_pub_; rclcpp::Publisher::SharedPtr lidar_colored_clusters_pub_; rclcpp::Publisher::SharedPtr lidar_detections_pub_; @@ -297,6 +298,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node MsgType msg_type_; TransformationType transformation_type_; static constexpr int MARKER_SIZE_PER_TRACK = 8; + + bool first_time_{true}; + Eigen::Vector4d ground_model_; }; } // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/package.xml b/calibrators/marker_radar_lidar_calibrator/package.xml index bd3bdb04..5def1c22 100644 --- a/calibrators/marker_radar_lidar_calibrator/package.xml +++ b/calibrators/marker_radar_lidar_calibrator/package.xml @@ -31,6 +31,7 @@ tf2_geometry_msgs tf2_ros tier4_calibration_msgs + tier4_ground_plane_utils visualization_msgs rclpy diff --git a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 233a866c..ade6c352 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -169,13 +170,13 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( radar_background_model_.leaf_size_ = this->declare_parameter("radar_background_model_leaf_size", 0.1); parameters_.max_calibration_range = - this->declare_parameter("max_calibration_range", 50.0); + this->declare_parameter("max_calibration_range", 20.0); parameters_.background_model_timeout = - this->declare_parameter("background_model_timeout", 5.0); + this->declare_parameter("background_model_timeout", 1.0); parameters_.min_foreground_distance = this->declare_parameter("min_foreground_distance", 0.4); parameters_.background_extraction_timeout = - this->declare_parameter("background_extraction_timeout", 15.0); + this->declare_parameter("background_extraction_timeout", 3.0); parameters_.ransac_threshold = this->declare_parameter("ransac_threshold", 0.2); parameters_.ransac_max_iterations = this->declare_parameter("ransac_max_iterations", 100); parameters_.lidar_cluster_max_tolerance = @@ -251,6 +252,8 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( this->create_publisher("lidar_colored_clusters", 10); lidar_detections_pub_ = this->create_publisher("lidar_detection_markers", 10); + lidar_plane_pub_ = + this->create_publisher("lidar_plane_pointcloud", 10); radar_background_pub_ = this->create_publisher("radar_background_pointcloud", 10); @@ -603,15 +606,51 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl return detections; } + pcl::PointCloud::Ptr ground_plane_inliers_ptr; + bool status; + + if (first_time_) { + Eigen::Isometry3d initial_base_to_lidar_transform; + + try { + rclcpp::Time t = rclcpp::Time(0); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); + + initial_base_to_lidar_transform = tf2::transformToEigen( + tf_buffer_->lookupTransform("base_link", "hesai_top", t, timeout).transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "could not get initial tf. %s", ex.what()); + } + + tier4_ground_plane_utils::GroundPlaneExtractorParameters parameters; + parameters.verbose_ = true; + parameters.use_crop_box_filter_ = false; + parameters.use_pca_rough_normal_ = false; + parameters.max_inlier_distance_ = 0.01; + parameters.min_plane_points_ = 300; + parameters.min_plane_points_percentage_ = 1.0; + parameters.max_cos_distance_ = 0.5; + parameters.max_iterations_ = 1000; + parameters.remove_outliers_ = false; + parameters.initial_base_to_lidar_transform_ = initial_base_to_lidar_transform; + std::vector outlier_models; + + std::tie(status, ground_model_, ground_plane_inliers_ptr) = + tier4_ground_plane_utils::extractGroundPlane( + lidar_pointcloud_ptr, parameters, outlier_models); + + first_time_ = false; + std::cout << "####### ground_model_: ######" << ground_model_.matrix() << std::endl; + } + pcl::PointCloud::Ptr foreground_pointcloud_ptr; - Eigen::Vector4f ground_model; extractForegroundPoints( - lidar_pointcloud_ptr, lidar_background_model_, true, foreground_pointcloud_ptr, ground_model); + lidar_pointcloud_ptr, lidar_background_model_, false, foreground_pointcloud_ptr, ground_model_); auto clusters = extractClusters( foreground_pointcloud_ptr, parameters_.lidar_cluster_max_tolerance, parameters_.lidar_cluster_min_points, parameters_.lidar_cluster_max_points); - detections = findReflectorsFromClusters(clusters, ground_model); + detections = findReflectorsFromClusters(clusters, ground_model_); // Visualization pcl::PointCloud::Ptr colored_clusters_pointcloud_ptr( @@ -663,6 +702,13 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl colored_clusters_msg.header = lidar_header_; lidar_colored_clusters_pub_->publish(colored_clusters_msg); + if (ground_plane_inliers_ptr) { + sensor_msgs::msg::PointCloud2 plane_msg; + pcl::toROSMsg(*ground_plane_inliers_ptr, plane_msg); + plane_msg.header = lidar_header_; + lidar_plane_pub_->publish(plane_msg); + } + return detections; } @@ -744,7 +790,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractRadarRefl } pcl::PointCloud::Ptr foreground_pointcloud_ptr; - Eigen::Vector4f ground_model; + Eigen::Vector4d ground_model; extractForegroundPoints( radar_pointcloud_ptr, radar_background_model_, false, foreground_pointcloud_ptr, ground_model); auto clusters = extractClusters( @@ -877,7 +923,7 @@ void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel( void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( const pcl::PointCloud::Ptr & sensor_pointcloud_ptr, const BackgroundModel & background_model, bool use_ransac, - pcl::PointCloud::Ptr & foreground_pointcloud_ptr, Eigen::Vector4f & ground_model) + pcl::PointCloud::Ptr & foreground_pointcloud_ptr, Eigen::Vector4d & ground_model) { RCLCPP_INFO(this->get_logger(), "Extracting foreground"); RCLCPP_INFO(this->get_logger(), "\t initial points: %lu", sensor_pointcloud_ptr->size()); @@ -971,7 +1017,7 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( this->get_logger(), "\t ransac filtered points: %lu", ransac_filtered_pointcloud_ptr->size()); foreground_pointcloud_ptr = ransac_filtered_pointcloud_ptr; - ground_model = Eigen::Vector4f( + ground_model = Eigen::Vector4d( coefficients_ptr->values[0], coefficients_ptr->values[1], coefficients_ptr->values[2], coefficients_ptr->values[3]); } @@ -1020,7 +1066,7 @@ ExtrinsicReflectorBasedCalibrator::extractClusters( std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFromClusters( const std::vector::Ptr> & clusters, - const Eigen::Vector4f & ground_model) + const Eigen::Vector4d & ground_model) { std::vector reflector_centers; RCLCPP_INFO(this->get_logger(), "Extracting lidar reflectors from clusters"); diff --git a/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml index c4ec2de4..73e6d56f 100644 --- a/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml +++ b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml @@ -34,7 +34,7 @@ - + From dea46c1d07f42340b6bd44e05459c1fffada05bb Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 25 Sep 2024 18:24:43 +0900 Subject: [PATCH 2/3] chore: remove kalman filter dep Signed-off-by: vividf --- calibrators/marker_radar_lidar_calibrator/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/calibrators/marker_radar_lidar_calibrator/package.xml b/calibrators/marker_radar_lidar_calibrator/package.xml index 5def1c22..acfdffd9 100644 --- a/calibrators/marker_radar_lidar_calibrator/package.xml +++ b/calibrators/marker_radar_lidar_calibrator/package.xml @@ -17,7 +17,6 @@ autoware_universe_utils eigen geometry_msgs - kalman_filter libceres-dev pcl_conversions pcl_ros From b3c0fbf113158baf3df9a589f15449c0a644e578 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 3 Oct 2024 11:18:04 +0900 Subject: [PATCH 3/3] T4 garage xx1 experiment (#199) * chore: tune parameter setting for smooth calibration Signed-off-by: vividf * chore: fix default rviz for text markers Signed-off-by: vividf * chore: fix default rviz for text markers Signed-off-by: vividf * chore: remove unused header Signed-off-by: vividf * chore: change function name to evaluateCombinations Signed-off-by: vividf * chore: change function name to get2DRotationDelta Signed-off-by: vividf * chore: change variable name Signed-off-by: vividf * chore: fix waiting to extract the background model Signed-off-by: vividf * chore: fix grammar error Signed-off-by: vividf * chore: fix function name and clean code Signed-off-by: vividf * chore: remove unnessary logging Signed-off-by: vividf * chore: fix int to size_t Signed-off-by: vividf * chore: fix estimateZeroRollTransformation logging Signed-off-by: vividf * chore: add std::size_t Signed-off-by: vividf * chore: NULL to nullptr Signed-off-by: vividf * chore: declare point type only once Signed-off-by: vividf * chore: remove class member and declare new structure Signed-off-by: vividf * chore: fix description of radar_optimization_frame Signed-off-by: vividf * chore: remove if condition Signed-off-by: vividf * chore: fix function name setDelta Signed-off-by: vividf --------- Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 66 +++-- .../transformation_estimator.hpp | 12 +- .../marker_radar_lidar_calibrator/types.hpp | 12 +- .../rviz/default.rviz | 31 +- .../src/marker_radar_lidar_calibrator.cpp | 273 ++++++++++-------- .../src/transformation_estimator.cpp | 15 +- 6 files changed, 227 insertions(+), 182 deletions(-) diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp index 88f5d88b..0959c95a 100644 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp @@ -41,7 +41,6 @@ #include #include -#include #include #include #include @@ -54,10 +53,16 @@ namespace marker_radar_lidar_calibrator { +struct TransformationResult +{ + pcl::PointCloud::Ptr lidar_points_ocs; + pcl::PointCloud::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 - pcl::PointCloud::Ptr extractRadarPointcloud(const std::shared_ptr & msg); + pcl::PointCloud::Ptr extractRadarPointcloud( + const std::shared_ptr & msg); std::vector extractLidarReflectors( const sensor_msgs::msg::PointCloud2::SharedPtr msg); std::vector extractRadarReflectors( - pcl::PointCloud::Ptr radar_pointcloud_ptr); + pcl::PointCloud::Ptr radar_pointcloud_ptr); void extractBackgroundModel( - const pcl::PointCloud::Ptr & sensor_pointcloud, + const pcl::PointCloud::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::Ptr & sensor_pointcloud, + const pcl::PointCloud::Ptr & sensor_pointcloud, const BackgroundModel & background_model, bool use_ransac, - pcl::PointCloud::Ptr & foreground_points, Eigen::Vector4d & ground_model); + pcl::PointCloud::Ptr & foreground_points, + Eigen::Vector4d & ground_model); - std::vector::Ptr> extractClusters( - const pcl::PointCloud::Ptr & foreground_pointcloud, + std::vector::Ptr> extractClusters( + const pcl::PointCloud::Ptr & foreground_pointcloud, const double cluster_max_tolerance, const int cluster_min_points, const int cluster_max_points); std::vector findReflectorsFromClusters( - const std::vector::Ptr> & clusters, + const std::vector::Ptr> & clusters, const Eigen::Vector4d & ground_model); bool checkInitialTransforms(); @@ -132,20 +139,26 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node const std::vector> & matches, builtin_interfaces::msg::Time & time); - std::tuple::Ptr, pcl::PointCloud::Ptr> getPointsSet(); - std::tuple getDelta(std::vector converged_tracks, bool is_crossval); + std::tuple< + pcl::PointCloud::Ptr, pcl::PointCloud::Ptr> + getPointsSet(); + std::tuple get2DRotationDelta( + std::vector converged_tracks, bool is_crossval); std::pair 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 & curr, int first_num, - std::vector> & combinations); + std::size_t n, std::size_t k, std::vector & curr, std::size_t first_num, + std::vector> & combinations); void selectCombinations( - int tracks_size, int num_of_samples, std::vector> & combinations); - void doEvaluation(std::vector> & combinations, int num_of_samples); + std::size_t tracks_size, std::size_t num_of_samples, + std::vector> & combinations); + void evaluateCombinations( + std::vector> & 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 active_tracks_; std::vector converged_tracks_; - // Converged points - pcl::PointCloud::Ptr lidar_points_ocs_; - pcl::PointCloud::Ptr radar_points_rcs_; - // Metrics std::vector output_metrics_; diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp index c264a5bb..c47bdf45 100644 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -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::Ptr lidar_points_ocs, - pcl::PointCloud::Ptr radar_points_rcs); - void setDelta(double delta_cos, double delta_sin); + pcl::PointCloud::Ptr lidar_points_ocs, + pcl::PointCloud::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::Ptr lidar_points_ocs_; - pcl::PointCloud::Ptr radar_points_rcs_; + pcl::PointCloud::Ptr lidar_points_ocs_; + pcl::PointCloud::Ptr radar_points_rcs_; Eigen::Isometry3d calibrated_radar_to_lidar_transformation_; Eigen::Isometry3d initial_radar_to_lidar_eigen_; diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp index 8caa0e93..6f5880b1 100644 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp +++ b/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; // cSpell:ignore FLANN + using TreeType = pcl::KdTreeFLANN; // cSpell:ignore FLANN using index_t = std::uint32_t; BackgroundModel() @@ -44,7 +48,7 @@ struct BackgroundModel max_point_( -std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max(), 1.f), - pointcloud_(new pcl::PointCloud) + pointcloud_(new pcl::PointCloud) { } @@ -53,7 +57,7 @@ struct BackgroundModel Eigen::Vector4f min_point_; Eigen::Vector4f max_point_; std::unordered_set set_; - pcl::PointCloud::Ptr pointcloud_; + pcl::PointCloud::Ptr pointcloud_; TreeType tree_; }; diff --git a/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz b/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz index 90edc3bf..60bfcc66 100644 --- a/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz +++ b/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,7 +256,7 @@ 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: @@ -266,6 +264,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /text_markers @@ -344,9 +343,9 @@ 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: Value: FPS (rviz_default_plugins) Yaw: 0.0031585693359375 @@ -354,10 +353,10 @@ Visualization Manager: 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 diff --git a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index ade6c352..c04e35d4 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp @@ -33,10 +33,12 @@ #include #include +#include #include #include #include #include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -184,7 +186,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( parameters_.lidar_cluster_min_points = this->declare_parameter("lidar_cluster_min_points", 3); parameters_.lidar_cluster_max_points = - this->declare_parameter("lidar_cluster_max_points", 2000); + this->declare_parameter("lidar_cluster_max_points", 500); parameters_.radar_cluster_max_tolerance = this->declare_parameter("radar_cluster_max_tolerance", 0.5); parameters_.radar_cluster_min_points = @@ -194,8 +196,8 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( parameters_.reflector_radius = this->declare_parameter("reflector_radius", 0.1); parameters_.reflector_max_height = this->declare_parameter("reflector_max_height", 1.2); parameters_.max_matching_distance = this->declare_parameter("max_matching_distance", 1.0); - parameters_.max_number_of_combination_samples = - this->declare_parameter("max_number_of_combination_samples", 10000); + parameters_.max_number_of_combination_samples = static_cast( + this->declare_parameter("max_number_of_combination_samples", 10000)); auto msg_type = this->declare_parameter("msg_type"); auto transformation_type = this->declare_parameter("transformation_type"); @@ -407,7 +409,7 @@ void ExtrinsicReflectorBasedCalibrator::backgroundModelRequestCallback( } RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "Waiting for the extracting background model"); + this->get_logger(), *this->get_clock(), 5000, "Waiting to extract the background model"); } RCLCPP_INFO(this->get_logger(), "Background model estimated"); @@ -479,29 +481,29 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( std::vector radar_detections; if (msg_type_ == MsgType::radar_tracks) { if (!latest_radar_tracks_msgs_ || latest_radar_tracks_msgs_->tracks.size() == 0) { - RCLCPP_INFO(this->get_logger(), "There were no tracks"); + RCLCPP_INFO(this->get_logger(), "There were no radar tracks"); return; } - pcl::PointCloud::Ptr radar_pointcloud_ptr = + pcl::PointCloud::Ptr radar_pointcloud_ptr = extractRadarPointcloud(latest_radar_tracks_msgs_); radar_detections = extractRadarReflectors(radar_pointcloud_ptr); latest_radar_tracks_msgs_->tracks.clear(); } else if (msg_type_ == MsgType::radar_scan) { if (!latest_radar_scan_msgs_ || latest_radar_scan_msgs_->returns.size() == 0) { if (latest_radar_scan_msgs_->returns.size() == 0) - RCLCPP_INFO(this->get_logger(), "There were no scan"); + RCLCPP_INFO(this->get_logger(), "There were no radar scans"); return; } - pcl::PointCloud::Ptr radar_pointcloud_ptr = + pcl::PointCloud::Ptr radar_pointcloud_ptr = extractRadarPointcloud(latest_radar_scan_msgs_); radar_detections = extractRadarReflectors(radar_pointcloud_ptr); latest_radar_scan_msgs_->returns.clear(); } else { if (!latest_radar_cloud_msgs_) { - RCLCPP_INFO(this->get_logger(), "There were no radar pointcloud"); + RCLCPP_INFO(this->get_logger(), "There were no radar pointclouds"); return; } - pcl::PointCloud::Ptr radar_pointcloud_ptr = + pcl::PointCloud::Ptr radar_pointcloud_ptr = extractRadarPointcloud(latest_radar_cloud_msgs_); radar_detections = extractRadarReflectors(radar_pointcloud_ptr); } @@ -526,25 +528,17 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( void ExtrinsicReflectorBasedCalibrator::radarTracksCallback( const radar_msgs::msg::RadarTracks::SharedPtr msg) { - if (!latest_radar_tracks_msgs_) { - latest_radar_tracks_msgs_ = msg; - } else { - latest_radar_tracks_msgs_->header = msg->header; - latest_radar_tracks_msgs_->tracks.insert( - latest_radar_tracks_msgs_->tracks.end(), msg->tracks.begin(), msg->tracks.end()); - } + latest_radar_tracks_msgs_->header = msg->header; + latest_radar_tracks_msgs_->tracks.insert( + latest_radar_tracks_msgs_->tracks.end(), msg->tracks.begin(), msg->tracks.end()); } void ExtrinsicReflectorBasedCalibrator::radarScanCallback( const radar_msgs::msg::RadarScan::SharedPtr msg) { - if (!latest_radar_scan_msgs_) { - latest_radar_scan_msgs_ = msg; - } else { - latest_radar_scan_msgs_->header = msg->header; - latest_radar_scan_msgs_->returns.insert( - latest_radar_scan_msgs_->returns.end(), msg->returns.begin(), msg->returns.end()); - } + latest_radar_scan_msgs_->header = msg->header; + latest_radar_scan_msgs_->returns.insert( + latest_radar_scan_msgs_->returns.end(), msg->returns.begin(), msg->returns.end()); } void ExtrinsicReflectorBasedCalibrator::radarCloudCallback( @@ -568,12 +562,14 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl valid_background_model = lidar_background_model_.valid_; } - pcl::PointCloud::Ptr lidar_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr lidar_pointcloud_ptr( + new pcl::PointCloud); pcl::fromROSMsg(*msg, *lidar_pointcloud_ptr); if (parameters_.use_lidar_initial_crop_box_filter) { - pcl::CropBox box_filter; - pcl::PointCloud::Ptr tmp_lidar_pointcloud_ptr(new pcl::PointCloud); + pcl::CropBox box_filter; + pcl::PointCloud::Ptr tmp_lidar_pointcloud_ptr( + new pcl::PointCloud); RCLCPP_INFO(this->get_logger(), "pre lidar_pointcloud_ptr=%lu", lidar_pointcloud_ptr->size()); RCLCPP_WARN( this->get_logger(), "crop box parameters=%f | %f | %f", @@ -606,7 +602,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl return detections; } - pcl::PointCloud::Ptr ground_plane_inliers_ptr; + pcl::PointCloud::Ptr ground_plane_inliers_ptr; bool status; if (first_time_) { @@ -643,7 +639,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl std::cout << "####### ground_model_: ######" << ground_model_.matrix() << std::endl; } - pcl::PointCloud::Ptr foreground_pointcloud_ptr; + pcl::PointCloud::Ptr foreground_pointcloud_ptr; extractForegroundPoints( lidar_pointcloud_ptr, lidar_background_model_, false, foreground_pointcloud_ptr, ground_model_); @@ -713,7 +709,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl } template -pcl::PointCloud::Ptr +pcl::PointCloud::Ptr ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr & msg) { static_assert( @@ -724,7 +720,7 @@ ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr< radar_frame_ = msg->header.frame_id; radar_header_ = msg->header; - auto radar_pointcloud_ptr = std::make_shared>(); + auto radar_pointcloud_ptr = std::make_shared>(); if constexpr (std::is_same::value) { radar_pointcloud_ptr->reserve(msg->tracks.size()); @@ -752,7 +748,7 @@ ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr< } std::vector ExtrinsicReflectorBasedCalibrator::extractRadarReflectors( - pcl::PointCloud::Ptr radar_pointcloud_ptr) + pcl::PointCloud::Ptr radar_pointcloud_ptr) { bool extract_background_model; bool valid_background_model; @@ -765,8 +761,9 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractRadarRefl } if (parameters_.use_radar_initial_crop_box_filter) { - pcl::CropBox box_filter; - pcl::PointCloud::Ptr tmp_radar_pointcloud_ptr(new pcl::PointCloud); + pcl::CropBox box_filter; + pcl::PointCloud::Ptr tmp_radar_pointcloud_ptr( + new pcl::PointCloud); box_filter.setMin(Eigen::Vector4f( parameters_.radar_initial_crop_box_min_x, parameters_.radar_initial_crop_box_min_y, parameters_.radar_initial_crop_box_min_z, 1.0)); @@ -789,8 +786,9 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractRadarRefl return detections; } - pcl::PointCloud::Ptr foreground_pointcloud_ptr; + pcl::PointCloud::Ptr foreground_pointcloud_ptr; Eigen::Vector4d ground_model; + extractForegroundPoints( radar_pointcloud_ptr, radar_background_model_, false, foreground_pointcloud_ptr, ground_model); auto clusters = extractClusters( @@ -830,7 +828,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractRadarRefl } void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel( - const pcl::PointCloud::Ptr & sensor_pointcloud_ptr, + const pcl::PointCloud::Ptr & sensor_pointcloud_ptr, const std_msgs::msg::Header & current_header, std_msgs::msg::Header & last_updated_header, std_msgs::msg::Header & first_header, BackgroundModel & background_model) { @@ -876,7 +874,7 @@ void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel( const auto & it = background_model.set_.emplace(index); if (it.second) { - PointType p_center; + common_types::PointType p_center; p_center.x = background_model.min_point_.x() + background_model.leaf_size_ * (x_index + 0.5f); p_center.y = background_model.min_point_.y() + background_model.leaf_size_ * (y_index + 0.5f); p_center.z = background_model.min_point_.z() + background_model.leaf_size_ * (z_index + 0.5f); @@ -921,16 +919,18 @@ void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel( } void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( - const pcl::PointCloud::Ptr & sensor_pointcloud_ptr, + const pcl::PointCloud::Ptr & sensor_pointcloud_ptr, const BackgroundModel & background_model, bool use_ransac, - pcl::PointCloud::Ptr & foreground_pointcloud_ptr, Eigen::Vector4d & ground_model) + pcl::PointCloud::Ptr & foreground_pointcloud_ptr, + Eigen::Vector4d & ground_model) { RCLCPP_INFO(this->get_logger(), "Extracting foreground"); RCLCPP_INFO(this->get_logger(), "\t initial points: %lu", sensor_pointcloud_ptr->size()); // Crop box - pcl::PointCloud::Ptr cropped_pointcloud_ptr(new pcl::PointCloud); - pcl::CropBox crop_filter; + pcl::PointCloud::Ptr cropped_pointcloud_ptr( + new pcl::PointCloud); + pcl::CropBox crop_filter; crop_filter.setMin(background_model.min_point_); crop_filter.setMax(background_model.max_point_); crop_filter.setInputCloud(sensor_pointcloud_ptr); @@ -938,7 +938,8 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( RCLCPP_INFO(this->get_logger(), "\t cropped points: %lu", cropped_pointcloud_ptr->size()); // Fast hash - pcl::PointCloud::Ptr voxel_filtered_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr voxel_filtered_pointcloud_ptr( + new pcl::PointCloud); voxel_filtered_pointcloud_ptr->reserve(cropped_pointcloud_ptr->size()); index_t x_cells = (background_model.max_point_.x() - background_model.min_point_.x()) / @@ -963,7 +964,8 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( this->get_logger(), "\t voxel filtered points: %lu", voxel_filtered_pointcloud_ptr->size()); // K-search - pcl::PointCloud::Ptr tree_filtered_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr tree_filtered_pointcloud_ptr( + new pcl::PointCloud); tree_filtered_pointcloud_ptr->reserve(voxel_filtered_pointcloud_ptr->size()); float min_foreground_square_distance = parameters_.min_foreground_distance * parameters_.min_foreground_distance; @@ -990,10 +992,11 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( // Plane ransac (since the orientation changes slightly between data, this one does not use the // background model) pcl::ModelCoefficients::Ptr coefficients_ptr(new pcl::ModelCoefficients); - pcl::PointCloud::Ptr ransac_filtered_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr ransac_filtered_pointcloud_ptr( + new pcl::PointCloud); pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices); - pcl::SACSegmentation seg; - pcl::ExtractIndices extract; + pcl::SACSegmentation seg; + pcl::ExtractIndices extract; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); // cSpell:ignore SACMODEL seg.setMethodType(pcl::SAC_RANSAC); @@ -1022,16 +1025,17 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints( coefficients_ptr->values[3]); } -std::vector::Ptr> +std::vector::Ptr> ExtrinsicReflectorBasedCalibrator::extractClusters( - const pcl::PointCloud::Ptr & foreground_pointcloud_ptr, + const pcl::PointCloud::Ptr & foreground_pointcloud_ptr, const double cluster_max_tolerance, const int cluster_min_points, const int cluster_max_points) { - pcl::search::KdTree::Ptr tree_ptr(new pcl::search::KdTree); + pcl::search::KdTree::Ptr tree_ptr( + new pcl::search::KdTree); tree_ptr->setInputCloud(foreground_pointcloud_ptr); std::vector cluster_indices; - pcl::EuclideanClusterExtraction cluster_extractor; + pcl::EuclideanClusterExtraction cluster_extractor; cluster_extractor.setClusterTolerance(cluster_max_tolerance); cluster_extractor.setMinClusterSize(cluster_min_points); cluster_extractor.setMaxClusterSize(cluster_max_points); @@ -1042,10 +1046,11 @@ ExtrinsicReflectorBasedCalibrator::extractClusters( RCLCPP_INFO( this->get_logger(), "Cluster extraction input size: %lu", foreground_pointcloud_ptr->size()); - std::vector::Ptr> cluster_vector; + std::vector::Ptr> cluster_vector; for (const auto & cluster : cluster_indices) { - pcl::PointCloud::Ptr cluster_pointcloud_ptr(new pcl::PointCloud); + pcl::PointCloud::Ptr cluster_pointcloud_ptr( + new pcl::PointCloud); cluster_pointcloud_ptr->reserve(cluster.indices.size()); for (const auto & idx : cluster.indices) { @@ -1065,7 +1070,7 @@ ExtrinsicReflectorBasedCalibrator::extractClusters( } std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFromClusters( - const std::vector::Ptr> & clusters, + const std::vector::Ptr> & clusters, const Eigen::Vector4d & ground_model) { std::vector reflector_centers; @@ -1073,7 +1078,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFr for (const auto & cluster_pointcloud_ptr : clusters) { float max_h = -std::numeric_limits::max(); - PointType highest_point; + common_types::PointType highest_point; for (const auto & p : cluster_pointcloud_ptr->points) { float height = @@ -1088,7 +1093,8 @@ std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFr continue; } - pcl::search::KdTree::Ptr tree_ptr(new pcl::search::KdTree); + pcl::search::KdTree::Ptr tree_ptr( + new pcl::search::KdTree); tree_ptr->setInputCloud(cluster_pointcloud_ptr); std::vector indexes; @@ -1141,12 +1147,12 @@ bool ExtrinsicReflectorBasedCalibrator::checkInitialTransforms() radar_optimization_to_lidar_eigen_ = tf2::transformToEigen(radar_optimization_to_lidar_msg_); - init_radar_optimization_to_radar_msg_ = + initial_radar_optimization_to_radar_msg_ = tf_buffer_->lookupTransform(parameters_.radar_optimization_frame, radar_frame_, t, timeout) .transform; initial_radar_optimization_to_radar_eigen_ = - tf2::transformToEigen(init_radar_optimization_to_radar_msg_); + tf2::transformToEigen(initial_radar_optimization_to_radar_msg_); got_initial_transform_ = true; } catch (tf2::TransformException & ex) { @@ -1175,9 +1181,14 @@ ExtrinsicReflectorBasedCalibrator::matchDetections( std::transform( lidar_detections.cbegin(), lidar_detections.cend(), std::back_inserter(lidar_detections_transformed), - [&radar_to_lidar_transform](const auto & lidar_detection) { + [&radar_to_lidar_transform, + &transformation_type = this->transformation_type_](const auto & lidar_detection) { auto transformed_point = radar_to_lidar_transform * lidar_detection; - transformed_point.z() = 0.f; + if ( + transformation_type == TransformationType::yaw_only_rotation_2d || + transformation_type == TransformationType::svd_2d) { + transformed_point.z() = 0.f; + } return transformed_point; }); @@ -1331,7 +1342,7 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches( return is_track_converged; } -std::tuple ExtrinsicReflectorBasedCalibrator::getDelta( +std::tuple ExtrinsicReflectorBasedCalibrator::get2DRotationDelta( std::vector converged_tracks, bool is_crossval) { double delta_cos_sum = 0.0; @@ -1379,18 +1390,21 @@ std::tuple ExtrinsicReflectorBasedCalibrator::getDelta( } std::tuple< - pcl::PointCloud::Ptr, - pcl::PointCloud::Ptr> + pcl::PointCloud::Ptr, pcl::PointCloud::Ptr> ExtrinsicReflectorBasedCalibrator::getPointsSet() { // Note: ocs=radar optimization coordinate system rcs=radar coordinate system - pcl::PointCloud::Ptr lidar_points_ocs(new pcl::PointCloud); - pcl::PointCloud::Ptr radar_points_rcs(new pcl::PointCloud); + pcl::PointCloud::Ptr lidar_points_ocs( + new pcl::PointCloud); + pcl::PointCloud::Ptr radar_points_rcs( + new pcl::PointCloud); lidar_points_ocs->reserve(converged_tracks_.size()); radar_points_rcs->reserve(converged_tracks_.size()); - auto eigen_to_pcl_2d = [](const auto & p) { return PointType(p.x(), p.y(), 0.0); }; - auto eigen_to_pcl_3d = [](const auto & p) { return PointType(p.x(), p.y(), p.z()); }; + auto eigen_to_pcl_2d = [](const auto & p) { return common_types::PointType(p.x(), p.y(), 0.0); }; + auto eigen_to_pcl_3d = [](const auto & p) { + return common_types::PointType(p.x(), p.y(), p.z()); + }; for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { auto track = converged_tracks_[track_index]; @@ -1431,9 +1445,13 @@ std::pair ExtrinsicReflectorBasedCalibrator::computeCalibrationE auto lidar_estimation = track.getLidarEstimation(); auto radar_estimation = track.getRadarEstimation(); auto lidar_estimation_transformed = radar_to_lidar_isometry * lidar_estimation; - lidar_estimation_transformed.z() = 0.0; - radar_estimation.z() = 0.0; + if ( + transformation_type_ == TransformationType::yaw_only_rotation_2d || + transformation_type_ == TransformationType::svd_2d) { + lidar_estimation_transformed.z() = 0.0; + radar_estimation.z() = 0.0; + } distance_error += (lidar_estimation_transformed - radar_estimation).norm(); yaw_error += getYawError(lidar_estimation_transformed, radar_estimation); } @@ -1444,58 +1462,66 @@ std::pair ExtrinsicReflectorBasedCalibrator::computeCalibrationE return std::make_pair(distance_error, yaw_error); } -void ExtrinsicReflectorBasedCalibrator::estimateTransformation() +TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation() { + TransformationResult transformation_result; TransformationEstimator estimator( initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, radar_optimization_to_lidar_eigen_); - Eigen::Isometry3d calibrated_radar_to_lidar_transformation; + if (transformation_type_ == TransformationType::yaw_only_rotation_2d) { - auto [delta_cos, delta_sin] = getDelta(converged_tracks_, false); - estimator.setDelta(delta_cos, delta_sin); + auto [delta_cos, delta_sin] = get2DRotationDelta(converged_tracks_, false); + estimator.set2DRotationDelta(delta_cos, delta_sin); estimator.estimateYawOnlyTransformation(); - calibrated_radar_to_lidar_transformation = estimator.getTransformation(); + transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation(); RCLCPP_INFO_STREAM( this->get_logger(), "Initial radar->lidar transform:\n" << initial_radar_to_lidar_eigen_.matrix()); RCLCPP_INFO_STREAM( - this->get_logger(), "Pure rotation calibration radar->lidar transform:\n" - << calibrated_radar_to_lidar_transformation.matrix()); + this->get_logger(), + "Pure rotation calibration radar->lidar transform:\n" + << transformation_result.calibrated_radar_to_lidar_transformation.matrix()); } else if (transformation_type_ == TransformationType::svd_2d) { - std::tie(lidar_points_ocs_, radar_points_rcs_) = getPointsSet(); - estimator.setPoints(lidar_points_ocs_, radar_points_rcs_); + std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) = + getPointsSet(); + estimator.setPoints( + transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs); estimator.estimateSVDTransformation(transformation_type_); - calibrated_radar_to_lidar_transformation = estimator.getTransformation(); + transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation(); RCLCPP_INFO_STREAM( this->get_logger(), "Initial radar->lidar transform:\n" << initial_radar_to_lidar_eigen_.matrix()); RCLCPP_INFO_STREAM( - this->get_logger(), "2D calibration radar->lidar transform:\n" - << calibrated_radar_to_lidar_transformation.matrix()); + this->get_logger(), + "2D calibration radar->lidar transform:\n" + << transformation_result.calibrated_radar_to_lidar_transformation.matrix()); } else if ( transformation_type_ == TransformationType::svd_3d || transformation_type_ == TransformationType::zero_roll_3d) { - std::tie(lidar_points_ocs_, radar_points_rcs_) = getPointsSet(); - estimator.setPoints(lidar_points_ocs_, radar_points_rcs_); + std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) = + getPointsSet(); + estimator.setPoints( + transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs); if (transformation_type_ == TransformationType::zero_roll_3d) estimator.estimateZeroRollTransformation(); else if (transformation_type_ == TransformationType::svd_3d) estimator.estimateSVDTransformation(transformation_type_); - calibrated_radar_to_lidar_transformation = estimator.getTransformation(); + transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation(); RCLCPP_INFO_STREAM( this->get_logger(), "Initial radar->lidar transform:\n" << initial_radar_to_lidar_eigen_.matrix()); RCLCPP_INFO_STREAM( - this->get_logger(), "3D calibration radar->lidar transform:\n" - << calibrated_radar_to_lidar_transformation.matrix()); + this->get_logger(), + "3D calibration radar->lidar transform:\n" + << transformation_result.calibrated_radar_to_lidar_transformation.matrix()); } - calculateCalibrationError(calibrated_radar_to_lidar_transformation); + return transformation_result; } -void ExtrinsicReflectorBasedCalibrator::calculateCalibrationError( +void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( Eigen::Isometry3d calibrated_radar_to_lidar_transformation) { // Estimate the pre & post calibration error @@ -1530,8 +1556,6 @@ void ExtrinsicReflectorBasedCalibrator::calculateCalibrationError( if ( calibrated_translation_difference < parameters_.max_initial_calibration_translation_error && calibrated_rotation_difference < parameters_.max_initial_calibration_rotation_error) { - RCLCPP_INFO( - this->get_logger(), "The calibration pose was chosen as the output calibration pose"); calibrated_radar_to_lidar_eigen_ = calibrated_radar_to_lidar_transformation; calibration_valid_ = true; calibration_distance_score_ = calibrated_distance_error; @@ -1548,20 +1572,20 @@ void ExtrinsicReflectorBasedCalibrator::calculateCalibrationError( } void ExtrinsicReflectorBasedCalibrator::findCombinations( - int n, int k, std::vector & curr, int first_num, - std::vector> & combinations) + std::size_t n, std::size_t k, std::vector & curr, std::size_t first_num, + std::vector> & combinations) { - int curr_size = static_cast(curr.size()); + auto curr_size = curr.size(); if (curr_size == k) { combinations.push_back(curr); return; } - int need = k - curr_size; - int remain = n - first_num + 1; - int available = remain - need; + auto need = k - curr_size; + auto remain = n - first_num + 1; + auto available = remain - need; - for (int num = first_num; num <= first_num + available; num++) { + for (auto num = first_num; num <= first_num + available; num++) { curr.push_back(num); findCombinations(n, k, curr, num + 1, combinations); curr.pop_back(); @@ -1571,37 +1595,40 @@ void ExtrinsicReflectorBasedCalibrator::findCombinations( } void ExtrinsicReflectorBasedCalibrator::selectCombinations( - int tracks_size, int num_of_samples, std::vector> & combinations) + std::size_t tracks_size, std::size_t num_of_samples, + std::vector> & combinations) { RCLCPP_INFO( this->get_logger(), - "Current number of combinations is: %d, converged_tracks_size: %d, num_of_samples: %d", - static_cast(combinations.size()), tracks_size, num_of_samples); + "Current number of combinations is: %zu, converged_tracks_size: %zu, num_of_samples: %zu", + combinations.size(), tracks_size, num_of_samples); // random select the combinations if the number of combinations is too large - if ( - combinations.size() > static_cast(parameters_.max_number_of_combination_samples)) { + if (combinations.size() > parameters_.max_number_of_combination_samples) { std::random_device rd; std::mt19937 mt(rd()); std::shuffle(combinations.begin(), combinations.end(), mt); combinations.resize(parameters_.max_number_of_combination_samples); RCLCPP_WARN( this->get_logger(), - "The number of combinations is set to: %d, because it exceeds the maximum number of " - "combination samples: %d", - static_cast(combinations.size()), parameters_.max_number_of_combination_samples); + "The number of combinations is set to: %zu, because it exceeds the maximum number of " + "combination samples: %zu", + combinations.size(), parameters_.max_number_of_combination_samples); } } -void ExtrinsicReflectorBasedCalibrator::doEvaluation( - std::vector> & combinations, int num_of_samples) +void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( + std::vector> & combinations, std::size_t num_of_samples, + TransformationResult transformation_result) { TransformationEstimator crossval_estimator( initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, radar_optimization_to_lidar_eigen_); - pcl::PointCloud::Ptr crossval_lidar_points_ocs(new pcl::PointCloud); - pcl::PointCloud::Ptr crossval_radar_points_rcs(new pcl::PointCloud); + pcl::PointCloud::Ptr crossval_lidar_points_ocs( + new pcl::PointCloud); + pcl::PointCloud::Ptr crossval_radar_points_rcs( + new pcl::PointCloud); std::vector crossval_converged_tracks_; crossval_lidar_points_ocs->reserve(num_of_samples); crossval_radar_points_rcs->reserve(num_of_samples); @@ -1619,9 +1646,9 @@ void ExtrinsicReflectorBasedCalibrator::doEvaluation( for (std::size_t i = 0; i < combination.size(); i++) { crossval_converged_tracks_.push_back(converged_tracks_[i]); } - auto [delta_cos, delta_sin] = getDelta(crossval_converged_tracks_, true); + auto [delta_cos, delta_sin] = get2DRotationDelta(crossval_converged_tracks_, true); - crossval_estimator.setDelta(delta_cos, delta_sin); + crossval_estimator.set2DRotationDelta(delta_cos, delta_sin); crossval_estimator.estimateYawOnlyTransformation(); } else { crossval_lidar_points_ocs->clear(); @@ -1629,8 +1656,10 @@ void ExtrinsicReflectorBasedCalibrator::doEvaluation( // calculate the transformation. for (std::size_t i = 0; i < combination.size(); i++) { - crossval_lidar_points_ocs->emplace_back(lidar_points_ocs_->points[combination[i]]); - crossval_radar_points_rcs->emplace_back(radar_points_rcs_->points[combination[i]]); + crossval_lidar_points_ocs->emplace_back( + transformation_result.lidar_points_ocs->points[combination[i]]); + crossval_radar_points_rcs->emplace_back( + transformation_result.radar_points_rcs->points[combination[i]]); } crossval_estimator.setPoints(crossval_lidar_points_ocs, crossval_radar_points_rcs); if (transformation_type_ == TransformationType::zero_roll_3d) @@ -1653,7 +1682,7 @@ void ExtrinsicReflectorBasedCalibrator::doEvaluation( auto calculate_std = [](std::vector & data, double mean) -> double { double sum = 0.0; - for (size_t i = 0; i < data.size(); i++) { + for (std::size_t i = 0; i < data.size(); i++) { sum += (data[i] - mean) * (data[i] - mean); } double variance = sum / data.size(); @@ -1676,18 +1705,19 @@ void ExtrinsicReflectorBasedCalibrator::doEvaluation( output_metrics_.push_back(static_cast(std_crossval_calibrated_yaw_error)); } -void ExtrinsicReflectorBasedCalibrator::crossValEvaluation() +void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( + TransformationResult transformation_result) { - int tracks_size = static_cast(converged_tracks_.size()); + auto tracks_size = converged_tracks_.size(); if (tracks_size <= 3) return; - for (int num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) { - std::vector> combinations; - std::vector curr; + for (std::size_t num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) { + std::vector> combinations; + std::vector curr; findCombinations(tracks_size - 1, num_of_samples, curr, 0, combinations); selectCombinations(tracks_size, num_of_samples, combinations); - doEvaluation(combinations, num_of_samples); + evaluateCombinations(combinations, num_of_samples, transformation_result); } } @@ -1713,8 +1743,9 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() return; } output_metrics_.clear(); - estimateTransformation(); - crossValEvaluation(); + auto transformation_result = estimateTransformation(); + evaluateTransformation(transformation_result.calibrated_radar_to_lidar_transformation); + crossValEvaluation(transformation_result); publishMetrics(); } diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index e43baaed..72681bf8 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -32,14 +32,14 @@ TransformationEstimator::TransformationEstimator( } void TransformationEstimator::setPoints( - pcl::PointCloud::Ptr lidar_points_ocs, - pcl::PointCloud::Ptr radar_points_rcs) + pcl::PointCloud::Ptr lidar_points_ocs, + pcl::PointCloud::Ptr radar_points_rcs) { lidar_points_ocs_ = lidar_points_ocs; radar_points_rcs_ = radar_points_rcs; } -void TransformationEstimator::setDelta(double delta_cos, double delta_sin) +void TransformationEstimator::set2DRotationDelta(double delta_cos, double delta_sin) { delta_cos_ = delta_cos; delta_sin_ = delta_sin; @@ -68,7 +68,8 @@ void TransformationEstimator::estimateSVDTransformation( rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate 3D SVD transformation"); } - pcl::registration::TransformationEstimationSVD estimator; + pcl::registration::TransformationEstimationSVD + estimator; Eigen::Matrix4f full_radar_to_radar_optimization_transformation; estimator.estimateRigidTransformation( *lidar_points_ocs_, *radar_points_rcs_, full_radar_to_radar_optimization_transformation); @@ -113,7 +114,7 @@ void TransformationEstimator::estimateZeroRollTransformation() { RCLCPP_INFO( rclcpp::get_logger("marker_radar_lidar_calibrator"), - "Estimate 3D transformation with roll is always zero"); + "Estimate the 3D transformation by restricting the roll to zero"); ceres::Problem problem; @@ -133,7 +134,7 @@ void TransformationEstimator::estimateZeroRollTransformation() RCLCPP_INFO( rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", initial_params_msg.c_str()); - for (size_t i = 0; i < lidar_points_ocs_->points.size(); i++) { + for (std::size_t i = 0; i < lidar_points_ocs_->points.size(); i++) { auto lidar_point = lidar_points_ocs_->points[i]; auto radar_point = radar_points_rcs_->points[i]; @@ -142,7 +143,7 @@ void TransformationEstimator::estimateZeroRollTransformation() ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( new SensorResidual(radar_point_eigen, lidar_point_eigen)); - problem.AddResidualBlock(cost_function, NULL, params.data()); + problem.AddResidualBlock(cost_function, nullptr, params.data()); } // Solve