From af4173c00277a2a2e7b36cb7bd5832fd87f6f4e3 Mon Sep 17 00:00:00 2001 From: vividf Date: Mon, 16 Dec 2024 15:09:28 +0900 Subject: [PATCH] chore: clean code Signed-off-by: vividf --- .../marker_radar_lidar_calibrator/README.md | 82 ++++----- .../marker_radar_lidar_calibrator.hpp | 4 +- .../transformation_estimator.hpp | 1 + .../marker_radar_lidar_calibrator/types.hpp | 21 --- .../visualization.hpp | 7 +- .../calibrator_ui.py | 2 +- .../scripts/metrics_plotter_node.py | 20 ++- .../src/marker_radar_lidar_calibrator.cpp | 38 ++--- .../src/track.cpp | 160 ------------------ .../src/visualization.cpp | 14 +- .../calibrator_wrapper.py | 4 - 11 files changed, 89 insertions(+), 264 deletions(-) delete mode 100644 calibrators/marker_radar_lidar_calibrator/src/track.cpp diff --git a/calibrators/marker_radar_lidar_calibrator/README.md b/calibrators/marker_radar_lidar_calibrator/README.md index 9670142a..9e749fc5 100644 --- a/calibrators/marker_radar_lidar_calibrator/README.md +++ b/calibrators/marker_radar_lidar_calibrator/README.md @@ -134,47 +134,47 @@ TBD. ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------------------------- | ------------- | ------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `radar_parallel_frame` | `std::string` | `base_link` | Auxiliar frame used in the 2d SVD-based method. | -| `msg_type` | `std::string` | `radar_tracks` / `radar_scan` / `radar_cloud` | The type of input radar objects. | -| `transformation_type` | `std::string` | `yaw_only_rotation_2d` / `svd_2d` / `svd_3d` / `roll_zero_3d` | Specifies the algorithm used to optimize the transformation between the radar frame and the radar parallel frame. | -| `use_lidar_initial_crop_box_filter` | `bool` | `true` | Enables or disables the initial cropping filter for lidar data processing. | -| `lidar_initial_crop_box_min_x` | `double` | `-50.0` | Minimum x-coordinate in meters for the initial lidar calibration area. | -| `lidar_initial_crop_box_min_y` | `double` | `-50.0` | Minimum y-coordinate in meters for the initial lidar calibration area. | -| `lidar_initial_crop_box_min_z` | `double` | `-50.0` | Minimum z-coordinate in meters for the initial lidar calibration area. | -| `lidar_initial_crop_box_max_x` | `double` | `50.0` | Maximum x-coordinate in meters for the initial lidar calibration area. | -| `lidar_initial_crop_box_max_y` | `double` | `50.0` | Maximum y-coordinate in meters for the initial lidar calibration area. | -| `lidar_initial_crop_box_max_z` | `double` | `50.0` | Maximum z-coordinate in meters for the initial lidar calibration area. | -| `use_radar_initial_crop_box_filter` | `bool` | `true` | Enables or disables the initial cropping filter for radar data processing. | -| `radar_initial_crop_box_min_x` | `double` | `-50.0` | Minimum x-coordinate in meters for the initial radar calibration area. | -| `radar_initial_crop_box_min_y` | `double` | `-50.0` | Minimum y-coordinate in meters for the initial radar calibration area. | -| `radar_initial_crop_box_min_z` | `double` | `-50.0` | Minimum z-coordinate in meters for the initial radar calibration area. | -| `radar_initial_crop_box_max_x` | `double` | `50.0` | Maximum x-coordinate in meters for the initial radar calibration area. | -| `radar_initial_crop_box_max_y` | `double` | `50.0` | Maximum y-coordinate in meters for the initial radar calibration area. | -| `radar_initial_crop_box_max_z` | `double` | `50.0` | Maximum z-coordinate in meters for the initial radar calibration area. | -| `lidar_background_model_leaf_size` | `double` | `0.1` | Voxel size in meters for the lidar background model. | -| `radar_background_model_leaf_size` | `double` | `0.1` | Voxel size in meters for the radar background model. | -| `max_calibration_range` | `double` | `50.0` | Maximum range for calibration in meters. | -| `background_model_timeout` | `double` | `5.0` | The background model will terminate if there are no new points in the background within this period, measured in seconds. | -| `min_foreground_distance` | `double` | `0.4` | Minimum distance in meters for extracting foreground points. | -| `background_extraction_timeout` | `double` | `15.0` | Timeout in seconds for background extraction processes. | -| `ransac_threshold` | `double` | `0.2` | Distance threshold in meters for the ground segmentation model. | -| `ransac_max_iterations` | `int` | `100` | The maximum number of iterations for the ground segmentation model. | -| `lidar_cluster_max_tolerance` | `double` | `0.5` | Maximum cluster tolerance in meters for extracting lidar cluster. | -| `lidar_cluster_min_points` | `int` | `3` | The minimum number of points required to form a valid lidar cluster. | -| `lidar_cluster_max_points` | `int` | `2000` | The maximum number of points allowed in a lidar cluster. | -| `radar_cluster_max_tolerance` | `double` | `0.5` | Maximum cluster tolerance in meters for extracting radar cluster. | -| `radar_cluster_min_points` | `int` | `1` | The minimum number of points required to form a valid radar cluster. | -| `radar_cluster_max_points` | `int` | `10` | The maximum number of points allowed in a radar cluster. | -| `reflector_radius` | `double` | `0.1` | The radius of the reflector in meters. | -| `reflector_max_height` | `double` | `1.2` | The maximum height in meters of the reflector in meters. | -| `max_matching_distance` | `double` | `1.0` | Maximum distance threshold in meters for matching lidar and radar. | -| `max_initial_calibration_translation_error` | `double` | `1.0` | Maximum allowable translation error in meters in the calibration process. If this error exceeds the specified value, a warning message will appear in the console. | -| `max_initial_calibration_rotation_error` | `double` | `45.0` | Maximum allowable rotation error in degrees in the calibration process. If this error exceeds the specified value, a warning message will appear in the console. | -| `max_number_of_combination_samples` | `int` | `10000` | The maximum number of samples from combinations that are used for cross-validation during the calibration process. | -| `match_count_for_convergence` | `int` | `10` | Number of match count for converge to the center of corner reflector | -| `reflector_points_threshold` | `int` | `10` | When the number of points detected by lidar on the reflector is less than reflector_points_threshold, use the average as the center point. If it is greater than or equal to the threshold, use the point farthest from the radar as the reflector's center point. | +| Name | Type | Default Value | Description | +| ------------------------------------------- | ------------- | ------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `radar_parallel_frame` | `std::string` | `base_link` | Auxiliar frame used in the 2d SVD-based method. | +| `msg_type` | `std::string` | `radar_tracks` / `radar_scan` / `radar_cloud` | The type of input radar objects. | +| `transformation_type` | `std::string` | `yaw_only_rotation_2d` / `svd_2d` / `svd_3d` / `roll_zero_3d` | Specifies the algorithm used to optimize the transformation between the radar frame and the radar parallel frame. | +| `use_lidar_initial_crop_box_filter` | `bool` | `true` | Enables or disables the initial cropping filter for lidar data processing. | +| `lidar_initial_crop_box_min_x` | `double` | `-50.0` | Minimum x-coordinate in meters for the initial lidar calibration area. | +| `lidar_initial_crop_box_min_y` | `double` | `-50.0` | Minimum y-coordinate in meters for the initial lidar calibration area. | +| `lidar_initial_crop_box_min_z` | `double` | `-50.0` | Minimum z-coordinate in meters for the initial lidar calibration area. | +| `lidar_initial_crop_box_max_x` | `double` | `50.0` | Maximum x-coordinate in meters for the initial lidar calibration area. | +| `lidar_initial_crop_box_max_y` | `double` | `50.0` | Maximum y-coordinate in meters for the initial lidar calibration area. | +| `lidar_initial_crop_box_max_z` | `double` | `50.0` | Maximum z-coordinate in meters for the initial lidar calibration area. | +| `use_radar_initial_crop_box_filter` | `bool` | `true` | Enables or disables the initial cropping filter for radar data processing. | +| `radar_initial_crop_box_min_x` | `double` | `-50.0` | Minimum x-coordinate in meters for the initial radar calibration area. | +| `radar_initial_crop_box_min_y` | `double` | `-50.0` | Minimum y-coordinate in meters for the initial radar calibration area. | +| `radar_initial_crop_box_min_z` | `double` | `-50.0` | Minimum z-coordinate in meters for the initial radar calibration area. | +| `radar_initial_crop_box_max_x` | `double` | `50.0` | Maximum x-coordinate in meters for the initial radar calibration area. | +| `radar_initial_crop_box_max_y` | `double` | `50.0` | Maximum y-coordinate in meters for the initial radar calibration area. | +| `radar_initial_crop_box_max_z` | `double` | `50.0` | Maximum z-coordinate in meters for the initial radar calibration area. | +| `lidar_background_model_leaf_size` | `double` | `0.1` | Voxel size in meters for the lidar background model. | +| `radar_background_model_leaf_size` | `double` | `0.1` | Voxel size in meters for the radar background model. | +| `max_calibration_range` | `double` | `50.0` | Maximum range for calibration in meters. | +| `background_model_timeout` | `double` | `5.0` | The background model will terminate if there are no new points in the background within this period, measured in seconds. | +| `min_foreground_distance` | `double` | `0.4` | Minimum distance in meters for extracting foreground points. | +| `background_extraction_timeout` | `double` | `15.0` | Timeout in seconds for background extraction processes. | +| `ransac_threshold` | `double` | `0.2` | Distance threshold in meters for the ground segmentation model. | +| `ransac_max_iterations` | `int` | `100` | The maximum number of iterations for the ground segmentation model. | +| `lidar_cluster_max_tolerance` | `double` | `0.5` | Maximum cluster tolerance in meters for extracting lidar cluster. | +| `lidar_cluster_min_points` | `int` | `3` | The minimum number of points required to form a valid lidar cluster. | +| `lidar_cluster_max_points` | `int` | `2000` | The maximum number of points allowed in a lidar cluster. | +| `radar_cluster_max_tolerance` | `double` | `0.5` | Maximum cluster tolerance in meters for extracting radar cluster. | +| `radar_cluster_min_points` | `int` | `1` | The minimum number of points required to form a valid radar cluster. | +| `radar_cluster_max_points` | `int` | `10` | The maximum number of points allowed in a radar cluster. | +| `reflector_radius` | `double` | `0.1` | The radius of the reflector in meters. | +| `reflector_max_height` | `double` | `1.2` | The maximum height in meters of the reflector in meters. | +| `max_matching_distance` | `double` | `1.0` | Maximum distance threshold in meters for matching lidar and radar. | +| `max_initial_calibration_translation_error` | `double` | `1.0` | Maximum allowable translation error in meters in the calibration process. If this error exceeds the specified value, a warning message will appear in the console. | +| `max_initial_calibration_rotation_error` | `double` | `45.0` | Maximum allowable rotation error in degrees in the calibration process. If this error exceeds the specified value, a warning message will appear in the console. | +| `max_number_of_combination_samples` | `int` | `10000` | The maximum number of samples from combinations that are used for cross-validation during the calibration process. | +| `min_frames_for_convergence` | `int` | `10` | Minimum number of frames required for converging to the center of corner reflector. | +| `reflector_points_threshold` | `int` | `10` | Threshold for determining the method to estimate the center of the reflector. If the number of LiDAR-detected points on the reflector is less than this threshold, the center is calculated as the average of the points. If the number is greater than or equal to the threshold, the farthest point from the radar is used as the reflector's center. | ## Requirements 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 a53d19f7..8b27a211 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 @@ -204,7 +204,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node double max_initial_calibration_translation_error; double max_initial_calibration_rotation_error; std::size_t max_number_of_combination_samples; - int match_count_for_convergence; + int min_frames_for_convergence; std::size_t reflector_points_threshold; } parameters_; @@ -290,7 +290,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node // Tracking bool tracking_active_{false}; int current_new_tracks_{false}; - int count_{0}; + int num_of_frame_{0}; std::vector> converging_tracks_; std::vector converged_tracks_; 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 a5e9cde9..cb208d8a 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 @@ -48,6 +48,7 @@ class TransformationEstimator void estimateZeroRollTransformation(); Eigen::Isometry3d getTransformation(); +private: double delta_cos_; double delta_sin_; pcl::PointCloud::Ptr lidar_points_ocs_; 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 18a5817e..92baae74 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 @@ -73,18 +73,6 @@ struct TransformationResult std::unordered_map calibrated_radar_to_lidar_transformations; - void initializeTransformations() - { - calibrated_radar_to_lidar_transformations[TransformationType::svd_2d] = - Eigen::Isometry3d::Identity(); - calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d] = - Eigen::Isometry3d::Identity(); - calibrated_radar_to_lidar_transformations[TransformationType::svd_3d] = - Eigen::Isometry3d::Identity(); - calibrated_radar_to_lidar_transformations[TransformationType::zero_roll_3d] = - Eigen::Isometry3d::Identity(); - } - void clear() { lidar_points_ocs.reset(); @@ -129,15 +117,6 @@ struct OutputMetrics } detections.clear(); } - - void initializeMethods() - { - methods.clear(); - methods[TransformationType::svd_2d] = CalibrationErrorMetrics(); - methods[TransformationType::yaw_only_rotation_2d] = CalibrationErrorMetrics(); - methods[TransformationType::svd_3d] = CalibrationErrorMetrics(); - methods[TransformationType::zero_roll_3d] = CalibrationErrorMetrics(); - } }; struct Track diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp index e8a990da..caf9f556 100644 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp @@ -36,7 +36,7 @@ struct VisualizationParamters Eigen::Isometry3d initial_radar_to_lidar_eigen; }; -struct Markers +struct DetectionMarkers { visualization_msgs::msg::MarkerArray lidar_detections_marker_array; visualization_msgs::msg::MarkerArray radar_detections_marker_array; @@ -50,7 +50,7 @@ class Visualization ~Visualization() = default; void setParameters(VisualizationParamters params); - Markers visualizationMarkers( + DetectionMarkers visualizeDetectionMarkers( const std::vector & lidar_detections, const std::vector & radar_detections, const std::vector> & matched_detections); @@ -63,9 +63,8 @@ class Visualization const size_t converged_tracks_size, std::unordered_map methods); - VisualizationParamters params_; - private: + VisualizationParamters params_; static constexpr double m_to_cm = 100.0; }; diff --git a/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py b/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py index 44fc9d8e..03e22bed 100644 --- a/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py +++ b/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/calibrator_ui.py @@ -215,7 +215,7 @@ def delete_lidar_radar_pair_button_callback(self): return self.pending_service = True - self.ros_interface.delete_lidar_radar_pair(pair_id) # Pass the integer pair_id + self.ros_interface.delete_lidar_radar_pair(pair_id) self.check_status() def send_calibration_button_callback(self): diff --git a/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py b/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py index 80ce7074..f109380c 100755 --- a/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py +++ b/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py @@ -1,5 +1,19 @@ #!/usr/bin/env python3 +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import math import matplotlib.pyplot as plt @@ -22,8 +36,8 @@ def __init__(self): # Define plot colors and styles self.color_distance_o = "C0o-" # Circle marker, solid line for distance self.color_yaw_o = "C1o-" # Circle marker, solid line for yaw - self.color_distance = "C0" # Solid color for distance (e.g., fill areas) - self.color_yaw = "C1" # Solid color for yaw (e.g., fill areas) + self.color_distance = "C0" + self.color_yaw = "C1" def initialize_figure(self, methods): num_rows = len(methods) + 1 # 1 row for distributions @@ -185,7 +199,7 @@ def plot_detection_distributions(self, ranges, pitches, yaws): # Define bin intervals range_bin_width = 5 # Interval of 5 meters pitch_bin_width = 0.2 # Interval of 0.2 degrees - yaw_bin_width = 10 # Interval of 3 degrees + yaw_bin_width = 10 # Interval of 10 degrees # Create discrete bins range_bins = np.arange( 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 565a8fa4..974d161e 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 @@ -111,7 +111,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para UPDATE_PARAM(p, max_initial_calibration_translation_error); UPDATE_PARAM(p, max_initial_calibration_rotation_error); UPDATE_PARAM(p, max_number_of_combination_samples); - UPDATE_PARAM(p, match_count_for_convergence); + UPDATE_PARAM(p, min_frames_for_convergence); UPDATE_PARAM(p, reflector_points_threshold); // transaction succeeds, now assign values @@ -180,11 +180,11 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( parameters_.max_calibration_range = this->declare_parameter("max_calibration_range", 50.0); parameters_.background_model_timeout = - this->declare_parameter("background_model_timeout", 3.0); + this->declare_parameter("background_model_timeout", 5.0); parameters_.min_foreground_distance = this->declare_parameter("min_foreground_distance", 0.4); parameters_.background_extraction_timeout = - this->declare_parameter("background_extraction_timeout", 5.0); + this->declare_parameter("background_extraction_timeout", 15.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 = @@ -204,10 +204,10 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( parameters_.max_matching_distance = this->declare_parameter("max_matching_distance", 1.0); parameters_.max_number_of_combination_samples = static_cast( this->declare_parameter("max_number_of_combination_samples", 2000)); - parameters_.match_count_for_convergence = - static_cast(this->declare_parameter("match_count_for_convergence", 10)); + parameters_.min_frames_for_convergence = + this->declare_parameter("min_frames_for_convergence", 10); parameters_.reflector_points_threshold = - static_cast(this->declare_parameter("reflector_points_threshold", 10)); + this->declare_parameter("reflector_points_threshold", 10); auto msg_type = this->declare_parameter("msg_type"); auto transformation_type = this->declare_parameter("transformation_type"); @@ -218,7 +218,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } else if (msg_type == "radar_cloud") { msg_type_ = MsgType::radar_cloud; } else { - throw std::runtime_error("Invalid param value: " + msg_type); + throw std::runtime_error("Invalid msg_type value: " + msg_type); } if (transformation_type == "svd_2d") { @@ -230,7 +230,7 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( } else if (transformation_type == "zero_roll_3d") { transformation_type_ = TransformationType::zero_roll_3d; } else { - throw std::runtime_error("Invalid param value: " + transformation_type); + throw std::runtime_error("Invalid transformation_type value: " + transformation_type); } parameters_.max_initial_calibration_translation_error = @@ -258,8 +258,6 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( tracking_markers_pub_ = this->create_publisher("tracking_markers", 10); text_markers_pub_ = this->create_publisher("text_markers", 10); - // metrics_pub_ = - // this->create_publisher("calibration_metrics", 10); metrics_pub_ = this->create_publisher( "calibration_metrics", 10); @@ -517,7 +515,6 @@ void ExtrinsicReflectorBasedCalibrator::loadDatabaseCallback( try { parseConvergedTracks(file, converged_tracks_); - // parseMatrices(file); parseHeader(file, "lidar_header:", lidar_header_); parseHeader(file, "radar_header:", radar_header_); lidar_frame_ = lidar_header_.frame_id; @@ -640,11 +637,11 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback( bool is_track_converged = trackMatches(matches); if (is_track_converged) calibrateSensors(); - auto output_markers = - visualization_.visualizationMarkers(lidar_detections, radar_detections, matches); - lidar_detections_pub_->publish(output_markers.lidar_detections_marker_array); - radar_detections_pub_->publish(output_markers.radar_detections_marker_array); - matches_markers_pub_->publish(output_markers.matches_marker_array); + auto detection_markers = + visualization_.visualizeDetectionMarkers(lidar_detections, radar_detections, matches); + lidar_detections_pub_->publish(detection_markers.lidar_detections_marker_array); + radar_detections_pub_->publish(detection_markers.radar_detections_marker_array); + matches_markers_pub_->publish(detection_markers.matches_marker_array); auto tracking_markers = visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_); @@ -1426,8 +1423,8 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches( return false; } - count_++; - if (count_ < parameters_.match_count_for_convergence) { + num_of_frame_++; + if (num_of_frame_ < parameters_.min_frames_for_convergence) { for (const auto & match : matches) { bool added_to_existing_group = false; @@ -1460,7 +1457,7 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches( std::remove_if( converging_tracks_.begin(), converging_tracks_.end(), [this](const std::vector & tracks) { - return tracks.size() < static_cast(parameters_.match_count_for_convergence / 2); + return tracks.size() < static_cast(parameters_.min_frames_for_convergence / 2); }), converging_tracks_.end()); @@ -1477,12 +1474,11 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches( updateTrackIds(converged_tracks_); - RCLCPP_INFO(this->get_logger(), "counting_matches number= %lu", converging_tracks_.size()); RCLCPP_INFO(this->get_logger(), "converged_tracks size= %lu", converged_tracks_.size()); current_new_tracks_ = converging_tracks_.size(); tracking_active_ = false; - count_ = 0; + num_of_frame_ = 0; converging_tracks_.clear(); if (!current_new_tracks_) { diff --git a/calibrators/marker_radar_lidar_calibrator/src/track.cpp b/calibrators/marker_radar_lidar_calibrator/src/track.cpp deleted file mode 100644 index 158a79fd..00000000 --- a/calibrators/marker_radar_lidar_calibrator/src/track.cpp +++ /dev/null @@ -1,160 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include - -namespace marker_radar_lidar_calibrator -{ - -Track::Track( - builtin_interfaces::msg::Time & t0, const KalmanFilter & initial_lidar_filter, - const KalmanFilter & initial_radar_filter, double lidar_convergence_thresh, - double radar_convergence_thresh, double timeout_thresh, double max_matching_thresh) -: latest_update_time_(t0), - lidar_filter_(initial_lidar_filter), - radar_filter_(initial_radar_filter), - lidar_convergence_thresh_(lidar_convergence_thresh), - radar_convergence_thresh_(radar_convergence_thresh), - timeout_thresh_(timeout_thresh), - max_matching_thresh_(max_matching_thresh), - first_observation_(true) -{ -} - -bool Track::match(const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection) -{ - return (lidar_detection - getLidarEstimation()).norm() < max_matching_thresh_ && - (radar_detection - getRadarEstimation()).norm() < max_matching_thresh_; -} - -bool Track::partialMatch( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection) -{ - return (lidar_detection - getLidarEstimation()).norm() < max_matching_thresh_ || - (radar_detection - getRadarEstimation()).norm() < max_matching_thresh_; -} - -void Track::update(const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection) -{ - lidar_filter_.predict(Eigen::Vector3d::Zero()); - lidar_filter_.update(lidar_detection); - radar_filter_.predict(Eigen::Vector3d::Zero()); - radar_filter_.update(radar_detection); - return; -} - -void Track::updateIfMatch( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection) -{ - if (match(lidar_detection, radar_detection)) { - update(lidar_detection, radar_detection); - } -} - -bool Track::converged() -{ - Eigen::MatrixXd lidar_p_matrix, radar_p_matrix; - - lidar_filter_.getP(lidar_p_matrix); - double lidar_max_p = lidar_p_matrix.diagonal().maxCoeff(); - - radar_filter_.getP(radar_p_matrix); - double radar_max_p = radar_p_matrix.diagonal().maxCoeff(); - - std::cout << "lidar p: " << lidar_max_p << " | " << radar_convergence_thresh_ << std::endl - << std::flush; - std::cout << "radar p: " << radar_max_p << " | " << radar_convergence_thresh_ << std::endl - << std::flush; - - return lidar_max_p < lidar_convergence_thresh_ && radar_max_p < radar_convergence_thresh_; -} - -bool Track::timedOut(builtin_interfaces::msg::Time & time) const -{ - const auto dt = (rclcpp::Time(time) - rclcpp::Time(latest_update_time_)).seconds(); - return dt < 0 || dt > timeout_thresh_; -} - -Eigen::Vector3d Track::getLidarEstimation() -{ - Eigen::MatrixXd lidar_estimation; - lidar_filter_.getX(lidar_estimation); - return Eigen::Vector3d(lidar_estimation.reshaped()); -} - -Eigen::Vector3d Track::getRadarEstimation() -{ - Eigen::MatrixXd radar_estimation; - radar_filter_.getX(radar_estimation); - return Eigen::Vector3d(radar_estimation.reshaped()); -} - -TrackFactory::TrackFactory( - double initial_lidar_cov, double initial_radar_cov, double lidar_measurement_cov, - double radar_measurement_cov, double lidar_process_cov, double radar_process_cov, - double lidar_convergence_thresh, double radar_convergence_thresh, double timeout_thresh, - double max_matching_distance) -: initial_lidar_cov_(initial_lidar_cov * initial_lidar_cov), - initial_radar_cov_(initial_radar_cov * initial_radar_cov), - lidar_convergence_thresh_(lidar_convergence_thresh * lidar_convergence_thresh), - radar_convergence_thresh_(radar_convergence_thresh * radar_convergence_thresh), - timeout_thresh_(timeout_thresh), - max_matching_distance_(max_matching_distance) -{ - lidar_measurement_cov = lidar_measurement_cov * lidar_measurement_cov; - radar_measurement_cov = radar_measurement_cov * radar_measurement_cov; - lidar_process_cov = lidar_process_cov * lidar_process_cov; - radar_process_cov = radar_process_cov * radar_process_cov; - - lidar_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - lidar_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0)); - lidar_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - lidar_filter_.setR(Eigen::DiagonalMatrix( - lidar_measurement_cov, lidar_measurement_cov, lidar_measurement_cov)); - lidar_filter_.setQ( - Eigen::DiagonalMatrix(lidar_process_cov, lidar_process_cov, lidar_process_cov)); - - radar_filter_.setA(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - radar_filter_.setB(Eigen::DiagonalMatrix(0.0, 0.0, 0.0)); - radar_filter_.setC(Eigen::DiagonalMatrix(1.0, 1.0, 1.0)); - radar_filter_.setR(Eigen::DiagonalMatrix( - radar_measurement_cov, radar_measurement_cov, radar_measurement_cov)); - radar_filter_.setQ( - Eigen::DiagonalMatrix(radar_process_cov, radar_process_cov, radar_process_cov)); -} - -Track TrackFactory::makeTrack( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection, - builtin_interfaces::msg::Time & t0) -{ - auto lidar_filter = lidar_filter_; - auto radar_filter = radar_filter_; - Eigen::DiagonalMatrix lidar_p0( - initial_lidar_cov_, initial_lidar_cov_, initial_lidar_cov_); - Eigen::DiagonalMatrix radar_p0( - initial_radar_cov_, initial_radar_cov_, initial_radar_cov_); - lidar_filter.init(lidar_detection, lidar_p0); - radar_filter.init(radar_detection, radar_p0); - - return Track( - t0, lidar_filter, radar_filter, lidar_convergence_thresh_, radar_convergence_thresh_, - timeout_thresh_, max_matching_distance_); -} - -} // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/src/visualization.cpp b/calibrators/marker_radar_lidar_calibrator/src/visualization.cpp index 2b21fba5..be67fab3 100644 --- a/calibrators/marker_radar_lidar_calibrator/src/visualization.cpp +++ b/calibrators/marker_radar_lidar_calibrator/src/visualization.cpp @@ -25,12 +25,12 @@ namespace marker_radar_lidar_calibrator void Visualization::setParameters(VisualizationParamters params) { params_ = params; } -Markers Visualization::visualizationMarkers( +DetectionMarkers Visualization::visualizeDetectionMarkers( const std::vector & lidar_detections, const std::vector & radar_detections, const std::vector> & matched_detections) { - Markers output_markers; + DetectionMarkers detection_markers; for (std::size_t detection_index = 0; detection_index < lidar_detections.size(); detection_index++) { const auto & detection_center = lidar_detections[detection_index]; @@ -51,7 +51,7 @@ Markers Visualization::visualizationMarkers( marker.color.r = 0.0; marker.color.g = 0.0; marker.color.b = 1.0; - output_markers.lidar_detections_marker_array.markers.push_back(marker); + detection_markers.lidar_detections_marker_array.markers.push_back(marker); } // Radar makers @@ -76,7 +76,7 @@ Markers Visualization::visualizationMarkers( marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 1.0; - output_markers.radar_detections_marker_array.markers.push_back(marker); + detection_markers.radar_detections_marker_array.markers.push_back(marker); // For 2D radar detection to represent that it has no z values. if ( @@ -92,7 +92,7 @@ Markers Visualization::visualizationMarkers( marker.scale.z = 0.2 * params_.reflector_radius; marker.points.push_back(p1); marker.points.push_back(p2); - output_markers.radar_detections_marker_array.markers.push_back(marker); + detection_markers.radar_detections_marker_array.markers.push_back(marker); } } @@ -117,10 +117,10 @@ Markers Visualization::visualizationMarkers( marker.color.b = 1.0; marker.points.push_back(eigenToPointMsg(lidar_detection_transformed)); marker.points.push_back(eigenToPointMsg(radar_detection)); - output_markers.matches_marker_array.markers.push_back(marker); + detection_markers.matches_marker_array.markers.push_back(marker); } - return output_markers; + return detection_markers; } visualization_msgs::msg::MarkerArray Visualization::visualizeTrackMarkers( diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py b/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py index cafcd98e..eb1dc768 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrator_wrapper.py @@ -118,10 +118,6 @@ def on_result(self, calibration_results: List[CalibrationResult]): parent_frame = calibration_result.transform_stamped.header.frame_id child_frame = calibration_result.transform_stamped.child_frame_id - print("debug: parent_frame: ", parent_frame) - print("debug: child_frame: ", child_frame) - print("debug: self.children: ", self.children) - if parent_frame not in self.parents or child_frame not in self.children: logging.error( f"Calibration result {parent_frame} -> {child_frame} was not expected. This is probably a configuration error in the launchers"