From 2a0c0a11dda0d92602c7d6107517068b7714fc72 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 1 Oct 2024 21:04:15 +0900 Subject: [PATCH 01/20] chore: tune parameter setting for smooth calibration Signed-off-by: vividf --- .../src/marker_radar_lidar_calibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..2d6fd9a3 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 @@ -184,7 +184,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 = From e4543c083579a4639eb1d2431ffb0cbbbdd3fe26 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 1 Oct 2024 21:18:43 +0900 Subject: [PATCH 02/20] chore: fix default rviz for text markers Signed-off-by: vividf --- .../rviz/default.rviz | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) 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 From 80605945b7503deb3d33bc9e46b4b705f7cbd56d Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 1 Oct 2024 21:18:43 +0900 Subject: [PATCH 03/20] chore: fix default rviz for text markers Signed-off-by: vividf From ede0fda743dba7e56d663194e9d629e2dfe5b093 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 10:30:51 +0900 Subject: [PATCH 04/20] chore: remove unused header Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 1 - 1 file changed, 1 deletion(-) 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..9b9b89bc 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 From a9bf861fcc5b7b9c4487f63025a1ebfd725616f5 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 10:34:31 +0900 Subject: [PATCH 05/20] chore: change function name to evaluateCombinations Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 2 +- .../src/marker_radar_lidar_calibrator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 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 9b9b89bc..5fb63b81 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 @@ -144,7 +144,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node std::vector> & combinations); void selectCombinations( int tracks_size, int num_of_samples, std::vector> & combinations); - void doEvaluation(std::vector> & combinations, int num_of_samples); + void evaluateCombinations(std::vector> & combinations, int num_of_samples); void publishMetrics(); void calibrateSensors(); 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 2d6fd9a3..43996fad 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 @@ -1593,7 +1593,7 @@ void ExtrinsicReflectorBasedCalibrator::selectCombinations( } } -void ExtrinsicReflectorBasedCalibrator::doEvaluation( +void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( std::vector> & combinations, int num_of_samples) { TransformationEstimator crossval_estimator( @@ -1687,7 +1687,7 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation() 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); } } From 9e27beafa2140022638dbe435c9d480a11420a09 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 10:41:43 +0900 Subject: [PATCH 06/20] chore: change function name to get2DRotationDelta Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 3 ++- .../src/marker_radar_lidar_calibrator.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 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 5fb63b81..2184560b 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 @@ -132,7 +132,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node builtin_interfaces::msg::Time & time); std::tuple::Ptr, pcl::PointCloud::Ptr> getPointsSet(); - std::tuple getDelta(std::vector converged_tracks, bool is_crossval); + std::tuple get2DRotationDelta( + std::vector converged_tracks, bool is_crossval); std::pair computeCalibrationError( const Eigen::Isometry3d & radar_to_lidar_isometry); 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 43996fad..15236e7b 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 @@ -1331,7 +1331,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; @@ -1451,7 +1451,7 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation() 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); + auto [delta_cos, delta_sin] = get2DRotationDelta(converged_tracks_, false); estimator.setDelta(delta_cos, delta_sin); estimator.estimateYawOnlyTransformation(); calibrated_radar_to_lidar_transformation = estimator.getTransformation(); @@ -1619,7 +1619,7 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( 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.estimateYawOnlyTransformation(); From 62272dcae499a8ef106cf8c08615ba1a7a7c148e Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 11:06:35 +0900 Subject: [PATCH 07/20] chore: change variable name Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 2 +- .../src/marker_radar_lidar_calibrator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 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 2184560b..fe34e805 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 @@ -256,7 +256,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}; 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 15236e7b..b2576cdb 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 @@ -1141,12 +1141,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) { From 027cf7354921c17d4795af091147fd57867655d0 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 11:14:39 +0900 Subject: [PATCH 08/20] chore: fix waiting to extract the background model Signed-off-by: vividf --- .../src/marker_radar_lidar_calibrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b2576cdb..5f1ad941 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 @@ -407,7 +407,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"); From 1f718805b09797dafe265a5f4dd8fd038c18c121 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 11:23:06 +0900 Subject: [PATCH 09/20] chore: fix grammar error Signed-off-by: vividf --- .../src/marker_radar_lidar_calibrator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 5f1ad941..4c3380c2 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 @@ -479,7 +479,7 @@ 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 = @@ -489,7 +489,7 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( } 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 = @@ -498,7 +498,7 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( 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 = From d56a46d5079d88bc4ddd46542503361f90ceb26d Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 14:47:35 +0900 Subject: [PATCH 10/20] chore: fix function name and clean code Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 4 ++-- .../src/marker_radar_lidar_calibrator.cpp | 9 +++++---- 2 files changed, 7 insertions(+), 6 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 fe34e805..4016e4df 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 @@ -137,8 +137,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node std::pair computeCalibrationError( const Eigen::Isometry3d & radar_to_lidar_isometry); - void estimateTransformation(); - void calculateCalibrationError(Eigen::Isometry3d calibrated_radar_to_lidar_transformation); + Eigen::Isometry3d estimateTransformation(); + void evaluateTransformation(Eigen::Isometry3d calibrated_radar_to_lidar_transformation); void crossValEvaluation(); void findCombinations( int n, int k, std::vector & curr, int first_num, 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 4c3380c2..3af5b68e 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 @@ -1444,7 +1444,7 @@ std::pair ExtrinsicReflectorBasedCalibrator::computeCalibrationE return std::make_pair(distance_error, yaw_error); } -void ExtrinsicReflectorBasedCalibrator::estimateTransformation() +Eigen::Isometry3d ExtrinsicReflectorBasedCalibrator::estimateTransformation() { TransformationEstimator estimator( initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, @@ -1492,10 +1492,10 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation() << calibrated_radar_to_lidar_transformation.matrix()); } - calculateCalibrationError(calibrated_radar_to_lidar_transformation); + return calibrated_radar_to_lidar_transformation; } -void ExtrinsicReflectorBasedCalibrator::calculateCalibrationError( +void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( Eigen::Isometry3d calibrated_radar_to_lidar_transformation) { // Estimate the pre & post calibration error @@ -1713,7 +1713,8 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() return; } output_metrics_.clear(); - estimateTransformation(); + auto calibrated_radar_to_lidar_transformation = estimateTransformation(); + evaluateTransformation(calibrated_radar_to_lidar_transformation); crossValEvaluation(); publishMetrics(); } From 100358f029c8f76e66a5cfdaaba27f650b3c5022 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 14:52:26 +0900 Subject: [PATCH 11/20] chore: remove unnessary logging Signed-off-by: vividf --- .../src/marker_radar_lidar_calibrator.cpp | 2 -- 1 file changed, 2 deletions(-) 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 3af5b68e..d5b1c1d4 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 @@ -1530,8 +1530,6 @@ void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( 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; From 4f35fa2ca86ee71fb2f40902c1d0e13d390bb306 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 16:28:12 +0900 Subject: [PATCH 12/20] chore: fix int to size_t Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 12 +++-- .../src/marker_radar_lidar_calibrator.cpp | 47 ++++++++++--------- 2 files changed, 31 insertions(+), 28 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 4016e4df..f2b6ab0f 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 @@ -141,11 +141,13 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node void evaluateTransformation(Eigen::Isometry3d calibrated_radar_to_lidar_transformation); void crossValEvaluation(); 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 evaluateCombinations(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); void publishMetrics(); void calibrateSensors(); @@ -201,7 +203,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 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 d5b1c1d4..cc3f7951 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,6 +33,7 @@ #include #include +#include #include #include #include @@ -194,8 +195,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"); @@ -1546,20 +1547,20 @@ void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( } 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(); @@ -1569,30 +1570,30 @@ 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::evaluateCombinations( - std::vector> & combinations, int num_of_samples) + std::vector> & combinations, std::size_t num_of_samples) { TransformationEstimator crossval_estimator( initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, @@ -1651,7 +1652,7 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( 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,12 +1677,12 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( void ExtrinsicReflectorBasedCalibrator::crossValEvaluation() { - 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); From 7e19f35455c29502f45ac40541ccd6a7c17659ac Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 16:30:40 +0900 Subject: [PATCH 13/20] chore: fix estimateZeroRollTransformation logging Signed-off-by: vividf --- .../src/transformation_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index e43baaed..a9f962d6 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -113,7 +113,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; From e60783e17e68d938e1779189b1e370b42a057c52 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 16:31:51 +0900 Subject: [PATCH 14/20] chore: add std::size_t Signed-off-by: vividf --- .../src/transformation_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index a9f962d6..f6534f9d 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -133,7 +133,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]; From bd0f11af63484c2b88a782810eeccda01dd0c2cc Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 16:32:31 +0900 Subject: [PATCH 15/20] chore: NULL to nullptr Signed-off-by: vividf --- .../src/transformation_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index f6534f9d..4bda5930 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -142,7 +142,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 From 4bce8c6307e3b3e0827690571eef9d9b47de94b6 Mon Sep 17 00:00:00 2001 From: vividf Date: Wed, 2 Oct 2024 20:14:24 +0900 Subject: [PATCH 16/20] chore: declare point type only once Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 27 ++--- .../transformation_estimator.hpp | 10 +- .../marker_radar_lidar_calibrator/types.hpp | 12 ++- .../src/marker_radar_lidar_calibrator.cpp | 101 ++++++++++-------- .../src/transformation_estimator.cpp | 7 +- 5 files changed, 91 insertions(+), 66 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 f2b6ab0f..a8b0fb75 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 @@ -56,7 +56,6 @@ namespace marker_radar_lidar_calibrator 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 }; @@ -96,29 +95,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(); @@ -131,7 +132,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node const std::vector> & matches, builtin_interfaces::msg::Time & time); - std::tuple::Ptr, pcl::PointCloud::Ptr> getPointsSet(); + std::tuple< + pcl::PointCloud::Ptr, pcl::PointCloud::Ptr> + getPointsSet(); std::tuple get2DRotationDelta( std::vector converged_tracks, bool is_crossval); @@ -291,8 +294,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node std::vector converged_tracks_; // Converged points - pcl::PointCloud::Ptr lidar_points_ocs_; - pcl::PointCloud::Ptr radar_points_rcs_; + 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..1f3c59a6 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,14 +36,13 @@ 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); + pcl::PointCloud::Ptr lidar_points_ocs, + pcl::PointCloud::Ptr radar_points_rcs); void setDelta(double delta_cos, double delta_sin); void estimateYawOnlyTransformation(); void estimateSVDTransformation( @@ -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/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index cc3f7951..81da2449 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 @@ -483,7 +483,7 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( 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(); @@ -493,7 +493,7 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( 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(); @@ -502,7 +502,7 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( 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); } @@ -569,12 +569,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", @@ -607,7 +609,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_) { @@ -644,7 +646,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_); @@ -714,7 +716,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractLidarRefl } template -pcl::PointCloud::Ptr +pcl::PointCloud::Ptr ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr & msg) { static_assert( @@ -725,7 +727,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()); @@ -753,7 +755,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; @@ -766,8 +768,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)); @@ -790,8 +793,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( @@ -831,7 +835,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) { @@ -877,7 +881,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); @@ -922,16 +926,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); @@ -939,7 +945,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()) / @@ -964,7 +971,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; @@ -991,10 +999,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); @@ -1023,16 +1032,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); @@ -1043,10 +1053,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) { @@ -1066,7 +1077,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; @@ -1074,7 +1085,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 = @@ -1089,7 +1100,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; @@ -1380,18 +1392,21 @@ std::tuple ExtrinsicReflectorBasedCalibrator::get2DRotationDelta } 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]; @@ -1599,8 +1614,10 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( 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); diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index 4bda5930..5df15f60 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -32,8 +32,8 @@ 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; @@ -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); From 15995f379dd6d0ff230590e4c39f77c1cd505706 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 3 Oct 2024 10:11:19 +0900 Subject: [PATCH 17/20] chore: remove class member and declare new structure Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 18 +++-- .../src/marker_radar_lidar_calibrator.cpp | 78 ++++++++++++------- 2 files changed, 61 insertions(+), 35 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 a8b0fb75..56c0f8b7 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 @@ -53,6 +53,13 @@ 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: @@ -140,9 +147,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node std::pair computeCalibrationError( const Eigen::Isometry3d & radar_to_lidar_isometry); - Eigen::Isometry3d estimateTransformation(); + TransformationResult estimateTransformation(); void evaluateTransformation(Eigen::Isometry3d calibrated_radar_to_lidar_transformation); - void crossValEvaluation(); + void crossValEvaluation(TransformationResult transformation_result); void findCombinations( std::size_t n, std::size_t k, std::vector & curr, std::size_t first_num, std::vector> & combinations); @@ -150,7 +157,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node 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); + std::vector> & combinations, std::size_t num_of_samples, + TransformationResult transformation_result); void publishMetrics(); void calibrateSensors(); @@ -293,10 +301,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/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp index 81da2449..39ce8af0 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 @@ -38,6 +38,7 @@ #include #include #include +#include #define UPDATE_PARAM(PARAM_STRUCT, NAME) update_param(parameters, #NAME, PARAM_STRUCT.NAME) @@ -1188,9 +1189,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; }); @@ -1447,9 +1453,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); } @@ -1460,55 +1470,63 @@ std::pair ExtrinsicReflectorBasedCalibrator::computeCalibrationE return std::make_pair(distance_error, yaw_error); } -Eigen::Isometry3d 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] = get2DRotationDelta(converged_tracks_, false); estimator.setDelta(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()); } - return calibrated_radar_to_lidar_transformation; + return transformation_result; } void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( @@ -1608,7 +1626,8 @@ void ExtrinsicReflectorBasedCalibrator::selectCombinations( } void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( - std::vector> & combinations, std::size_t num_of_samples) + 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_, @@ -1645,8 +1664,10 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( // 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) @@ -1692,7 +1713,8 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( output_metrics_.push_back(static_cast(std_crossval_calibrated_yaw_error)); } -void ExtrinsicReflectorBasedCalibrator::crossValEvaluation() +void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( + TransformationResult transformation_result) { auto tracks_size = converged_tracks_.size(); if (tracks_size <= 3) return; @@ -1703,7 +1725,7 @@ void ExtrinsicReflectorBasedCalibrator::crossValEvaluation() findCombinations(tracks_size - 1, num_of_samples, curr, 0, combinations); selectCombinations(tracks_size, num_of_samples, combinations); - evaluateCombinations(combinations, num_of_samples); + evaluateCombinations(combinations, num_of_samples, transformation_result); } } @@ -1729,9 +1751,9 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors() return; } output_metrics_.clear(); - auto calibrated_radar_to_lidar_transformation = estimateTransformation(); - evaluateTransformation(calibrated_radar_to_lidar_transformation); - crossValEvaluation(); + auto transformation_result = estimateTransformation(); + evaluateTransformation(transformation_result.calibrated_radar_to_lidar_transformation); + crossValEvaluation(transformation_result); publishMetrics(); } From c123d03542c93bf49a8e442b568f35ee99a35aa3 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 3 Oct 2024 10:18:14 +0900 Subject: [PATCH 18/20] chore: fix description of radar_optimization_frame Signed-off-by: vividf --- .../marker_radar_lidar_calibrator.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 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 56c0f8b7..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 @@ -177,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; From 00f9aa4e17dbfafa7989f0626df6b8f50df5b64d Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 3 Oct 2024 10:40:02 +0900 Subject: [PATCH 19/20] chore: remove if condition Signed-off-by: vividf --- .../src/marker_radar_lidar_calibrator.cpp | 20 ++++++------------- 1 file changed, 6 insertions(+), 14 deletions(-) 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 39ce8af0..94f46a1b 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 @@ -528,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( From c790b938e50aaca2056a258eb0257dd7bd4ef579 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 3 Oct 2024 10:43:20 +0900 Subject: [PATCH 20/20] chore: fix function name setDelta Signed-off-by: vividf --- .../transformation_estimator.hpp | 2 +- .../src/marker_radar_lidar_calibrator.cpp | 4 ++-- .../src/transformation_estimator.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) 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 1f3c59a6..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 @@ -43,7 +43,7 @@ class TransformationEstimator void setPoints( pcl::PointCloud::Ptr lidar_points_ocs, pcl::PointCloud::Ptr radar_points_rcs); - void setDelta(double delta_cos, double delta_sin); + void set2DRotationDelta(double delta_cos, double delta_sin); void estimateYawOnlyTransformation(); void estimateSVDTransformation( ExtrinsicReflectorBasedCalibrator::TransformationType transformation_type); 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 94f46a1b..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 @@ -1471,7 +1471,7 @@ TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation() if (transformation_type_ == TransformationType::yaw_only_rotation_2d) { auto [delta_cos, delta_sin] = get2DRotationDelta(converged_tracks_, false); - estimator.setDelta(delta_cos, delta_sin); + estimator.set2DRotationDelta(delta_cos, delta_sin); estimator.estimateYawOnlyTransformation(); transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation(); RCLCPP_INFO_STREAM( @@ -1648,7 +1648,7 @@ void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( } 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(); diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp index 5df15f60..72681bf8 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -39,7 +39,7 @@ void TransformationEstimator::setPoints( 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;