diff --git a/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt b/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt index abb003dd..b6cce020 100644 --- a/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt +++ b/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt @@ -3,32 +3,26 @@ cmake_minimum_required(VERSION 3.5) project(marker_radar_lidar_calibrator) find_package(autoware_cmake REQUIRED) - autoware_package() -ament_python_install_package(${PROJECT_NAME}) +find_package(Ceres REQUIRED) -ament_export_include_directories( - include - ${OpenCV_INCLUDE_DIRS} -) +ament_python_install_package(${PROJECT_NAME}) ament_auto_add_executable(marker_radar_lidar_calibrator src/marker_radar_lidar_calibrator.cpp - src/track.cpp + src/transformation_estimator.cpp + src/visualization.cpp + src/utils.cpp src/main.cpp ) target_link_libraries(marker_radar_lidar_calibrator - ${OpenCV_LIBS} + ${CERES_LIBRARIES} ) install(PROGRAMS scripts/calibrator_ui_node.py - DESTINATION lib/${PROJECT_NAME} -) - -install(PROGRAMS scripts/metrics_plotter_node.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/calibrators/marker_radar_lidar_calibrator/README.md b/calibrators/marker_radar_lidar_calibrator/README.md index 97f6f5f6..f836abb4 100644 --- a/calibrators/marker_radar_lidar_calibrator/README.md +++ b/calibrators/marker_radar_lidar_calibrator/README.md @@ -6,23 +6,25 @@ A tutorial for this calibrator can be found [here](../../docs/tutorials/marker_r The package `marker_radar_lidar_calibrator` performs extrinsic calibration between radar and 3d lidar sensors used in autonomous driving and robotics. -Currently, the calibrator only supports radars whose detection interface includes distance and azimuth angle, but do not offer elevation angle. For example, ARS408 radars can be calibrated with this tool. Also, note that the 3d lidar should have a high enough resolution to present several returns on the [radar reflector](#radar-reflector) (calibration target). +The calibrator supports radars whose detection interface includes distance, azimuth angle, and elevation angle, such as the ARS548. It also supports radars that provide distance and azimuth angle but lack elevation angle capability, such as the ARS408. + +Also, note that the 3d lidar should have a high enough resolution to present several returns on the [radar reflector](#radar-reflector) (calibration target). ## Inner-workings / Algorithms -The calibrator computes the center of the reflectors from the pointcloud and pairs them to the radar objects/tracks. Afterwards, both an SVD-based and a yaw-only rotation estimation algorithm are applied to these matched points to estimate the rigid transformation between sensors. +The calibrator computes the center of the reflectors from the pointcloud and pairs them to the radar objects/tracks. Afterward, estimation algorithms are applied to these matched points to estimate the rigid transformation between sensors. -Due to the complexity of the problem, the process in split in the following steps: constructing a background model, extracting the foreground to detect reflectors, matching and filtering the lidar and radar detections, and estimating the rigid transformation between the radar and lidar sensors. +Due to the complexity of the problem, the process is split into the following steps: constructing a background model, extracting the foreground to detect reflectors, matching and filtering the lidar and radar detections, and estimating the rigid transformation between the radar and lidar sensors. -In what follows, we proceed to explain each step, making a point to put emphasis on the parts that the user must take into consideration to use phis package effectively. +In what follows, we proceed to explain each step, making a point to put emphasis on the parts that the user must take into consideration to use this package effectively. \*Note: although the radar can provide either detections and/or objects/tracks, we treat them as points in this package, and as such may refer to the radar pointcloud when needed. ### Step 1: Background model construction -Detecting corner reflectors in an unknown environment, without imposing impractical restrictions on the reflectors themselves, the operators, or the environment, it a challenging problem. From the perspective of the lidar, radar reflectors may be confused with the floor or other metallic objects, and from the radar's perspective, although corner reflectors are detected by the sensor (the user must confirm it themselves before attempting to use this tool!), other objects are also detected, with no practical way to tell them apart most of the time. +Detecting corner reflectors in an unknown environment, without imposing impractical restrictions on the reflectors themselves, the operators, or the environment, is a challenging problem. From the perspective of the lidar, radar reflectors may be confused with the floor or other metallic objects, and from the radar's perspective, although corner reflectors are detected by the sensor (the user must confirm it themselves before attempting to use this tool!), other objects are also detected, with no practical way to tell them apart most of the time. -For these reasons, we avoid addressing the full problem an instead leverage the use of background models. To do this, the user must first present the sensors an environment with no radar reflectors nor any dynamic objects (mostly persons) in the space that is to be used for calibration. The tool will collect data for a set period of time or until there is no new information. For each modality, this data is then turned into voxels, marking the space of each occupied voxel as `background` in the following steps. +For these reasons, we avoid addressing the full problem and instead leverage the use of background models. To do this, the user must first present the sensors an environment with no radar reflectors nor any dynamic objects (mostly persons) in the space that is to be used for calibration. The tool will collect data for a set period of time or until there is no new information. For each modality, this data is then turned into voxels, marking the space of each occupied voxel as `background` in the following steps. ### Step 2: Foreground extraction and reflector detection @@ -55,122 +57,131 @@ Once background model construction finishes and the foreground extraction proces ### Step 3: Matching and filtering -The output of the previous step consists of two lists of points of potentials radar reflector candidates for each sensor. However, it is not possible to directly match points among these lists, and they are expected to contain a high number of false positives on both sensors. +The output of the previous step consists of two lists of points of potential radar reflector candidates for each sensor. However, it is not possible to directly match points among these lists, and they are expected to contain a high number of false positives on both sensors. To address this issue, we rely on a heuristic that leverages the accuracy of initial calibration. Usually, robot/vehicle CAD designs allow an initial calibration with an accuracy of a few centimeters/degrees, and direct sensor calibration is only used to refine it. Using the initial radar-lidar calibration, we project each lidar corner reflector candidate into the radar coordinates and for each candidate we compute the closest candidate from the other modality. We consider real radar-lidar pairs of corner reflectors those pairs who are mutually their closest candidate. -Matches using this heuristic can still contain incorrect pairs and false positives, which is why we employ a Kalman filter to both improve the estimations and check for temporal consistency (false positives are not usually consistent in time). - -Once matches' estimations converge (using a covariance matrix criteria), they are added to the calibration list. +Matches using this heuristic can still contain incorrect pairs and false positives. To address this, we use a naive tracking algorithm to verify whether the matches fall within the radar reflector's radius compared to matches from the previous frame. Once the pairs are consistently tracked over a specified number of frames, we assume the matches have converged and add them to the calibration list. ### Step 4: Rigid transformation estimation -After matching detection pairs, we apply rigid transformation estimation algorithms to those pairs to estimate the transformation between the radar and lidar sensors. We currently support two algorithms: a 2d SVD-based method and a yaw-only rotation method. +After matching detection pairs, we apply rigid transformation estimation algorithms to those pairs to estimate the transformation between the radar and lidar sensors. Currently, we support four algorithms: the 2d SVD-based method, the yaw-only rotation method, the 3d zero-roll SVD-based method, and the 3d SVD method. + +The 2d SVD-based method and the yaw-only rotation method are designed for radars that provide distance and azimuth angle measurements but lack elevation angle capability, such as the ARS408. In contrast, the 3d zero-roll SVD-based method and the 3d SVD method are suited for radars that include distance, azimuth angle, and elevation angle in their detection interface, such as the ARS548. ### 2d SVD-based method In this method, we reduce the problem to a 2d transformation estimation since radar detections lack a z component (elevation is fixed to zero). -However, because lidar detections are in the lidar frame and likely involve a 3d transformation (non-zero roll and\or pitch) to the radar frame, we transform the lidar detections to a frame dubbed the `radar parallel` frame and then set their z component to zero. The `radar parallel` frame has only a 2d transformation (x, y, yaw) relative to the radar frame. By dropping the z-component we explicitly give up on computing a 3D pose, which was not possible due to the nature of the radar. +However, because lidar detections are in the lidar frame and likely involve a 3d transformation (non-zero roll and\or pitch) to the radar frame, we transform the lidar detections to a frame dubbed the `radar optimization` frame and then set their z component to zero. The `radar optimization` frame has only a 2d transformation (x, y, yaw) relative to the radar frame. By dropping the z-component we explicitly give up on computing a 3d pose, which was not possible due to the nature of the radar. -In autonomous vehicles, radars are mounted in a way designed to minimize pitch and roll angles, maximizing their performance and measurement range. This means the radar sensors are aligned as parallel as possible to the ground plane, making the `base_link` a suitable choice for the `radar parallel` frame. +In autonomous vehicles, radars are mounted in a way designed to minimize pitch and roll angles, maximizing their performance and measurement range. This means the radar sensors are aligned as parallel as possible to the ground plane, making the `base_link` a suitable choice for the `radar optimization` frame. -\*\*Note: this assumes that the lidar to `radar parallel` frame is either hardcoded or previously calibrated +\*\*Note: this assumes that the lidar to `radar optimization` frame is either hardcoded or previously calibrated -Next, we apply the SVD-based rigid transformation estimation algorithm between the lidar detections in the radar parallel frame and the radar detections in the radar frame. This allows us to estimate the transformation between the lidar and radar by multiplying the radar-to-radar-parallel transformation (calibrated) with the radar-parallel-to-lidar transformation (known before-handed). The SVD-based algorithm, provided by PCL, leverages SVD to find the optimal rotation component and then computes the translation component based on the rotation. +Next, we apply the SVD-based rigid transformation estimation algorithm between the lidar detections in the radar optimization frame and the radar detections in the radar frame. This allows us to estimate the transformation between the lidar and radar by multiplying the radar-to-radar-parallel transformation (calibrated) with the radar-parallel-to-lidar transformation (known before-handed). The SVD-based algorithm, provided by PCL, leverages SVD to find the optimal rotation component and then computes the translation component based on the rotation. ### Yaw-only rotation method -This method, on the other hand, utilizes the initial radar-to-lidar transformation to calculate lidar detections in the radar frame. We then calculate the average yaw angle difference of all pairs, considering only yaw rotation between the lidar and radar detections in the radar frame, to estimate a yaw-only rotation transformation in the radar frame. Finally, we estimate the transformation between the lidar and radar by multiplying the yaw-only rotation transformation with the initial radar-to-lidar transformation. +This method utilizes the initial radar-to-lidar transformation to calculate lidar detections in the radar frame. We then calculate the average yaw angle difference of all pairs, considering only yaw rotation between the lidar and radar detections in the radar frame, to estimate a yaw-only rotation transformation in the radar frame. Finally, we estimate the transformation between the lidar and radar by multiplying the yaw-only rotation transformation with the initial radar-to-lidar transformation. Generally, the 2d SVD-based method is preferred when valid; otherwise, the yaw-only rotation method is used as the calibration output. -### Diagram +### 3d zero roll SVD-based method -Below, you can see how the algorithm is implemented in the `marker_radar_lidar_calibrator` package. +This method is designed for radars that provide distance, azimuth angle, and elevation angle, but requires the roll angle to be fixed at zero relatives to the `radar optimization` frame / `base_link`. +We apply the 3d SVD-based rigid transformation estimation algorithm to optimize the pitch, yaw, and translation components while explicitly fixing the roll angle to zero. The optimization is performed between the LiDAR detections in the radar optimization frame and the radar detections in the radar frame. -![marker_radar_lidar_calibrator](../../docs/images/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.jpg) +### 3d SVD method + +This method applies to radars that provide distance, azimuth angle, and elevation angle without making any assumptions about the roll, pitch, or yaw angles. + +The 3d SVD rigid transformation estimation algorithm is directly applied to the lidar detections in the radar optimization frame and the radar detections in the radar frame, optimizing the complete 3d transformation, including roll, pitch, yaw, and translation components. ## ROS Interfaces ### Input -| Name | Type | Description | -| ------------------------ | ------------------------------- | ------------------------- | -| `input_lidar_pointcloud` | `sensor_msgs::msg::PointCloud2` | Lidar pointcloud's topic. | -| `input_radar_msg` | `radar_msgs::msg::RadarTracks` | Radar objects' topic. | +| Name | Type | Description | +| ------------------------ | ----------------------------------------------------------------------------------------------- | -------------------------------------------- | +| `input_lidar_pointcloud` | `sensor_msgs::msg::PointCloud2` | Topic for the lidar pointcloud. | +| `input_radar_msg` | `radar_msgs::msg::RadarTracks` / `radar_msgs::msg::RadarScan` / `sensor_msgs::msg::PointCloud2` | Topic for radar tracks, scan, or pointcloud. | ### Output -| Name | Type | Description | -| ----------------------------- | -------------------------------------- | --------------------------------------------------------- | -| `lidar_background_pointcloud` | `sensor_msgs::msg::PointCloud2` | Lidar's background pointcloud. | -| `lidar_foreground_pointcloud` | `sensor_msgs::msg::PointCloud2` | Lidar's foreground pointcloud. | -| `lidar_colored_clusters` | `sensor_msgs::msg::PointCloud2` | Lidar's colored pointcloud clusters. | -| `lidar_detection_markers` | `visualization_msgs::msg::MarkerArray` | Lidar detections. | -| `radar_background_pointcloud` | `sensor_msgs::msg::PointCloud2` | Radar's background pointcloud from the radar. | -| `radar_foreground_pointcloud` | `sensor_msgs::msg::PointCloud2` | Radar's foreground pointcloud from the radar. | -| `radar_detection_markers` | `visualization_msgs::msg::MarkerArray` | Radar detections. | -| `matches_markers` | `visualization_msgs::msg::MarkerArray` | Matched lidar and radar detections. | -| `tracking_markers` | `visualization_msgs::msg::MarkerArray` | Reflectors' tracks. | -| `text_markers` | `visualization_msgs::msg::Marker` | Calibration metrics' markers. | -| `calibration_metrics` | `std_msgs::msg::Float32MultiArray` | Calibration metrics as vector for visualization purposes. | +| Name | Type | Description | +| ----------------------------- | ---------------------------------------------------- | --------------------------------------------------------- | +| `lidar_background_pointcloud` | `sensor_msgs::msg::PointCloud2` | Lidar's background pointcloud. | +| `lidar_foreground_pointcloud` | `sensor_msgs::msg::PointCloud2` | Lidar's foreground pointcloud. | +| `lidar_colored_clusters` | `sensor_msgs::msg::PointCloud2` | Lidar's colored pointcloud clusters. | +| `lidar_detection_markers` | `visualization_msgs::msg::MarkerArray` | Lidar detections. | +| `radar_background_pointcloud` | `sensor_msgs::msg::PointCloud2` | Radar's background pointcloud from the radar. | +| `radar_foreground_pointcloud` | `sensor_msgs::msg::PointCloud2` | Radar's foreground pointcloud from the radar. | +| `radar_detection_markers` | `visualization_msgs::msg::MarkerArray` | Radar detections. | +| `matches_markers` | `visualization_msgs::msg::MarkerArray` | Matched lidar and radar detections. | +| `tracking_markers` | `visualization_msgs::msg::MarkerArray` | Reflectors' tracks. | +| `text_markers` | `visualization_msgs::msg::Marker` | Calibration metrics' markers. | +| `calibration_metrics` | `tier4_calibration_msgs::msg::` `CalibrationMetrics` | Calibration metrics as vector for visualization purposes. | ### Services -| Name | Type | Description | -| -------------------------- | ----------------------------------------------------- | ---------------------------------------------------------------------------------------------------- | -| `extrinsic_calibration` | `tier4_calibration_msgs::` `srv::ExtrinsicCalibrator` | Generic calibration service. The call is blocked until the calibration process finishes. | -| `extract_background_model` | `std_srvs::srv::Empty` | Starts to extract the background model from radar and lidar data. | -| `add_lidar_radar_pair` | `std_srvs::srv::Empty` | Adds lidar-radar pairs for calibration. | -| `delete_lidar_radar_pair` | `std_srvs::srv::Empty` | Deletes the latest lidar-radar pair. | -| `send_calibration` | `std_srvs::srv::Empty` | Finishes the calibration process and sends the calibration result to the sensor calibration manager. | +| Name | Type | Description | +| -------------------------- | ------------------------------------------------------ | ---------------------------------------------------------------------------------------------------- | +| `extrinsic_calibration` | `tier4_calibration_msgs::` `srv::ExtrinsicCalibrator` | Generic calibration service. The call is blocked until the calibration process finishes. | +| `extract_background_model` | `std_srvs::srv::Empty` | Starts to extract the background model from radar and lidar data. | +| `add_lidar_radar_pair` | `std_srvs::srv::Empty` | Adds lidar-radar pairs for calibration. | +| `delete_lidar_radar_pair` | `tier4_calibration_msgs::` `srv::DeleteLidarRadarPair` | Deletes the LiDAR-radar pair with the specified ID. | +| `send_calibration` | `std_srvs::srv::Empty` | Finishes the calibration process and sends the calibration result to the sensor calibration manager. | +| `load_database` | `tier4_calibration_msgs::srv::FileSrv` | Loads the matched lidar and radar pairs from a file. | +| `save_database` | `tier4_calibration_msgs::srv::FileSrv` | Saves the matched lidar and radar pairs to a file. | ## Parameters ### 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` | The type of input radar objects. (Not available yet, currently only support radar tracks.) | -| `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. (Not available yet.) | -| `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. | +| Name | Type | Default Value | Description | +| ------------------------------------------- | ------------- | ------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `radar_optimization_frame` | `std::string` | `base_link` | Auxiliar frame used in the 2d SVD-based method, 3d zero roll SVD-based method and 3d svd 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 optimization 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 @@ -188,9 +199,7 @@ It is recommended that the user mount the radar reflector on a tripod and ensure - While extracting the background model, ensure that no reflector, person, or moving object is present in the calibration area. -- The calibrator provides a button to delete any mismatched pairs (e.g., an object detected by both radar and lidar). However, some outliers may not be easily detectable by human eyes, leading to inaccurate results as the calibration proceeds even with these anomalies present. Future enhancements will aim to improve outlier detection, thereby refining the calibration accuracy. - -- The calibrator should be able to handle different lidar and radar sensors. So far, We calibrated the Velodyne VLS-128 lidar sensor, Pandar-40P lidar sensor, and ARS 408 radar sensor with good calibration results. +- The calibrator should be able to handle different lidar and radar sensors. So far, We calibrated the Velodyne VLS-128 lidar sensor, Pandar-40P lidar sensor, ARS 408 radar sensor, and ARS 548 radar sensor with good calibration results. ## Pro tips/recommendations 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 d7c9fd82..92e818df 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 @@ -12,23 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ -#define MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ +#pragma once #include -#include #include +#include #include #include #include #include #include +#include #include #include #include #include +#include +#include +#include #include +#include #include #include @@ -38,13 +42,11 @@ #include #include -#include +#include #include -#include #include #include #include -#include #include #include #include @@ -56,9 +58,12 @@ namespace marker_radar_lidar_calibrator class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node { public: - using PointType = pcl::PointXYZ; using index_t = std::uint32_t; + enum class MsgType { radar_tracks, radar_scan, radar_cloud }; + + enum class CornerReflectorEstimationMethod { average_points, longest_distance_point }; + explicit ExtrinsicReflectorBasedCalibrator(const rclcpp::NodeOptions & options); protected: @@ -81,32 +86,55 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node const std::shared_ptr response); void deleteTrackRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + const std::shared_ptr request, + const std::shared_ptr response); + + void loadDatabaseCallback( + const std::shared_ptr request, + std::shared_ptr response); + + void saveDatabaseCallback( + const std::shared_ptr request, + std::shared_ptr response); + + void logErrorAndRespond( + std::shared_ptr & response, + const std::string & error_message); void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void radarCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg); - std::vector extractReflectors( + void radarTracksCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg); + + void radarScanCallback(const radar_msgs::msg::RadarScan::SharedPtr msg); + + void radarCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + template + pcl::PointCloud::Ptr extractRadarPointcloud( + const std::shared_ptr & msg); + + std::vector extractLidarReflectors( const sensor_msgs::msg::PointCloud2::SharedPtr msg); - std::vector extractReflectors(const radar_msgs::msg::RadarTracks::SharedPtr msg); + std::vector extractRadarReflectors( + 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::Vector4f & ground_model); + pcl::PointCloud::Ptr & foreground_points, + Eigen::Vector4f & 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::Vector4f & ground_model); bool checkInitialTransforms(); @@ -115,42 +143,33 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node const std::vector & lidar_detections, const std::vector & radar_detections); - bool trackMatches( - const std::vector> & matches, - builtin_interfaces::msg::Time & time); - - std::tuple::Ptr, pcl::PointCloud::Ptr, double, double> - getPointsSetAndDelta(); - std::pair computeCalibrationError( - const Eigen::Isometry3d & radar_to_lidar_isometry); - void estimateTransformation( - pcl::PointCloud::Ptr lidar_points_pcs, - pcl::PointCloud::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum); - void findCombinations( - int n, int k, std::vector & curr, int first_num, - std::vector> & combinations); - void crossValEvaluation( - pcl::PointCloud::Ptr lidar_points_pcs, - pcl::PointCloud::Ptr radar_points_rcs); + bool trackMatches(const std::vector> & matches); + + std::tuple< + pcl::PointCloud::Ptr, pcl::PointCloud::Ptr> + getPointsSet(std::vector::iterator & begin, std::vector::iterator & end); + std::tuple get2DRotationDelta( + std::vector::iterator & begin, std::vector::iterator & end, bool is_crossval); + + TransformationResult estimateTransformation(std::size_t track_index); + void evaluateTransformation(TransformationResult transformation_result, std::size_t track_index); + void crossValEvaluation(TransformationResult transformation_result); + void evaluateCombinations( + std::vector> & combinations, std::size_t num_of_samples, + TransformationResult transformation_result); + void publishMetrics(); void calibrateSensors(); - void visualizationMarkers( - const std::vector & lidar_detections, - const std::vector & radar_detections, - const std::vector> & matched_detections); - void visualizeTrackMarkers(); - void deleteTrackMarkers(); - void drawCalibrationStatusText(); - geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen); - double getYawError(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2); rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); struct Parameters { - std::string radar_parallel_frame; // frame that is assumed to be parallel to the radar (needed - // for radars that do not provide elevation) + std::string radar_optimization_frame; // If the radar does not provide elevation, + // this frame needs to be parallel to the radar + // and should only use the 2D transformation. + bool use_lidar_initial_crop_box_filter; double lidar_initial_crop_box_min_x; double lidar_initial_crop_box_min_y; @@ -185,7 +204,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node double max_matching_distance; double max_initial_calibration_translation_error; double max_initial_calibration_rotation_error; - int max_number_of_combination_samples; + std::size_t max_number_of_combination_samples; + int min_frames_for_convergence; + std::size_t reflector_points_threshold; } parameters_; // ROS Interface @@ -209,17 +230,22 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr matches_markers_pub_; rclcpp::Publisher::SharedPtr tracking_markers_pub_; rclcpp::Publisher::SharedPtr text_markers_pub_; - rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; rclcpp::Subscription::SharedPtr lidar_sub_; - rclcpp::Subscription::SharedPtr radar_sub_; + rclcpp::Subscription::SharedPtr radar_tracks_sub_; + rclcpp::Subscription::SharedPtr radar_scan_sub_; + rclcpp::Subscription::SharedPtr radar_cloud_sub_; rclcpp::Service::SharedPtr calibration_request_server_; + rclcpp::Service::SharedPtr load_database_service_server_; + rclcpp::Service::SharedPtr save_database_service_server_; rclcpp::Service::SharedPtr background_model_service_server_; rclcpp::Service::SharedPtr tracking_service_server_; rclcpp::Service::SharedPtr send_calibration_service_server_; - rclcpp::Service::SharedPtr delete_track_service_server_; + rclcpp::Service::SharedPtr + delete_track_service_server_; // Threading, sync, and result std::mutex mutex_; @@ -233,8 +259,12 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node Eigen::Isometry3d initial_radar_to_lidar_eigen_; Eigen::Isometry3d calibrated_radar_to_lidar_eigen_; - geometry_msgs::msg::Transform radar_parallel_to_lidar_msg_; - Eigen::Isometry3d radar_parallel_to_lidar_eigen_; + // radar optimization is the frame that radar optimize the transformation to. + geometry_msgs::msg::Transform radar_optimization_to_lidar_msg_; + Eigen::Isometry3d radar_optimization_to_lidar_eigen_; + + geometry_msgs::msg::Transform initial_radar_optimization_to_radar_msg_; + Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_; bool got_initial_transform_{false}; bool broadcast_tf_{false}; @@ -254,21 +284,25 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node BackgroundModel lidar_background_model_; BackgroundModel radar_background_model_; - radar_msgs::msg::RadarTracks::SharedPtr latest_radar_msgs_; + radar_msgs::msg::RadarTracks::SharedPtr latest_radar_tracks_msgs_; + radar_msgs::msg::RadarScan::SharedPtr latest_radar_scan_msgs_; + sensor_msgs::msg::PointCloud2::SharedPtr latest_radar_cloud_msgs_; // Tracking bool tracking_active_{false}; int current_new_tracks_{false}; - TrackFactory::Ptr factory_ptr_; - std::vector active_tracks_; + int num_of_frame_{0}; + std::vector> converging_tracks_; std::vector converged_tracks_; // Metrics - std::vector output_metrics_; + OutputMetrics output_metrics_; - static constexpr int MARKER_SIZE_PER_TRACK = 8; + // Visualization + Visualization visualization_; + + MsgType msg_type_; + TransformationType transformation_type_; }; } // namespace marker_radar_lidar_calibrator - -#endif // MARKER_RADAR_LIDAR_CALIBRATOR__MARKER_RADAR_LIDAR_CALIBRATOR_HPP_ diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp new file mode 100644 index 00000000..311f30af --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp @@ -0,0 +1,61 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace marker_radar_lidar_calibrator +{ + +struct SensorResidual +{ + SensorResidual(const Eigen::Vector4d & radar_point, const Eigen::Vector4d & lidar_point) + : radar_point_(radar_point), lidar_point_(lidar_point) + { + } + + template + bool operator()(T const * const params, T * s_residuals) const + { + // parameters: x, y, z, pitch, yaw. + Eigen::Matrix transformation_matrix = Eigen::Matrix::Identity(4, 4); + Eigen::Matrix rotation_matrix; + + transformation_matrix(0, 3) = T(params[0]); + transformation_matrix(1, 3) = T(params[1]); + transformation_matrix(2, 3) = T(params[2]); + + // This rotation matrix is rotate from radar to optimization frames (usually base_link). + // To avoid make sure that the Y axis does not approaches 90 degrees to avoid gimbal lock. + rotation_matrix = (Eigen::AngleAxis(T(params[4]), Eigen::Vector3::UnitZ()) * + Eigen::AngleAxis(T(params[3]), Eigen::Vector3::UnitY()) * + Eigen::AngleAxis(T(0), Eigen::Vector3::UnitX())) + .matrix(); + + transformation_matrix.block(0, 0, 3, 3) = rotation_matrix; + + Eigen::Map> residuals(s_residuals); + Eigen::Matrix residuals4d = + lidar_point_.cast() - transformation_matrix * radar_point_.cast(); + residuals = residuals4d.block(0, 0, 3, 1); + + return true; + } + + Eigen::Vector4d radar_point_; + Eigen::Vector4d lidar_point_; +}; + +} // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp deleted file mode 100644 index 769278f7..00000000 --- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/track.hpp +++ /dev/null @@ -1,91 +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. - -#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ -#define MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ - -#include -#include -#include -#include -#include - -#include - -namespace marker_radar_lidar_calibrator -{ - -using autoware::kalman_filter::KalmanFilter; - -class Track -{ -public: - bool match(const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection); - bool partialMatch( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection); - void update(const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection); - void updateIfMatch( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection); - bool converged(); - bool timedOut(builtin_interfaces::msg::Time & time) const; - - Eigen::Vector3d getLidarEstimation(); - Eigen::Vector3d getRadarEstimation(); - -protected: - 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); - - builtin_interfaces::msg::Time latest_update_time_; - KalmanFilter lidar_filter_, radar_filter_; - - double lidar_convergence_thresh_; - double radar_convergence_thresh_; - double timeout_thresh_; - double max_matching_thresh_; - bool first_observation_; - - friend class TrackFactory; -}; - -class TrackFactory -{ -public: - using Ptr = std::shared_ptr; - - 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); - - Track makeTrack( - const Eigen::Vector3d & lidar_detection, const Eigen::Vector3d & radar_detection, - builtin_interfaces::msg::Time & t0); - - KalmanFilter lidar_filter_, radar_filter_; - - double initial_lidar_cov_; - double initial_radar_cov_; - double lidar_convergence_thresh_; - double radar_convergence_thresh_; - double timeout_thresh_; - double max_matching_distance_; -}; - -} // namespace marker_radar_lidar_calibrator - -#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TRACK_HPP_ 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 new file mode 100644 index 00000000..cb208d8a --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace marker_radar_lidar_calibrator +{ + +class TransformationEstimator +{ +public: + TransformationEstimator( + Eigen::Isometry3d initial_radar_to_lidar_eigen, + Eigen::Isometry3d initial_radar_to_radar_optimization_eigen, + Eigen::Isometry3d radar_optimization_to_lidar_eigen); + void setPoints( + pcl::PointCloud::Ptr lidar_points_ocs, + pcl::PointCloud::Ptr radar_points_rcs); + void set2DRotationDelta(double delta_cos, double delta_sin); + void estimateYawOnlyTransformation(); + void estimateSVDTransformation(TransformationType transformation_type); + void estimateZeroRollTransformation(); + Eigen::Isometry3d getTransformation(); + +private: + double delta_cos_; + double delta_sin_; + 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_; + Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_; + Eigen::Isometry3d radar_optimization_to_lidar_eigen_; +}; + +} // namespace marker_radar_lidar_calibrator 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..8517fac6 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 @@ -12,27 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ -#define MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +#pragma once #include +#include + #include #include #include #include -#include +#include #include +#include +#include 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 +51,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,10 +60,72 @@ struct BackgroundModel Eigen::Vector4f min_point_; Eigen::Vector4f max_point_; std::unordered_set set_; - pcl::PointCloud::Ptr pointcloud_; + pcl::PointCloud::Ptr pointcloud_; TreeType tree_; }; -} // namespace marker_radar_lidar_calibrator +enum class TransformationType { svd_2d, yaw_only_rotation_2d, svd_3d, zero_roll_3d }; + +struct TransformationResult +{ + pcl::PointCloud::Ptr lidar_points_ocs; + pcl::PointCloud::Ptr radar_points_rcs; + std::unordered_map + calibrated_radar_to_lidar_transformations; + + void clear() + { + lidar_points_ocs.reset(); + radar_points_rcs.reset(); + calibrated_radar_to_lidar_transformations.clear(); + } +}; + +struct CalibrationErrorMetrics +{ + std::vector calibrated_distance_errors; + std::vector calibrated_yaw_errors; + std::vector avg_crossval_calibrated_distance_errors; + std::vector avg_crossval_calibrated_yaw_errors; + std::vector std_crossval_calibrated_distance_errors; + std::vector std_crossval_calibrated_yaw_errors; + + void clear() + { + calibrated_distance_errors.clear(); + calibrated_yaw_errors.clear(); + avg_crossval_calibrated_distance_errors.clear(); + avg_crossval_calibrated_yaw_errors.clear(); + std_crossval_calibrated_distance_errors.clear(); + std_crossval_calibrated_yaw_errors.clear(); + } +}; -#endif // MARKER_RADAR_LIDAR_CALIBRATOR__TYPES_HPP_ +struct OutputMetrics +{ + int num_of_converged_tracks = 0; + std::vector num_of_samples; + std::unordered_map methods; + std::vector detections; + + void clear() + { + num_of_converged_tracks = 0; + num_of_samples.clear(); + for (auto & [type, metrics] : methods) { + metrics.clear(); + } + detections.clear(); + } +}; + +struct Track +{ + int id; + Eigen::Vector3d lidar_estimation; + Eigen::Vector3d radar_estimation; + double distance_error; + double yaw_error; +}; + +} // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/utils.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/utils.hpp new file mode 100644 index 00000000..c527934d --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/utils.hpp @@ -0,0 +1,56 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include + +namespace marker_radar_lidar_calibrator +{ + +std::string toString(TransformationType type); +std::string toStringWithPrecision(const float value, const int n); +geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen); + +void updateTrackIds(std::vector & converged_tracks); + +std::pair computeCalibrationError( + std::vector::iterator & begin, std::vector::iterator & end, + const TransformationType transformation_type, const Eigen::Isometry3d & radar_to_lidar_isometry, + const bool record_error_in_track); +double getDistanceError( + TransformationType transformation_type, Eigen::Vector3d v1, Eigen::Vector3d v2); +double getYawError(Eigen::Vector3d v1, Eigen::Vector3d v2); + +size_t combination_count(const size_t n, const size_t k); +void generateAllCombinations( + const std::size_t n, const std::size_t k, std::vector> & combinations); +void selectCombinations( + const std::size_t n, const std::size_t k, const std::size_t max_number_of_combination_samples, + std::vector> & combinations); + +// Load database +void parseHeader( + std::ifstream & file, const std::string & header_name, std_msgs::msg::Header & header); +void parseConvergedTracks(std::ifstream & file, std::vector & converged_tracks); + +} // namespace marker_radar_lidar_calibrator 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 new file mode 100644 index 00000000..d9a222bf --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/visualization.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include + +#include +#include + +namespace marker_radar_lidar_calibrator +{ + +struct VisualizationParameters +{ + std_msgs::msg::Header lidar_header; + std_msgs::msg::Header radar_header; + TransformationType transformation_type; + double reflector_radius; + int marker_size_per_track; + Eigen::Isometry3d initial_radar_to_lidar_eigen; +}; + +struct DetectionMarkers +{ + visualization_msgs::msg::MarkerArray lidar_detections_marker_array; + visualization_msgs::msg::MarkerArray radar_detections_marker_array; + visualization_msgs::msg::MarkerArray matches_marker_array; +}; + +class Visualization +{ +public: + Visualization() = default; + ~Visualization() = default; + + void setParameters(VisualizationParameters params); + DetectionMarkers visualizeDetectionMarkers( + const std::vector & lidar_detections, + const std::vector & radar_detections, + const std::vector> & matched_detections); + visualization_msgs::msg::MarkerArray visualizeTrackMarkers( + const std::vector & converged_tracks, + const Eigen::Isometry3d & calibrated_radar_to_lidar_eigen); + + visualization_msgs::msg::MarkerArray deleteTrackMarkers(const size_t converged_tracks_size); + visualization_msgs::msg::Marker drawCalibrationStatusText( + const size_t converged_tracks_size, TransformationType type, CalibrationErrorMetrics metrics); + +private: + VisualizationParameters params_; + static constexpr double m_to_cm = 100.0; +}; + +} // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml b/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml index f6b11b6a..bef9419a 100644 --- a/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml +++ b/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml @@ -4,9 +4,11 @@ - + - + + + @@ -29,11 +31,13 @@ - + + + - + 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 d2c2e5be..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 @@ -14,6 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from PySide2.QtWidgets import QFileDialog +from PySide2.QtWidgets import QLabel +from PySide2.QtWidgets import QLineEdit from PySide2.QtWidgets import QMainWindow from PySide2.QtWidgets import QPushButton from PySide2.QtWidgets import QVBoxLayout @@ -31,30 +34,42 @@ def __init__(self, ros_interface): self.pending_service = False self.calibration_sent = False self.background_model_done = False + self.database_loaded = False - self.extract_background_model_status = False - self.add_lidar_radar_pair_status = False - self.delete_lidar_radar_pair_status = False - self.send_calibration_status = False + self.extract_background_model_service_status = False + self.add_lidar_radar_pair_service_status = False + self.delete_lidar_radar_pair_service_status = False + self.send_calibration_service_status = False + self.load_database_service_status = False + self.save_database_service_status = False self.ros_interface.set_extract_background_model_callback( self.extract_background_model_result_callback, - self.extract_background_model_status_callback, + self.extract_background_model_service_status_callback, ) self.ros_interface.set_add_lidar_radar_pair_callback( self.add_lidar_radar_pair_result_callback, - self.add_lidar_radar_pair_status_callback, + self.add_lidar_radar_pair_service_status_callback, ) self.ros_interface.set_delete_lidar_radar_pair_callback( self.delete_lidar_radar_pair_result_callback, - self.delete_lidar_radar_pair_status_callback, + self.delete_lidar_radar_pair_service_status_callback, ) self.ros_interface.set_send_calibration_callback( self.send_calibration_result_callback, - self.send_calibration_status_callback, + self.send_calibration_service_status_callback, + ) + + self.ros_interface.set_load_database_callback( + self.load_database_result_callback, + self.load_database_status_callback, + ) + + self.ros_interface.set_save_database_callback( + self.save_database_result_callback, self.save_database_status_callback ) self.widget = QWidget(self) @@ -75,7 +90,14 @@ def __init__(self, ros_interface): self.add_lidar_radar_pair_button.clicked.connect(self.add_lidar_radar_pair_button_callback) self.layout.addWidget(self.add_lidar_radar_pair_button) - self.delete_lidar_radar_pair_button = QPushButton("Delete previous lidar-radar pair") + self.delete_pair_label = QLabel("Enter ID to delete pair:") + self.layout.addWidget(self.delete_pair_label) + self.delete_pair_input = QLineEdit() + self.delete_pair_input.setPlaceholderText("Enter pair ID") + self.delete_pair_input.setText("-1") # Set the default value as -1 + self.layout.addWidget(self.delete_pair_input) + + self.delete_lidar_radar_pair_button = QPushButton("Delete lidar-radar pair") self.delete_lidar_radar_pair_button.setEnabled(False) self.delete_lidar_radar_pair_button.clicked.connect( self.delete_lidar_radar_pair_button_callback @@ -87,23 +109,40 @@ def __init__(self, ros_interface): self.send_calibration_button.clicked.connect(self.send_calibration_button_callback) self.layout.addWidget(self.send_calibration_button) + self.load_database_button = QPushButton("Load database") + self.load_database_button.setEnabled(False) + self.load_database_button.clicked.connect(self.load_database_button_callback) + self.layout.addWidget(self.load_database_button) + + self.save_database_button = QPushButton("Save database") + self.save_database_button.setEnabled(False) + self.save_database_button.clicked.connect(self.save_database_button_callback) + self.layout.addWidget(self.save_database_button) + self.show() def check_status(self): disable_buttons = self.calibration_sent or self.pending_service self.extract_background_model_button.setEnabled( not self.background_model_done - and self.extract_background_model_status + and self.extract_background_model_service_status and not disable_buttons ) self.add_lidar_radar_pair_button.setEnabled( - self.add_lidar_radar_pair_status and not disable_buttons + self.add_lidar_radar_pair_service_status and not disable_buttons ) self.delete_lidar_radar_pair_button.setEnabled( - self.delete_lidar_radar_pair_status and not disable_buttons + self.delete_lidar_radar_pair_service_status and not disable_buttons ) self.send_calibration_button.setEnabled( - self.send_calibration_status and not disable_buttons + self.send_calibration_service_status and not disable_buttons + ) + + self.load_database_button.setEnabled( + self.load_database_service_status and not disable_buttons and not self.database_loaded + ) + self.save_database_button.setEnabled( + self.save_database_service_status and not disable_buttons ) def extract_background_model_result_callback(self, result): @@ -111,24 +150,24 @@ def extract_background_model_result_callback(self, result): self.background_model_done = True self.check_status() - def extract_background_model_status_callback(self, status): - self.extract_background_model_status = status + def extract_background_model_service_status_callback(self, status): + self.extract_background_model_service_status = status self.check_status() def add_lidar_radar_pair_result_callback(self, result): self.pending_service = False self.check_status() - def add_lidar_radar_pair_status_callback(self, status): - self.add_lidar_radar_pair_status = status + def add_lidar_radar_pair_service_status_callback(self, status): + self.add_lidar_radar_pair_service_status = status self.check_status() def delete_lidar_radar_pair_result_callback(self, result): self.pending_service = False self.check_status() - def delete_lidar_radar_pair_status_callback(self, status): - self.delete_lidar_radar_pair_status = status + def delete_lidar_radar_pair_service_status_callback(self, status): + self.delete_lidar_radar_pair_service_status = status self.check_status() def send_calibration_result_callback(self, result): @@ -136,8 +175,25 @@ def send_calibration_result_callback(self, result): self.calibration_sent = True self.check_status() - def send_calibration_status_callback(self, status): - self.send_calibration_status = status + def send_calibration_service_status_callback(self, status): + self.send_calibration_service_status = status + self.check_status() + + def load_database_result_callback(self, result): + self.pending_service = False + self.database_loaded = result.success + self.check_status() + + def load_database_status_callback(self, status): + self.load_database_service_status = status + self.check_status() + + def save_database_result_callback(self, result): + self.pending_service = False + self.check_status() + + def save_database_status_callback(self, status): + self.save_database_service_status = status self.check_status() def extract_background_model_button_callback(self): @@ -151,11 +207,40 @@ def add_lidar_radar_pair_button_callback(self): self.check_status() def delete_lidar_radar_pair_button_callback(self): + pair_id_text = self.delete_pair_input.text() + try: + pair_id = int(pair_id_text) # Convert input to integer + except ValueError: + print("Please enter a valid numeric pair ID.") + return + self.pending_service = True - self.ros_interface.delete_lidar_radar_pair() + self.ros_interface.delete_lidar_radar_pair(pair_id) self.check_status() def send_calibration_button_callback(self): self.pending_service = True self.ros_interface.send_calibration() self.check_status() + + def load_database_button_callback(self): + filename, _ = QFileDialog.getOpenFileName(None, "Open File", ".", "Text Files (*.txt)") + + if len(filename) == 0: + return + + self.pending_service = True + self.ros_interface.load_database(filename) + self.check_status() + + def save_database_button_callback(self): + filename, _ = QFileDialog.getSaveFileName(None, "Save File", ".", "Text Files (*.txt)") + if len(filename) == 0: + return + + if not filename.endswith(".txt"): + filename += ".txt" + + self.pending_service = True + self.ros_interface.save_database(filename) + self.check_status() diff --git a/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py b/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py index a0407c45..7089937a 100644 --- a/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py +++ b/calibrators/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator/ros_interface.py @@ -19,6 +19,8 @@ from rclpy.executors import SingleThreadedExecutor from rclpy.node import Node from std_srvs.srv import Empty +from tier4_calibration_msgs.srv import DeleteLidarRadarPair +from tier4_calibration_msgs.srv import FileSrv class ServiceWrapper: @@ -57,6 +59,28 @@ def __call__(self): self.future = self.client.call_async(req) +class DeleteLidarRadarPairServiceWrapper(ServiceWrapper): + def __init__(self, node, name): + super().__init__() + self.client = node.create_client(DeleteLidarRadarPair, name) + + def __call__(self, pair_id): + req = DeleteLidarRadarPair.Request() + req.pair_id = pair_id # Set the pair_id in the request + self.future = self.client.call_async(req) + + +class FileServiceWrapper(ServiceWrapper): + def __init__(self, node, name): + super().__init__() + self.client = node.create_client(FileSrv, name) + + def __call__(self, file): + req = FileSrv.Request() + req.file = file + self.future = self.client.call_async(req) + + class RosInterface(Node): def __init__(self): super().__init__("marker_radar_lidar_calibrator_ui") @@ -67,14 +91,20 @@ def __init__(self): self.extract_background_model_client = EmptyServiceWrapper(self, "extract_background_model") self.add_lidar_radar_pair_client = EmptyServiceWrapper(self, "add_lidar_radar_pair") - self.delete_lidar_radar_pair_client = EmptyServiceWrapper(self, "delete_lidar_radar_pair") + self.delete_lidar_radar_pair_client = DeleteLidarRadarPairServiceWrapper( + self, "delete_lidar_radar_pair" + ) self.send_calibration_client = EmptyServiceWrapper(self, "send_calibration") + self.load_database_client = FileServiceWrapper(self, "load_database") + self.save_database_client = FileServiceWrapper(self, "save_database") self.client_list = [ self.extract_background_model_client, self.add_lidar_radar_pair_client, self.delete_lidar_radar_pair_client, self.send_calibration_client, + self.load_database_client, + self.save_database_client, ] self.timer = self.create_timer(0.1, self.timer_callback) @@ -99,18 +129,35 @@ def set_send_calibration_callback(self, result_callback, status_callback): self.send_calibration_client.set_result_callback(result_callback) self.send_calibration_client.set_status_callback(status_callback) + def set_load_database_callback(self, result_callback, status_callback): + with self.lock: + self.load_database_client.set_result_callback(result_callback) + self.load_database_client.set_status_callback(status_callback) + + def set_save_database_callback(self, result_callback, status_callback): + with self.lock: + self.save_database_client.set_result_callback(result_callback) + self.save_database_client.set_status_callback(status_callback) + def extract_background_model(self): self.extract_background_model_client() def add_lidar_radar_pair(self): self.add_lidar_radar_pair_client() - def delete_lidar_radar_pair(self): - self.delete_lidar_radar_pair_client() + def delete_lidar_radar_pair(self, pair_id): + print(f"Requesting deletion of lidar-radar pair with ID: {pair_id}") + self.delete_lidar_radar_pair_client(pair_id) def send_calibration(self): self.send_calibration_client() + def load_database(self, file): + self.load_database_client(file) + + def save_database(self, file): + self.save_database_client(file) + def timer_callback(self): with self.lock: for client in self.client_list: diff --git a/calibrators/marker_radar_lidar_calibrator/package.xml b/calibrators/marker_radar_lidar_calibrator/package.xml index 19f0bee2..c607dead 100644 --- a/calibrators/marker_radar_lidar_calibrator/package.xml +++ b/calibrators/marker_radar_lidar_calibrator/package.xml @@ -5,6 +5,7 @@ 0.0.1 The marker_radar_lidar_calibrator package Kenzo Lobos Tsunekawa + Yi-Hsiang Fang BSD @@ -13,10 +14,10 @@ autoware_cmake - autoware_kalman_filter autoware_universe_utils eigen geometry_msgs + libceres-dev pcl_conversions pcl_ros radar_msgs diff --git a/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz b/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz index 90edc3bf..ebaf1dd5 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: 1805 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -60,7 +60,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.019999999552965164 + Size (m): 0.009999999776482582 Style: Flat Squares Topic: Depth: 5 @@ -162,7 +162,7 @@ Visualization Manager: Position Transformer: XYZ Selectable: true Size (Pixels): 3 - Size (m): 0.07000000029802322 + Size (m): 0.009999999776482582 Style: Flat Squares Topic: Depth: 5 @@ -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,14 +256,15 @@ 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: - {} + calibration_status: true 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: -14.38258171081543 + Y: -0.09864789992570877 + Z: 7.7625932693481445 Target Frame: Value: FPS (rviz_default_plugins) Yaw: 0.0031585693359375 @@ -354,10 +353,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1403 + Height: 2096 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000021600000796fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000796000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b080000003efc0100000002fb0000000800540069006d0065010000000000000b080000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000008ec0000079600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -366,6 +365,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2486 - X: 1994 - Y: 0 + Width: 2824 + X: 70 + Y: 27 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 eeeb92fb..11d5bba6 100755 --- a/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py +++ b/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py @@ -17,211 +17,333 @@ import math import matplotlib.pyplot as plt +import matplotlib.ticker as mticker # cspell:ignore mticker import numpy as np import rclpy from rclpy.node import Node -from std_msgs.msg import Float32MultiArray +from tier4_calibration_msgs.msg import CalibrationMetrics class MetricsPlotter: def __init__(self): - self.fig, self.axes = plt.subplots(nrows=2, ncols=2, figsize=(8, 6)) - self.subplot0 = self.axes[0, 0] - self.subplot1 = self.axes[0, 1] - self.subplot2 = self.axes[1, 0] - self.subplot3 = self.axes[1, 1] - plt.gcf().canvas.set_window_title("Metrics plotter") - - self.color_distance_o = "C0o-" - self.color_yaw_o = "C1o-" + self.fig = None + self.subplots = {} + self.metrics_data = {} + self.m_to_cm = 100 + + # 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" self.color_yaw = "C1" - ( - self.num_of_reflectors_list, - self.calibration_distance_error_list, - self.calibration_yaw_error_list, - self.crossval_sample_list, - self.crossval_distance_error_list, - self.crossval_yaw_error_list, - self.std_crossval_distance_error_list, - self.std_crossval_yaw_error_list, - ) = ([], [], [], [], [], [], [], []) - - self.m_to_cm = 100 + def initialize_figure(self, methods): + num_rows = len(methods) + 1 # 1 row for distributions + num_cols = 4 + + # Create the figure with the fixed layout + self.fig, self.axes = plt.subplots(nrows=num_rows, ncols=num_cols, figsize=(16, 12)) + self.fig.canvas.manager.set_window_title("Metrics and Detection Distributions") + + # Flatten axes for easier indexing and assignment + self.axes = self.axes.reshape(num_rows, num_cols) + + # Assign subplots for methods + self.subplots = {method: {} for method in methods} + for idx, method in enumerate(methods): + self.subplots[method] = { + "crossval_distance": self.axes[idx, 0], + "crossval_yaw": self.axes[idx, 1], + "average_distance": self.axes[idx, 2], + "average_yaw": self.axes[idx, 3], + } + + # Assign subplots for detection distributions + self.detection_subplots = { + "range": self.axes[len(methods), 0], + "pitch": self.axes[len(methods), 1], + "yaw": self.axes[len(methods), 2], + } + + # Leave the last column empty for a clean layout + for ax in self.axes[2, 3:]: + ax.axis("off") - self.plot_label_and_set_xy_lim() plt.tight_layout() plt.pause(0.1) - def plot_label_and_set_xy_lim(self): - self.subplot0.set_title("cross-validation error: distance") - self.subplot0.set_xlabel("number of tracks") - self.subplot0.set_ylabel("distance error [cm]") - - self.subplot1.set_title("cross-validation error: yaw") - self.subplot1.set_xlabel("number of tracks") - self.subplot1.set_ylabel("yaw error [deg]") + def initialize_metrics(self, methods): + for method in methods: + if method not in self.metrics_data: + self.metrics_data[method] = { + "num_of_reflectors_list": [], + "calibrated_distance_error_list": [], + "calibrated_yaw_error_list": [], + "crossval_sample_list": [], + "crossval_distance_error_list": [], + "crossval_yaw_error_list": [], + "std_crossval_distance_error_list": [], + "std_crossval_yaw_error_list": [], + } - self.subplot2.set_title("average error: distance") - self.subplot2.set_xlabel("number of tracks") - self.subplot2.set_ylabel("distance error [cm]") - - self.subplot3.set_title("average error: yaw") - self.subplot3.set_xlabel("number of tracks") - self.subplot3.set_ylabel("yaw error [deg]") + def plot_label_and_set_xy_lim(self): + if not hasattr(self, "axes"): + return # Skip if the axes are not initialized - max_ylim0 = ( - max(self.crossval_distance_error_list) if self.crossval_distance_error_list else 5 - ) - max_ylim1 = max(self.crossval_yaw_error_list) if self.crossval_yaw_error_list else 1 - max_ylim2 = ( - max(self.calibration_distance_error_list) if self.calibration_distance_error_list else 5 - ) - max_ylim3 = max(self.calibration_yaw_error_list) if self.calibration_yaw_error_list else 1 + for method, subplots in self.subplots.items(): + subplots["crossval_distance"].set_title(f"{method}\nCross-validation error: distance") + subplots["crossval_distance"].set_xlabel("Number of tracks") + subplots["crossval_distance"].set_ylabel("Distance error [cm]") - cross_val_xlim = ( - self.crossval_sample_list[-1] - if self.crossval_sample_list and self.crossval_sample_list[-1] >= 5 - else 5 - ) - avg_xlim = ( - self.num_of_reflectors_list[-1] - if self.num_of_reflectors_list and self.num_of_reflectors_list[-1] >= 5 - else 5 - ) + subplots["crossval_yaw"].set_title(f"{method}\nCross-validation error: yaw") + subplots["crossval_yaw"].set_xlabel("Number of tracks") + subplots["crossval_yaw"].set_ylabel("Yaw error [deg]") - self.subplot0.set_xlim(2.9, cross_val_xlim + 0.3) - self.subplot1.set_xlim(2.9, cross_val_xlim + 0.3) - self.subplot2.set_xlim(2.9, avg_xlim + 0.3) - self.subplot3.set_xlim(2.9, avg_xlim + 0.3) + subplots["average_distance"].set_title(f"{method}\nAverage error: distance") + subplots["average_distance"].set_xlabel("Number of tracks") + subplots["average_distance"].set_ylabel("Distance error [cm]") - self.subplot0.set_ylim(0, max_ylim0 + 5) - self.subplot1.set_ylim(0, max_ylim1 + 0.1) - self.subplot2.set_ylim(0, max_ylim2 + 5) - self.subplot3.set_ylim(0, max_ylim3 + 0.1) + subplots["average_yaw"].set_title(f"{method}\nAverage error: yaw") + subplots["average_yaw"].set_xlabel("Number of tracks") + subplots["average_yaw"].set_ylabel("Yaw error [deg]") for ax in self.axes.flat: ax.xaxis.set_major_locator(plt.MaxNLocator(integer=True)) - def is_delete_operation(self, msg_array): - return self.num_of_reflectors_list and msg_array[0] < self.num_of_reflectors_list[-1] - - def remove_avg_error_from_list(self): - for i in range(min(2, len(self.num_of_reflectors_list))): - self.calibration_distance_error_list.pop() - self.calibration_yaw_error_list.pop() - self.num_of_reflectors_list.pop() - - def add_avg_error_to_list(self, msg_array): - num_of_reflectors = msg_array[0] - calibration_distance_error = msg_array[1] * self.m_to_cm - calibration_yaw_error = 0 if math.isnan(msg_array[2]) else msg_array[2] - - if num_of_reflectors >= 3: - self.num_of_reflectors_list.append(num_of_reflectors) - self.calibration_distance_error_list.append(calibration_distance_error) - self.calibration_yaw_error_list.append(calibration_yaw_error) - - def add_crossval_error_to_list(self, msg_array): - ( - self.crossval_sample_list, - self.crossval_distance_error_list, - self.crossval_yaw_error_list, - self.std_crossval_distance_error_list, - self.std_crossval_yaw_error_list, - ) = ([], [], [], [], []) - - for i in range((len(msg_array) - 3) // 5): - self.crossval_sample_list.append(msg_array[3 + i * 5]) - self.crossval_distance_error_list.append(msg_array[3 + i * 5 + 1] * self.m_to_cm) - self.crossval_yaw_error_list.append(msg_array[3 + i * 5 + 2]) - self.std_crossval_distance_error_list.append(msg_array[3 + i * 5 + 3] * self.m_to_cm) - self.std_crossval_yaw_error_list.append(msg_array[3 + i * 5 + 4]) - - def draw_avg_subplots(self): - self.subplot2.clear() - self.subplot3.clear() - - self.subplot2.plot( - self.num_of_reflectors_list, - self.calibration_distance_error_list, - self.color_distance_o, + def update_metrics(self, msg): + # Extract methods from the incoming message + methods_in_msg = [method.method_name for method in msg.method_metrics] + + # Initialize figure and metrics for new methods + if not self.metrics_data or set(self.metrics_data.keys()) != set(methods_in_msg): + self.initialize_figure(methods_in_msg) + self.initialize_metrics(methods_in_msg) + + # Update metrics for each method in the message + for method in msg.method_metrics: + method_name = method.method_name + metrics = self.metrics_data[method_name] + metrics["num_of_reflectors_list"] = list(range(1, msg.num_of_converged_tracks + 1)) + metrics["calibrated_distance_error_list"] = [ + value * self.m_to_cm for value in method.calibrated_distance_errors + ] + metrics["calibrated_yaw_error_list"] = method.calibrated_yaw_errors + + metrics["crossval_sample_list"] = msg.num_of_samples + metrics["crossval_distance_error_list"] = [ + value * self.m_to_cm for value in method.avg_crossval_calibrated_distance_errors + ] + metrics["crossval_yaw_error_list"] = method.avg_crossval_calibrated_yaw_errors + metrics["std_crossval_distance_error_list"] = [ + value * self.m_to_cm for value in method.std_crossval_calibrated_distance_errors + ] + metrics["std_crossval_yaw_error_list"] = method.std_crossval_calibrated_yaw_errors + + def compute_detection_metrics(self, detections): + epsilon = 1e-6 + ranges = [] + pitches = [] + yaws = [] + + for detection in detections: + range_ = math.sqrt(detection.x**2 + detection.y**2 + detection.z**2) + range_ = max(range_, epsilon) + ranges.append(range_) + + pitch = math.degrees(math.asin(max(-1.0, min(1.0, detection.z / range_)))) + pitches.append(pitch) + + yaw = math.degrees(math.atan2(detection.y, detection.x)) + yaws.append(yaw) + + return ranges, pitches, yaws + + def plot_detection_distributions(self, ranges, pitches, yaws): + # Clear previous plots + for subplot in self.detection_subplots.values(): + subplot.clear() + + if not ranges or not pitches or not yaws: + return + + # 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 10 degrees + + # Create discrete bins + range_bins = np.arange( + math.floor(min(ranges) / range_bin_width) * range_bin_width, + math.ceil(max(ranges) / range_bin_width) * range_bin_width + range_bin_width, + range_bin_width, ) - self.subplot3.plot( - self.num_of_reflectors_list, - self.calibration_yaw_error_list, - self.color_yaw_o, + pitch_bins = np.arange( + math.floor(min(pitches) / pitch_bin_width) * pitch_bin_width, + math.ceil(max(pitches) / pitch_bin_width) * pitch_bin_width + pitch_bin_width, + pitch_bin_width, ) - - if len(self.num_of_reflectors_list) > 0: - # draw annotations for the last point - self.subplot2.annotate( - f"{self.calibration_distance_error_list[-1]:.2f}", # noqa E231 - xy=(self.num_of_reflectors_list[-1], self.calibration_distance_error_list[-1]), - color=self.color_distance, - ) - self.subplot3.annotate( - f"{self.calibration_yaw_error_list[-1]:.2f}", # noqa E231 - xy=(self.num_of_reflectors_list[-1], self.calibration_yaw_error_list[-1]), - color=self.color_yaw, - ) - - def draw_crossval_subplots(self): - self.subplot0.clear() - self.subplot1.clear() - - self.subplot0.plot( - self.crossval_sample_list, - self.crossval_distance_error_list, - self.color_distance_o, - ) - self.subplot1.plot( - self.crossval_sample_list, - self.crossval_yaw_error_list, - self.color_yaw_o, + yaw_bins = np.arange( + math.floor(min(yaws) / yaw_bin_width) * yaw_bin_width, + math.ceil(max(yaws) / yaw_bin_width) * yaw_bin_width + yaw_bin_width, + yaw_bin_width, ) - # draw std and mean of error - self.subplot0.fill_between( - self.crossval_sample_list, - np.array(self.crossval_distance_error_list) - - np.array(self.std_crossval_distance_error_list), - np.array(self.crossval_distance_error_list) - + np.array(self.std_crossval_distance_error_list), - color=self.color_distance, - alpha=0.3, + # Count occurrences in each bin and cast counts to integers + range_counts = np.histogram(ranges, bins=range_bins)[0].astype(int) + pitch_counts = np.histogram(pitches, bins=pitch_bins)[0].astype(int) + yaw_counts = np.histogram(yaws, bins=yaw_bins)[0].astype(int) + + # Plot range distribution as a bar chart + self.detection_subplots["range"].bar( + range_bins[:-1], range_counts, color="C2", alpha=0.7, width=range_bin_width * 0.5 ) - self.subplot1.fill_between( - self.crossval_sample_list, - np.array(self.crossval_yaw_error_list) - np.array(self.std_crossval_yaw_error_list), - np.array(self.crossval_yaw_error_list) + np.array(self.std_crossval_yaw_error_list), - color=self.color_yaw, - alpha=0.3, + self.detection_subplots["range"].set_title("Range Distribution") + self.detection_subplots["range"].set_xlabel("Range [m]") + self.detection_subplots["range"].set_ylabel("Count") + self.detection_subplots["range"].set_xticks(range_bins) + self.detection_subplots["range"].yaxis.set_major_locator(mticker.MaxNLocator(integer=True)) + + # Plot pitch distribution as a bar chart + self.detection_subplots["pitch"].bar( + pitch_bins[:-1], pitch_counts, color="C3", alpha=0.7, width=pitch_bin_width * 0.5 ) + self.detection_subplots["pitch"].set_title("Pitch Distribution") + self.detection_subplots["pitch"].set_xlabel("Pitch [deg]") + self.detection_subplots["pitch"].set_ylabel("Count") + self.detection_subplots["pitch"].set_xticks(pitch_bins) + self.detection_subplots["pitch"].yaxis.set_major_locator(mticker.MaxNLocator(integer=True)) + + # Plot yaw distribution as a bar chart + self.detection_subplots["yaw"].bar( + yaw_bins[:-1], yaw_counts, color="C4", alpha=0.7, width=yaw_bin_width * 0.5 + ) + self.detection_subplots["yaw"].set_title("Yaw Distribution") + self.detection_subplots["yaw"].set_xlabel("Yaw [deg]") + self.detection_subplots["yaw"].set_ylabel("Count") + self.detection_subplots["yaw"].set_xticks(yaw_bins) + self.detection_subplots["yaw"].yaxis.set_major_locator(mticker.MaxNLocator(integer=True)) + + plt.tight_layout() + plt.pause(0.1) - # annotate the last value - if len(self.crossval_sample_list) > 0: - self.subplot0.annotate( - f"{self.crossval_distance_error_list[-1]:.2f}", # noqa E231 - xy=(self.crossval_sample_list[-1], self.crossval_distance_error_list[-1]), - color=self.color_distance, - ) - self.subplot1.annotate( - f"{self.crossval_yaw_error_list[-1]:.2f}", # noqa E231 - xy=(self.crossval_sample_list[-1], self.crossval_yaw_error_list[-1]), - color=self.color_yaw, - ) + def draw_subplots(self): + for method, subplots in self.subplots.items(): + metrics = self.metrics_data[method] + + # Clear previous plots + subplots["crossval_distance"].clear() + subplots["crossval_yaw"].clear() + subplots["average_distance"].clear() + subplots["average_yaw"].clear() + + # Filter data where the number of reflectors >= 3 + filtered_indices = [ + i for i, num in enumerate(metrics["num_of_reflectors_list"]) if num >= 3 + ] + + filtered_reflectors = [metrics["num_of_reflectors_list"][i] for i in filtered_indices] + filtered_distance_errors = [ + metrics["calibrated_distance_error_list"][i] for i in filtered_indices + ] + filtered_yaw_errors = [ + metrics["calibrated_yaw_error_list"][i] for i in filtered_indices + ] + + if filtered_reflectors and filtered_distance_errors: + subplots["average_distance"].plot( + filtered_reflectors, + filtered_distance_errors, + self.color_distance_o, + ) + subplots["average_distance"].annotate( + f"{filtered_distance_errors[-1]:.2f}", + xy=(filtered_reflectors[-1], filtered_distance_errors[-1]), + color=self.color_distance, + ) + + if filtered_reflectors and filtered_yaw_errors: + subplots["average_yaw"].plot( + filtered_reflectors, + filtered_yaw_errors, + self.color_yaw_o, + ) + subplots["average_yaw"].annotate( + f"{filtered_yaw_errors[-1]:.2f}", + xy=(filtered_reflectors[-1], filtered_yaw_errors[-1]), + color=self.color_yaw, + ) + + if ( + metrics["crossval_sample_list"] + and metrics["crossval_distance_error_list"] + and metrics["std_crossval_distance_error_list"] + ): + subplots["crossval_distance"].plot( + metrics["crossval_sample_list"], + metrics["crossval_distance_error_list"], + self.color_distance_o, + ) + subplots["crossval_distance"].annotate( + f"{metrics['crossval_distance_error_list'][-1]:.2f}", + xy=( + metrics["crossval_sample_list"][-1], + metrics["crossval_distance_error_list"][-1], + ), + color=self.color_distance, + ) + subplots["crossval_distance"].fill_between( + metrics["crossval_sample_list"], + np.array(metrics["crossval_distance_error_list"]) + - np.array(metrics["std_crossval_distance_error_list"]), + np.array(metrics["crossval_distance_error_list"]) + + np.array(metrics["std_crossval_distance_error_list"]), + color=self.color_distance, + alpha=0.3, + ) + + if ( + metrics["crossval_sample_list"] + and metrics["crossval_yaw_error_list"] + and metrics["std_crossval_yaw_error_list"] + ): + subplots["crossval_yaw"].plot( + metrics["crossval_sample_list"], + metrics["crossval_yaw_error_list"], + self.color_yaw_o, + ) + subplots["crossval_yaw"].annotate( + f"{metrics['crossval_yaw_error_list'][-1]:.2f}", + xy=( + metrics["crossval_sample_list"][-1], + metrics["crossval_yaw_error_list"][-1], + ), + color=self.color_yaw, + ) + subplots["crossval_yaw"].fill_between( + metrics["crossval_sample_list"], + np.array(metrics["crossval_yaw_error_list"]) + - np.array(metrics["std_crossval_yaw_error_list"]), + np.array(metrics["crossval_yaw_error_list"]) + + np.array(metrics["std_crossval_yaw_error_list"]), + color=self.color_yaw, + alpha=0.3, + ) def draw_with_msg(self, msg): - msg_array = msg.data - if self.is_delete_operation(msg_array): - self.remove_avg_error_from_list() - self.add_avg_error_to_list(msg_array) - self.add_crossval_error_to_list(msg_array) - self.draw_avg_subplots() - self.draw_crossval_subplots() + methods_in_msg = [method.method_name for method in msg.method_metrics] + + # Initialize missing methods dynamically + if not self.metrics_data or set(self.metrics_data.keys()) != set(methods_in_msg): + self.initialize_figure(methods_in_msg) + self.initialize_metrics(methods_in_msg) + + self.update_metrics(msg) + self.draw_subplots() self.plot_label_and_set_xy_lim() + + # Handle detections + ranges, pitches, yaws = self.compute_detection_metrics(msg.detections) + self.plot_detection_distributions(ranges, pitches, yaws) plt.tight_layout() plt.pause(0.1) @@ -229,10 +351,10 @@ def draw_with_msg(self, msg): class MetricsPlotterNode(Node): def __init__(self): super().__init__("plot_metric") - self.metrics_plotter = MetricsPlotter() self.subscription = self.create_subscription( - Float32MultiArray, "calibration_metrics", self.listener_callback, 10 + CalibrationMetrics, "calibration_metrics", self.listener_callback, 10 ) + self.metrics_plotter = MetricsPlotter() def listener_callback(self, msg): self.metrics_plotter.draw_with_msg(msg) 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 7a17df34..0b1e3fe9 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 @@ -12,8 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "marker_radar_lidar_calibrator/types.hpp" + #include #include +#include #include #include @@ -22,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -32,13 +34,14 @@ #include #include +#include #include #include #include -#include #include #include #include +#include #include #include @@ -64,7 +67,6 @@ void update_param( namespace marker_radar_lidar_calibrator { - rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback( const std::vector & parameters) { @@ -75,7 +77,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para Parameters p = parameters_; try { - UPDATE_PARAM(p, radar_parallel_frame); + UPDATE_PARAM(p, radar_optimization_frame); UPDATE_PARAM(p, use_lidar_initial_crop_box_filter); UPDATE_PARAM(p, lidar_initial_crop_box_min_x); UPDATE_PARAM(p, lidar_initial_crop_box_min_y); @@ -110,6 +112,9 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para UPDATE_PARAM(p, max_matching_distance); 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, min_frames_for_convergence); + UPDATE_PARAM(p, reflector_points_threshold); // transaction succeeds, now assign values parameters_ = p; @@ -127,7 +132,8 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( { tf_buffer_ = std::make_shared(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); - parameters_.radar_parallel_frame = this->declare_parameter("radar_parallel_frame"); + parameters_.radar_optimization_frame = + this->declare_parameter("radar_optimization_frame"); parameters_.use_lidar_initial_crop_box_filter = this->declare_parameter("use_lidar_initial_crop_box_filter", true); @@ -195,34 +201,51 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( this->declare_parameter("radar_cluster_min_points", 1); parameters_.radar_cluster_max_points = this->declare_parameter("radar_cluster_max_points", 10); - parameters_.reflector_radius = this->declare_parameter("reflector_radius", 0.1); + parameters_.reflector_radius = this->declare_parameter("reflector_radius", 0.095); 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); - - double initial_lidar_cov = this->declare_parameter("initial_lidar_cov", 0.5); - double initial_radar_cov = this->declare_parameter("initial_radar_cov", 2.0); - double lidar_measurement_cov = this->declare_parameter("lidar_measurement_cov", 0.03); - double radar_measurement_cov = this->declare_parameter("radar_measurement_cov", 0.05); - double lidar_process_cov = this->declare_parameter("lidar_process_cov", 0.01); - double radar_process_cov = this->declare_parameter("radar_process_cov", 0.01); - double lidar_convergence_thresh = - this->declare_parameter("lidar_convergence_thresh", 0.03); - double radar_convergence_thresh = - this->declare_parameter("radar_convergence_thresh", 0.03); - double timeout_thresh = this->declare_parameter("timeout_thresh", 3.0); + parameters_.max_number_of_combination_samples = static_cast( + this->declare_parameter("max_number_of_combination_samples", 500)); + + if (parameters_.max_number_of_combination_samples >= 10000) { + throw std::runtime_error( + "// Please do not set this parameter larger than 10000 for efficiency!"); + } + + parameters_.min_frames_for_convergence = + this->declare_parameter("min_frames_for_convergence", 10); + parameters_.reflector_points_threshold = + this->declare_parameter("reflector_points_threshold", 10); + + auto msg_type = this->declare_parameter("msg_type"); + auto transformation_type = this->declare_parameter("transformation_type"); + if (msg_type == "radar_tracks") { + msg_type_ = MsgType::radar_tracks; + } else if (msg_type == "radar_scan") { + msg_type_ = MsgType::radar_scan; + } else if (msg_type == "radar_cloud") { + msg_type_ = MsgType::radar_cloud; + } else { + throw std::runtime_error("Invalid msg_type value: " + msg_type); + } + + if (transformation_type == "svd_2d") { + transformation_type_ = TransformationType::svd_2d; + } else if (transformation_type == "yaw_only_rotation_2d") { + transformation_type_ = TransformationType::yaw_only_rotation_2d; + } else if (transformation_type == "svd_3d") { + transformation_type_ = TransformationType::svd_3d; + } else if (transformation_type == "zero_roll_3d") { + transformation_type_ = TransformationType::zero_roll_3d; + } else { + throw std::runtime_error("Invalid transformation_type value: " + transformation_type); + } parameters_.max_initial_calibration_translation_error = this->declare_parameter("max_initial_calibration_translation_error", 1.0); parameters_.max_initial_calibration_rotation_error = this->declare_parameter("max_initial_calibration_rotation_error", 45.0); - factory_ptr_ = std::make_shared( - initial_lidar_cov, initial_radar_cov, lidar_measurement_cov, radar_measurement_cov, - lidar_process_cov, radar_process_cov, lidar_convergence_thresh, radar_convergence_thresh, - timeout_thresh, parameters_.max_matching_distance); - lidar_background_pub_ = this->create_publisher("lidar_background_pointcloud", 10); lidar_foreground_pub_ = @@ -243,16 +266,29 @@ 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); lidar_sub_ = this->create_subscription( "input_lidar_pointcloud", rclcpp::SensorDataQoS(), std::bind(&ExtrinsicReflectorBasedCalibrator::lidarCallback, this, std::placeholders::_1)); - radar_sub_ = this->create_subscription( - "input_radar_objects", rclcpp::SensorDataQoS(), - std::bind(&ExtrinsicReflectorBasedCalibrator::radarCallback, this, std::placeholders::_1)); + if (msg_type_ == MsgType::radar_tracks) { + radar_tracks_sub_ = this->create_subscription( + "input_radar_msg", rclcpp::SensorDataQoS(), + std::bind( + &ExtrinsicReflectorBasedCalibrator::radarTracksCallback, this, std::placeholders::_1)); + } else if (msg_type_ == MsgType::radar_scan) { + radar_scan_sub_ = this->create_subscription( + "input_radar_msg", rclcpp::SensorDataQoS(), + std::bind( + &ExtrinsicReflectorBasedCalibrator::radarScanCallback, this, std::placeholders::_1)); + } else if (msg_type_ == MsgType::radar_cloud) { + radar_cloud_sub_ = this->create_subscription( + "input_radar_msg", rclcpp::SensorDataQoS(), + std::bind( + &ExtrinsicReflectorBasedCalibrator::radarCloudCallback, this, std::placeholders::_1)); + } // The service server runs in a dedicated thread calibration_api_srv_callback_group_ = @@ -278,6 +314,13 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator( std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, calibration_ui_srv_callback_group_); + load_database_service_server_ = this->create_service( + "load_database", + std::bind( + &ExtrinsicReflectorBasedCalibrator::loadDatabaseCallback, this, std::placeholders::_1, + std::placeholders::_2), + rmw_qos_profile_services_default, calibration_ui_srv_callback_group_); + timer_ = rclcpp::create_timer( this, get_clock(), std::chrono::seconds(1), std::bind(&ExtrinsicReflectorBasedCalibrator::timerCallback, this)); @@ -341,10 +384,20 @@ void ExtrinsicReflectorBasedCalibrator::timerCallback() } if (converged_tracks_.size() > 0 && !delete_track_service_server_) { - delete_track_service_server_ = this->create_service( - "delete_lidar_radar_pair", + delete_track_service_server_ = + this->create_service( + "delete_lidar_radar_pair", + std::bind( + &ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback, this, + std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, calibration_ui_srv_callback_group_); + } + + if (converged_tracks_.size() > 0 && !save_database_service_server_) { + save_database_service_server_ = this->create_service( + "save_database", std::bind( - &ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback, this, std::placeholders::_1, + &ExtrinsicReflectorBasedCalibrator::saveDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, calibration_ui_srv_callback_group_); } @@ -371,7 +424,7 @@ void ExtrinsicReflectorBasedCalibrator::backgroundModelRequestCallback( } RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "Waiting for the calibration to end"); + this->get_logger(), *this->get_clock(), 5000, "Waiting to extract the background model"); } RCLCPP_INFO(this->get_logger(), "Background model estimated"); @@ -393,38 +446,60 @@ void ExtrinsicReflectorBasedCalibrator::trackingRequestCallback( rclcpp::sleep_for(1s); std::unique_lock lock(mutex_); - if (!tracking_active_ && current_new_tracks_) { + if (!tracking_active_) { break; } - - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, - "Waiting for detections to converge. Active tracks=%lu. New tracks=%d", active_tracks_.size(), - current_new_tracks_); } RCLCPP_INFO(this->get_logger(), "New converged detections: %d", current_new_tracks_); } void ExtrinsicReflectorBasedCalibrator::deleteTrackRequestCallback( - [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr response) + const std::shared_ptr request, + const std::shared_ptr response) { using std::chrono_literals::operator""s; - if (converged_tracks_.size() > 0) { - converged_tracks_.pop_back(); + int track_id_to_delete = (request->pair_id < 0) + ? static_cast(converged_tracks_.size()) + request->pair_id + 1 + : request->pair_id; + + // Search for the track with the specified ID + auto it = std::find_if( + converged_tracks_.begin(), converged_tracks_.end(), + [&track_id_to_delete](const Track & track) { return track.id == track_id_to_delete; }); + + if (it != converged_tracks_.end()) { + // Track found; delete it + auto delete_markers = visualization_.deleteTrackMarkers(converged_tracks_.size()); + tracking_markers_pub_->publish(delete_markers); + converged_tracks_.erase(it); + updateTrackIds(converged_tracks_); calibrateSensors(); - visualizeTrackMarkers(); - deleteTrackMarkers(); - drawCalibrationStatusText(); + auto tracking_markers = + visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_); + tracking_markers_pub_->publish(tracking_markers); + auto text_markers = visualization_.drawCalibrationStatusText( + converged_tracks_.size(), transformation_type_, + output_metrics_.methods[transformation_type_]); + text_markers_pub_->publish(text_markers); + + response->success = true; + response->message = "Track successfully deleted."; RCLCPP_INFO( - this->get_logger(), "The last track was successfully deleted. Remaining converged tracks: %d", - static_cast(converged_tracks_.size())); - // sleep for 1s for the plotter node to finish plotting. + this->get_logger(), + "Track with ID '%d' was successfully deleted. Remaining converged tracks: %d", + track_id_to_delete, static_cast(converged_tracks_.size())); + + // Sleep for 1 second for the plotter node to finish plotting rclcpp::sleep_for(1s); } else { - RCLCPP_WARN(this->get_logger(), "There are no converged tracks available"); + // Track not found + response->success = false; + response->message = "Track ID not found."; + RCLCPP_WARN( + this->get_logger(), "Track with ID '%d' not found. No tracks were deleted.", + track_id_to_delete); } } @@ -435,49 +510,193 @@ void ExtrinsicReflectorBasedCalibrator::sendCalibrationCallback( std::unique_lock lock(mutex_); send_calibration_ = true; } +void ExtrinsicReflectorBasedCalibrator::loadDatabaseCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + const std::string & file_path = request->file; + + std::ifstream file(file_path); + if (!file.is_open()) { + logErrorAndRespond(response, "Failed to open the file: " + file_path); + return; + } + + try { + parseConvergedTracks(file, converged_tracks_); + parseHeader(file, "lidar_header:", lidar_header_); + parseHeader(file, "radar_header:", radar_header_); + lidar_frame_ = lidar_header_.frame_id; + radar_frame_ = radar_header_.frame_id; + } catch (const std::exception & e) { + logErrorAndRespond(response, e.what()); + return; + } + + file.close(); + response->success = true; + RCLCPP_INFO(this->get_logger(), "Database successfully loaded from: %s", file_path.c_str()); + + while (!checkInitialTransforms()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Please provide TF for your LiDAR and radar data"); + } + + calibrateSensors(); + auto tracking_markers = + visualization_.visualizeTrackMarkers(converged_tracks_, calibrated_radar_to_lidar_eigen_); + tracking_markers_pub_->publish(tracking_markers); + auto text_markers = visualization_.drawCalibrationStatusText( + converged_tracks_.size(), transformation_type_, output_metrics_.methods[transformation_type_]); + text_markers_pub_->publish(text_markers); +} + +void ExtrinsicReflectorBasedCalibrator::logErrorAndRespond( + std::shared_ptr & response, + const std::string & error_message) +{ + response->success = false; + RCLCPP_ERROR(this->get_logger(), "%s", error_message.c_str()); +} + +void ExtrinsicReflectorBasedCalibrator::saveDatabaseCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + const std::string & file_path = request->file; + + std::ofstream file(file_path); + + if (!file.is_open()) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Failed to open the file: %s", file_path.c_str()); + return; + } + + // Write headers + file << std::left << std::setw(20) << "lidar_estimation_x" << std::setw(20) + << "lidar_estimation_y" << std::setw(20) << "lidar_estimation_z" << std::setw(20) + << "radar_estimation_x" << std::setw(20) << "radar_estimation_y" << std::setw(20) + << "radar_estimation_z" + << "\n"; + + // Write tracks + for (const auto & track : converged_tracks_) { + file << std::setw(20) << track.lidar_estimation.x() << std::setw(20) + << track.lidar_estimation.y() << std::setw(20) << track.lidar_estimation.z() + << std::setw(20) << track.radar_estimation.x() << std::setw(20) + << track.radar_estimation.y() << std::setw(20) << track.radar_estimation.z() << "\n"; + } + + // Write header + file << "\nlidar_header:\n"; + file << "stamp_sec " << lidar_header_.stamp.sec << "\n"; + file << "stamp_nanosec " << lidar_header_.stamp.nanosec << "\n"; + file << "frame_id " << lidar_header_.frame_id << "\n"; + + file << "\nradar_header:\n"; + file << "stamp_sec " << radar_header_.stamp.sec << "\n"; + file << "stamp_nanosec " << radar_header_.stamp.nanosec << "\n"; + file << "frame_id " << radar_header_.frame_id << "\n"; + + file.close(); + response->success = true; + RCLCPP_INFO(this->get_logger(), "Tracks successfully stored in: %s", file_path.c_str()); +} void ExtrinsicReflectorBasedCalibrator::lidarCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "lidarCallback"); - if (!latest_radar_msgs_ || latest_radar_msgs_->tracks.size() == 0) { - RCLCPP_INFO(this->get_logger(), "There were no tracks"); - return; + 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 radar tracks"); + return; + } + pcl::PointCloud::Ptr radar_pointcloud_ptr = + extractRadarPointcloud(latest_radar_tracks_msgs_); + radar_detections = extractRadarReflectors(radar_pointcloud_ptr); + latest_radar_tracks_msgs_->tracks.clear(); + } else if (msg_type_ == MsgType::radar_scan) { + if (!latest_radar_scan_msgs_ || latest_radar_scan_msgs_->returns.size() == 0) { + RCLCPP_INFO(this->get_logger(), "There were no radar scans"); + return; + } + pcl::PointCloud::Ptr radar_pointcloud_ptr = + extractRadarPointcloud(latest_radar_scan_msgs_); + radar_detections = extractRadarReflectors(radar_pointcloud_ptr); + latest_radar_scan_msgs_->returns.clear(); + } else { + if (!latest_radar_cloud_msgs_) { + RCLCPP_INFO(this->get_logger(), "There were no radar pointclouds"); + return; + } + pcl::PointCloud::Ptr radar_pointcloud_ptr = + extractRadarPointcloud(latest_radar_cloud_msgs_); + radar_detections = extractRadarReflectors(radar_pointcloud_ptr); } - auto lidar_detections = extractReflectors(msg); - auto radar_detections = extractReflectors(latest_radar_msgs_); - latest_radar_msgs_->tracks.clear(); + auto lidar_detections = extractLidarReflectors(msg); + + if (!checkInitialTransforms()) return; auto matches = matchDetections(lidar_detections, radar_detections); - bool is_track_converged = trackMatches(matches, msg->header.stamp); + bool is_track_converged = trackMatches(matches); if (is_track_converged) calibrateSensors(); - visualizationMarkers(lidar_detections, radar_detections, matches); - visualizeTrackMarkers(); - drawCalibrationStatusText(); + 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_); + tracking_markers_pub_->publish(tracking_markers); + auto text_markers = visualization_.drawCalibrationStatusText( + converged_tracks_.size(), transformation_type_, output_metrics_.methods[transformation_type_]); + text_markers_pub_->publish(text_markers); RCLCPP_INFO( this->get_logger(), - "Lidar detections: %lu Radar detections: %lu Matches: %lu Active tracks; %lu Converged tracks: " + "Lidar detections: %lu Radar detections: %lu Matches: %lu Converged tracks: " "%lu", - lidar_detections.size(), radar_detections.size(), matches.size(), active_tracks_.size(), - converged_tracks_.size()); + lidar_detections.size(), radar_detections.size(), matches.size(), converged_tracks_.size()); } -void ExtrinsicReflectorBasedCalibrator::radarCallback( +void ExtrinsicReflectorBasedCalibrator::radarTracksCallback( const radar_msgs::msg::RadarTracks::SharedPtr msg) { - if (!latest_radar_msgs_) { - latest_radar_msgs_ = 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()); + } +} + +void ExtrinsicReflectorBasedCalibrator::radarScanCallback( + const radar_msgs::msg::RadarScan::SharedPtr msg) +{ + if (!latest_radar_scan_msgs_) { + latest_radar_scan_msgs_ = msg; } else { - latest_radar_msgs_->header = msg->header; - latest_radar_msgs_->tracks.insert( - latest_radar_msgs_->tracks.end(), msg->tracks.begin(), msg->tracks.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()); } } -std::vector ExtrinsicReflectorBasedCalibrator::extractReflectors( +void ExtrinsicReflectorBasedCalibrator::radarCloudCallback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + latest_radar_cloud_msgs_ = msg; +} + +std::vector ExtrinsicReflectorBasedCalibrator::extractLidarReflectors( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { lidar_frame_ = msg->header.frame_id; @@ -492,12 +711,14 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector 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", @@ -530,7 +751,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector return detections; } - pcl::PointCloud::Ptr foreground_pointcloud_ptr; + pcl::PointCloud::Ptr foreground_pointcloud_ptr; Eigen::Vector4f ground_model; extractForegroundPoints( lidar_pointcloud_ptr, lidar_background_model_, true, foreground_pointcloud_ptr, ground_model); @@ -593,11 +814,48 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector return detections; } -std::vector ExtrinsicReflectorBasedCalibrator::extractReflectors( - const radar_msgs::msg::RadarTracks::SharedPtr msg) +template +pcl::PointCloud::Ptr +ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr & msg) { + static_assert( + std::is_same::value || + std::is_same::value || + std::is_same::value, + "Unsupported message type"); + radar_frame_ = msg->header.frame_id; radar_header_ = msg->header; + auto radar_pointcloud_ptr = std::make_shared>(); + + if constexpr (std::is_same::value) { + radar_pointcloud_ptr->reserve(msg->tracks.size()); + for (const auto & track : msg->tracks) { + radar_pointcloud_ptr->emplace_back(track.position.x, track.position.y, track.position.z); + } + } else if constexpr (std::is_same::value) { + radar_pointcloud_ptr->reserve(msg->returns.size()); + for (const auto & radar_return : msg->returns) { + float range = radar_return.range; + float azimuth = radar_return.azimuth; + float elevation = radar_return.elevation; + + float x = range * std::cos(azimuth) * std::cos(elevation); + float y = range * std::sin(azimuth) * std::cos(elevation); + float z = range * std::sin(elevation); + + radar_pointcloud_ptr->emplace_back(x, y, z); + } + } else if constexpr (std::is_same::value) { + pcl::fromROSMsg(*msg, *radar_pointcloud_ptr); + } + + return radar_pointcloud_ptr; +} + +std::vector ExtrinsicReflectorBasedCalibrator::extractRadarReflectors( + pcl::PointCloud::Ptr radar_pointcloud_ptr) +{ bool extract_background_model; bool valid_background_model; std::vector detections; @@ -608,16 +866,10 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector valid_background_model = radar_background_model_.valid_; } - pcl::PointCloud::Ptr radar_pointcloud_ptr(new pcl::PointCloud); - radar_pointcloud_ptr->reserve(msg->tracks.size()); - - for (const auto & track : msg->tracks) { - radar_pointcloud_ptr->emplace_back(track.position.x, track.position.y, track.position.z); - } - 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)); @@ -631,7 +883,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector if (extract_background_model && !valid_background_model) { extractBackgroundModel( - radar_pointcloud_ptr, msg->header, latest_updated_radar_header_, first_radar_header_, + radar_pointcloud_ptr, radar_header_, latest_updated_radar_header_, first_radar_header_, radar_background_model_); return detections; } @@ -640,7 +892,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector return detections; } - pcl::PointCloud::Ptr foreground_pointcloud_ptr; + pcl::PointCloud::Ptr foreground_pointcloud_ptr; Eigen::Vector4f ground_model; extractForegroundPoints( radar_pointcloud_ptr, radar_background_model_, false, foreground_pointcloud_ptr, ground_model); @@ -681,7 +933,7 @@ std::vector ExtrinsicReflectorBasedCalibrator::extractReflector } 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) { @@ -727,7 +979,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); @@ -772,16 +1024,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::Vector4f & ground_model) + pcl::PointCloud::Ptr & foreground_pointcloud_ptr, + Eigen::Vector4f & 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); @@ -789,7 +1043,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()) / @@ -814,7 +1069,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; @@ -841,10 +1097,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); @@ -873,16 +1130,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); @@ -893,10 +1151,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) { @@ -916,7 +1175,7 @@ ExtrinsicReflectorBasedCalibrator::extractClusters( } std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFromClusters( - const std::vector::Ptr> & clusters, + const std::vector::Ptr> & clusters, const Eigen::Vector4f & ground_model) { std::vector reflector_centers; @@ -924,7 +1183,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 = @@ -939,7 +1198,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; @@ -950,12 +1210,33 @@ std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFr highest_point, parameters_.reflector_radius, indexes, squared_distances) > 0) { Eigen::Vector3d center = Eigen::Vector3d::Zero(); - for (const auto & index : indexes) { - const auto & p = cluster_pointcloud_ptr->points[index]; - center += Eigen::Vector3d(p.x, p.y, p.z); + if (cluster_pointcloud_ptr->points.size() > parameters_.reflector_points_threshold) { + // Locate the center of the reflector at the maximum distance (This works better for high + // resolution LiDARs) + double max_distance = -std::numeric_limits::infinity(); + for (const auto & index : indexes) { + const auto & point = cluster_pointcloud_ptr->points[index]; + const Eigen::Vector3d point_in_lidar_frame = Eigen::Vector3d(point.x, point.y, point.z); + const Eigen::Vector3d point_in_radar_frame = + initial_radar_to_lidar_eigen_ * point_in_lidar_frame; + + const auto point_xy_distance = + std::hypot(point_in_radar_frame.x(), point_in_radar_frame.y()); + if (point_xy_distance > max_distance) { + max_distance = point_xy_distance; + center = initial_radar_to_lidar_eigen_.inverse() * point_in_radar_frame; + } + } + } else { + // Locate the center of the reflector by averaging all of the points in the cluster + for (const auto & index : indexes) { + const auto & p = cluster_pointcloud_ptr->points[index]; + center += Eigen::Vector3d(p.x, p.y, p.z); + } + + center /= indexes.size(); } - center /= indexes.size(); RCLCPP_INFO( this->get_logger(), "\t Lidar reflector id=%lu size=%lu center: x=%.2f y=%.2f z=%.2f", reflector_centers.size(), indexes.size(), center.x(), center.y(), center.z()); @@ -968,14 +1249,14 @@ std::vector ExtrinsicReflectorBasedCalibrator::findReflectorsFr bool ExtrinsicReflectorBasedCalibrator::checkInitialTransforms() { - if (lidar_frame_ == "" || radar_frame_ == "") { - return false; - } - if (got_initial_transform_) { return true; } + if (lidar_frame_ == "" || radar_frame_ == "") { + return false; + } + try { rclcpp::Time t = rclcpp::Time(0); rclcpp::Duration timeout = rclcpp::Duration::from_seconds(1.0); @@ -986,18 +1267,50 @@ bool ExtrinsicReflectorBasedCalibrator::checkInitialTransforms() initial_radar_to_lidar_eigen_ = tf2::transformToEigen(initial_radar_to_lidar_msg_); calibrated_radar_to_lidar_eigen_ = initial_radar_to_lidar_eigen_; - radar_parallel_to_lidar_msg_ = - tf_buffer_->lookupTransform(parameters_.radar_parallel_frame, lidar_frame_, t, timeout) + radar_optimization_to_lidar_msg_ = + tf_buffer_->lookupTransform(parameters_.radar_optimization_frame, lidar_frame_, t, timeout) + .transform; + + radar_optimization_to_lidar_eigen_ = tf2::transformToEigen(radar_optimization_to_lidar_msg_); + + initial_radar_optimization_to_radar_msg_ = + tf_buffer_->lookupTransform(parameters_.radar_optimization_frame, radar_frame_, t, timeout) .transform; - radar_parallel_to_lidar_eigen_ = tf2::transformToEigen(radar_parallel_to_lidar_msg_); + initial_radar_optimization_to_radar_eigen_ = + tf2::transformToEigen(initial_radar_optimization_to_radar_msg_); + + RCLCPP_INFO_STREAM( + this->get_logger(), "radar_optimization_to_lidar_eigen_:\n" + << radar_optimization_to_lidar_eigen_.matrix()); + RCLCPP_INFO_STREAM( + this->get_logger(), "initial_radar_optimization_to_radar_eigen_:\n" + << initial_radar_optimization_to_radar_eigen_.matrix()); got_initial_transform_ = true; } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "could not get initial tf. %s", ex.what()); + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "could not get initial tf. %s", ex.what()); return false; } + // Set visualization parameters + VisualizationParameters params; + params.lidar_header = lidar_header_; + params.radar_header = radar_header_; + params.transformation_type = transformation_type_; + params.reflector_radius = parameters_.reflector_radius; + if ( + transformation_type_ == TransformationType::svd_2d || + transformation_type_ == TransformationType::yaw_only_rotation_2d) { + params.marker_size_per_track = 9; + } else { + params.marker_size_per_track = 8; + } + + params.initial_radar_to_lidar_eigen = initial_radar_to_lidar_eigen_; + visualization_.setParameters(params); + return true; } @@ -1008,7 +1321,7 @@ ExtrinsicReflectorBasedCalibrator::matchDetections( { std::vector> matched_detections; - if (lidar_detections.size() == 0 || radar_detections.size() == 0 || !checkInitialTransforms()) { + if (lidar_detections.size() == 0 || radar_detections.size() == 0) { return matched_detections; } @@ -1019,9 +1332,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::svd_2d || + transformation_type == TransformationType::yaw_only_rotation_2d) { + transformed_point.z() = 0.f; + } return transformed_point; }); @@ -1106,8 +1424,7 @@ ExtrinsicReflectorBasedCalibrator::matchDetections( } bool ExtrinsicReflectorBasedCalibrator::trackMatches( - const std::vector> & matches, - builtin_interfaces::msg::Time & current_time) + const std::vector> & matches) { std::unique_lock lock(mutex_); @@ -1115,102 +1432,89 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches( return false; } - // Check if active tracks expired - auto timed_out_end = std::remove_if( - active_tracks_.begin(), active_tracks_.end(), - [¤t_time](const auto & track) { return track.timedOut(current_time); }); - - active_tracks_.erase(timed_out_end, active_tracks_.end()); + num_of_frame_++; + if (num_of_frame_ < parameters_.min_frames_for_convergence) { + for (const auto & match : matches) { + bool added_to_existing_group = false; + + for (auto & track_group : converging_tracks_) { + bool is_tracking_same_match = + std::any_of(track_group.begin(), track_group.end(), [this, &match](const Track & track) { + double lidar_distance = (track.lidar_estimation - match.first).norm(); + double radar_distance = (track.radar_estimation - match.second).norm(); + return lidar_distance < parameters_.reflector_radius && + radar_distance < parameters_.reflector_radius; + }); + + if (is_tracking_same_match) { + track_group.emplace_back(Track{track_group[0].id, match.first, match.second, 0, 0}); + added_to_existing_group = true; + break; + } + } - // Update tracks - for (const auto & [lidar_detection, radar_detection] : matches) { - if (std::any_of( - converged_tracks_.begin(), converged_tracks_.end(), - [&lidar_detection, &radar_detection](auto & track) { - return track.partialMatch(lidar_detection, radar_detection); - })) { - continue; + if (!added_to_existing_group) { + converging_tracks_.emplace_back(std::vector{ + Track{static_cast(converged_tracks_.size()), match.first, match.second, 0, 0}}); + } } + return false; + } - bool new_track = std::all_of( - active_tracks_.begin(), active_tracks_.end(), - [&lidar_detection, &radar_detection](auto & track) { - return !track.match(lidar_detection, radar_detection); + // Proceed only if we have enough tracks in each converging track group + converging_tracks_.erase( + std::remove_if( + converging_tracks_.begin(), converging_tracks_.end(), + [this](const std::vector & tracks) { + return tracks.size() < static_cast(parameters_.min_frames_for_convergence / 2); + }), + converging_tracks_.end()); + + for (const auto & track_group : converging_tracks_) { + auto max_distance_track = std::max_element( + track_group.begin(), track_group.end(), [](const Track & a, const Track & b) { + double distance_a = std::hypot(a.lidar_estimation.x(), a.lidar_estimation.y()); + double distance_b = std::hypot(b.lidar_estimation.x(), b.lidar_estimation.y()); + return distance_a < distance_b; }); - if (new_track) { - active_tracks_.push_back( - factory_ptr_->makeTrack(lidar_detection, radar_detection, current_time)); - } else { - std::for_each( - active_tracks_.begin(), active_tracks_.end(), - [&lidar_detection, &radar_detection](auto & track) { - track.updateIfMatch(lidar_detection, radar_detection); - }); - } + converged_tracks_.push_back(*max_distance_track); } - // Move converged tracks - std::copy_if( - active_tracks_.begin(), active_tracks_.end(), std::back_inserter(converged_tracks_), - [](auto & track) { return track.converged(); }); + updateTrackIds(converged_tracks_); - current_new_tracks_ += std::transform_reduce( - active_tracks_.begin(), active_tracks_.end(), 0, std::plus{}, - [](auto & track) { return track.converged(); }); + RCLCPP_INFO(this->get_logger(), "converged_tracks size= %lu", converged_tracks_.size()); - bool is_track_converged = std::any_of( - active_tracks_.begin(), active_tracks_.end(), [](auto & track) { return track.converged(); }); + current_new_tracks_ = converging_tracks_.size(); + tracking_active_ = false; + num_of_frame_ = 0; + converging_tracks_.clear(); - auto converged_end = std::remove_if( - active_tracks_.begin(), active_tracks_.end(), - [¤t_time](auto & track) { return track.converged(); }); - - active_tracks_.erase(converged_end, active_tracks_.end()); - - if (current_new_tracks_ > 0 && active_tracks_.size() == 0) { - tracking_active_ = false; + if (!current_new_tracks_) { + return false; } - - return is_track_converged; + return true; } -std::tuple< - pcl::PointCloud::Ptr, - pcl::PointCloud::Ptr, double, double> -ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() +std::tuple ExtrinsicReflectorBasedCalibrator::get2DRotationDelta( + std::vector::iterator & begin, std::vector::iterator & end, bool is_crossval) { - // Define two sets of 2D points (just 3D points with z=0) - // Note: pcs=parallel coordinate system rcs=radar coordinate system - pcl::PointCloud::Ptr lidar_points_pcs(new pcl::PointCloud); - pcl::PointCloud::Ptr radar_points_rcs(new pcl::PointCloud); - lidar_points_pcs->reserve(converged_tracks_.size()); - radar_points_rcs->reserve(converged_tracks_.size()); - double delta_cos_sum = 0.0; double delta_sin_sum = 0.0; - auto eigen_to_pcl_2d = [](const auto & p) { return PointType(p.x(), p.y(), 0.0); }; - - for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { - auto track = converged_tracks_[track_index]; + for (auto track = begin; track != end; ++track) { // lidar coordinates - const auto & lidar_estimation = track.getLidarEstimation(); - // to radar parallel coordinates - const auto & lidar_estimation_pcs = radar_parallel_to_lidar_eigen_ * lidar_estimation; // to radar coordinates - const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation; - const auto & radar_estimation_rcs = track.getRadarEstimation(); - lidar_points_pcs->emplace_back(eigen_to_pcl_2d(lidar_estimation_pcs)); - radar_points_rcs->emplace_back(eigen_to_pcl_2d(radar_estimation_rcs)); + const auto & lidar_transformed_estimation = + initial_radar_to_lidar_eigen_ * track->lidar_estimation; const double lidar_transformed_norm = lidar_transformed_estimation.norm(); const double lidar_transformed_cos = lidar_transformed_estimation.x() / lidar_transformed_norm; const double lidar_transformed_sin = lidar_transformed_estimation.y() / lidar_transformed_norm; - const double radar_norm = radar_estimation_rcs.norm(); - const double radar_cos = radar_estimation_rcs.x() / radar_norm; - const double radar_sin = radar_estimation_rcs.y() / radar_norm; + const double radar_norm = track->radar_estimation.norm(); + const double radar_cos = track->radar_estimation.x() / radar_norm; + const double radar_sin = track->radar_estimation.y() / radar_norm; // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b) // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b) @@ -1220,6 +1524,65 @@ ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() delta_sin_sum += delta_angle_sin; delta_cos_sum += delta_angle_cos; + if (!is_crossval) { + // logging + RCLCPP_INFO_STREAM( + this->get_logger(), "lidar_estimation:\n" + << track->lidar_estimation.matrix()); + RCLCPP_INFO_STREAM( + this->get_logger(), "lidar_transformed_estimation:\n" + << lidar_transformed_estimation.matrix()); + RCLCPP_INFO_STREAM( + this->get_logger(), "radar_estimation:\n" + << track->radar_estimation.matrix()); + } + } + + auto track_size = std::distance(begin, end); + double delta_cos = delta_cos_sum / track_size; + double delta_sin = -delta_sin_sum / track_size; + + return {delta_cos, delta_sin}; +} + +std::tuple< + pcl::PointCloud::Ptr, pcl::PointCloud::Ptr> +ExtrinsicReflectorBasedCalibrator::getPointsSet( + std::vector::iterator & begin, std::vector::iterator & end) +{ + // 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); + + auto track_size = std::distance(begin, end); + lidar_points_ocs->reserve(track_size); + radar_points_rcs->reserve(track_size); + + 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 (auto track = begin; track != end; ++track) { + // lidar coordinates + const auto & lidar_estimation = track->lidar_estimation; + // to radar optimization coordinates + const auto & lidar_estimation_ocs = radar_optimization_to_lidar_eigen_ * lidar_estimation; + // to radar coordinates + const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation; + const auto & radar_estimation_rcs = track->radar_estimation; + + if ( + transformation_type_ == TransformationType::svd_2d || + transformation_type_ == TransformationType::yaw_only_rotation_2d) { + lidar_points_ocs->emplace_back(eigen_to_pcl_2d(lidar_estimation_ocs)); + radar_points_rcs->emplace_back(eigen_to_pcl_2d(radar_estimation_rcs)); + } else { + lidar_points_ocs->emplace_back(eigen_to_pcl_3d(lidar_estimation_ocs)); + radar_points_rcs->emplace_back(eigen_to_pcl_3d(radar_estimation_rcs)); + } // logging RCLCPP_INFO_STREAM(this->get_logger(), "lidar_estimation:\n" << lidar_estimation.matrix()); RCLCPP_INFO_STREAM( @@ -1229,100 +1592,106 @@ ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta() this->get_logger(), "radar_estimation_rcs:\n" << radar_estimation_rcs.matrix()); } - return {lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum}; + return {lidar_points_ocs, radar_points_rcs}; } -std::pair ExtrinsicReflectorBasedCalibrator::computeCalibrationError( - const Eigen::Isometry3d & radar_to_lidar_isometry) +TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation( + std::size_t track_index) { - double distance_error = 0.0; - double yaw_error = 0.0; + TransformationResult transformation_result; + TransformationEstimator estimator( + initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, + radar_optimization_to_lidar_eigen_); - for (auto & track : converged_tracks_) { - 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; - - distance_error += (lidar_estimation_transformed - radar_estimation).norm(); - yaw_error += getYawError(lidar_estimation_transformed, radar_estimation); - } + auto begin = converged_tracks_.begin(); + auto end = begin + track_index + 1; - distance_error /= static_cast(converged_tracks_.size()); - yaw_error *= 180.0 / (M_PI * static_cast(converged_tracks_.size())); - - return std::make_pair(distance_error, yaw_error); -} + if ( + transformation_type_ == TransformationType::svd_2d || + transformation_type_ == TransformationType::yaw_only_rotation_2d) { + // yaw only rotation + auto [delta_cos, delta_sin] = get2DRotationDelta(begin, end, false); + estimator.set2DRotationDelta(delta_cos, delta_sin); + estimator.estimateYawOnlyTransformation(); + transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d] = + estimator.getTransformation(); + + // svd 2d transformation + std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) = + getPointsSet(begin, end); + estimator.setPoints( + transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs); + estimator.estimateSVDTransformation(transformation_type_); + transformation_result.calibrated_radar_to_lidar_transformations[TransformationType::svd_2d] = + estimator.getTransformation(); -void ExtrinsicReflectorBasedCalibrator::estimateTransformation( - pcl::PointCloud::Ptr lidar_points_pcs, - pcl::PointCloud::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum) -{ - // Note: pcs=parallel coordinate system rcs=radar coordinate system - // Estimate full transformation using SVD - pcl::registration::TransformationEstimationSVD estimator; - Eigen::Matrix4f full_radar_to_radar_parallel_transformation; - estimator.estimateRigidTransformation( - *lidar_points_pcs, *radar_points_rcs, full_radar_to_radar_parallel_transformation); - Eigen::Isometry3d calibrated_2d_radar_to_radar_parallel_transformation( - full_radar_to_radar_parallel_transformation.cast()); - - // Check that it is actually a 2D transformation - auto calibrated_2d_radar_to_radar_parallel_rpy = autoware::universe_utils::getRPY( - tf2::toMsg(calibrated_2d_radar_to_radar_parallel_transformation).orientation); - double calibrated_2d_radar_to_radar_parallel_z = - calibrated_2d_radar_to_radar_parallel_transformation.translation().z(); - double calibrated_2d_radar_to_radar_parallel_roll = calibrated_2d_radar_to_radar_parallel_rpy.x; - double calibrated_2d_radar_to_radar_parallel_pitch = calibrated_2d_radar_to_radar_parallel_rpy.y; + RCLCPP_INFO_STREAM( + this->get_logger(), "Initial radar->lidar transform:\n" + << initial_radar_to_lidar_eigen_.matrix()); + RCLCPP_INFO_STREAM( + this->get_logger(), + "Yaw-only rotation 2D radar->lidar transform:\n" + << transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::yaw_only_rotation_2d] + .matrix()); + RCLCPP_INFO_STREAM( + this->get_logger(), + "SVD 2D calibration radar->lidar transform:\n" + << transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::svd_2d] + .matrix()); + } else { + std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) = + getPointsSet(begin, end); + estimator.setPoints( + transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs); + + // zero roll 3d transformation + estimator.estimateZeroRollTransformation(); + transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::zero_roll_3d] = + estimator.getTransformation(); + + // svd 3d transformation + estimator.estimateSVDTransformation(transformation_type_); + transformation_result.calibrated_radar_to_lidar_transformations[TransformationType::svd_3d] = + 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(), + "Roll zero 3D calibration radar->lidar transform:\n" + << transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::zero_roll_3d] + .matrix()); - if ( - calibrated_2d_radar_to_radar_parallel_z != 0.0 || - calibrated_2d_radar_to_radar_parallel_roll != 0.0 || - calibrated_2d_radar_to_radar_parallel_pitch != 0.0) { - RCLCPP_ERROR( + RCLCPP_INFO_STREAM( this->get_logger(), - "The estimated 2D translation was not really 2D. Continue at your own risk. z=%.3f roll=%.3f " - "pitch=%.3f", - calibrated_2d_radar_to_radar_parallel_z, calibrated_2d_radar_to_radar_parallel_roll, - calibrated_2d_radar_to_radar_parallel_pitch); + "SVD 3D calibration radar->lidar transform:\n" + << transformation_result + .calibrated_radar_to_lidar_transformations[TransformationType::svd_3d] + .matrix()); } - calibrated_2d_radar_to_radar_parallel_transformation.translation().z() = - (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse()).translation().z(); - Eigen::Isometry3d calibrated_2d_radar_to_lidar_transformation = - calibrated_2d_radar_to_radar_parallel_transformation * radar_parallel_to_lidar_eigen_; - - // Estimate the 2D transformation estimating only yaw - double delta_cos = delta_cos_sum / converged_tracks_.size(); - double delta_sin = -delta_sin_sum / converged_tracks_.size(); + return transformation_result; +} - Eigen::Matrix3d delta_rotation; - delta_rotation << delta_cos, -delta_sin, 0.0, delta_sin, delta_cos, 0.0, 0.0, 0.0, 1.0; - Eigen::Isometry3d delta_transformation = Eigen::Isometry3d::Identity(); - delta_transformation.linear() = delta_rotation; - Eigen::Isometry3d calibrated_rotation_radar_to_lidar_transformation = - delta_transformation * initial_radar_to_lidar_eigen_; +void ExtrinsicReflectorBasedCalibrator::evaluateTransformation( + TransformationResult transformation_result, std::size_t track_index) +{ + auto begin = converged_tracks_.begin(); + auto end = begin + track_index + 1; // Estimate the pre & post calibration error auto [initial_distance_error, initial_yaw_error] = - computeCalibrationError(initial_radar_to_lidar_eigen_); - auto [calibrated_2d_distance_error, calibrated_2d_yaw_error] = - computeCalibrationError(calibrated_2d_radar_to_lidar_transformation); - auto [calibrated_rotation_distance_error, calibrated_rotation_yaw_error] = - computeCalibrationError(calibrated_rotation_radar_to_lidar_transformation); - - 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_2d_radar_to_lidar_transformation.matrix()); - RCLCPP_INFO_STREAM( - this->get_logger(), "Pure rotation calibration radar->lidar transform:\n" - << calibrated_rotation_radar_to_lidar_transformation.matrix()); - - // Evaluate the different calibrations and decide on an output + computeCalibrationError(begin, end, transformation_type_, initial_radar_to_lidar_eigen_, false); + RCLCPP_INFO( + this->get_logger(), + "Initial calibration error: detection2detection.distance=%.4fm yaw=%.4f degrees", + initial_distance_error, initial_yaw_error); + auto compute_transformation_difference = [](const Eigen::Isometry3d & t1, const Eigen::Isometry3d & t2) -> std::pair { double translation_difference = (t2.inverse() * t1).translation().norm(); @@ -1331,500 +1700,226 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation( return std::make_pair(translation_difference, rotation_difference); }; - RCLCPP_INFO( - this->get_logger(), - "Initial calibration error: detection2detection.distance=%.4fm yaw=%.4f degrees", - initial_distance_error, initial_yaw_error); - RCLCPP_INFO( - this->get_logger(), - "Final calibration error: detection2detection.distance=%.4fm yaw=%.4f degrees", - calibrated_2d_distance_error, calibrated_2d_yaw_error); - RCLCPP_INFO( - this->get_logger(), - "Final calibration error (rotation only): detection2detection.distance=%.4fm yaw=%.4f degrees", - calibrated_rotation_distance_error, calibrated_rotation_yaw_error); - - auto [calibrated_2d_translation_difference, calibrated_2d_rotation_difference] = - compute_transformation_difference( - initial_radar_to_lidar_eigen_, calibrated_2d_radar_to_lidar_transformation); - auto [calibrated_rotation_translation_difference, calibrated_rotation_rotation_difference] = - compute_transformation_difference( - initial_radar_to_lidar_eigen_, calibrated_rotation_radar_to_lidar_transformation); - std::unique_lock lock(mutex_); - if ( - calibrated_2d_translation_difference < parameters_.max_initial_calibration_translation_error && - calibrated_2d_rotation_difference < parameters_.max_initial_calibration_rotation_error) { + for (const auto & [type, transformation] : + transformation_result.calibrated_radar_to_lidar_transformations) { + auto record_error_in_track = + true ? track_index == converged_tracks_.size() - 1 && type == transformation_type_ : false; + + auto [distance_error, yaw_error] = computeCalibrationError( + begin, end, transformation_type_, transformation, record_error_in_track); + output_metrics_.methods[type].calibrated_distance_errors.push_back(distance_error); + output_metrics_.methods[type].calibrated_yaw_errors.push_back(yaw_error); + + if (type == transformation_type_) { + auto [calibrated_translation_difference, calibrated_rotation_difference] = + compute_transformation_difference(initial_radar_to_lidar_eigen_, transformation); + std::unique_lock lock(mutex_); + if ( + calibrated_translation_difference < parameters_.max_initial_calibration_translation_error && + calibrated_rotation_difference < parameters_.max_initial_calibration_rotation_error && + track_index == converged_tracks_.size() - 1) { + calibrated_radar_to_lidar_eigen_ = transformation; + calibration_valid_ = true; + calibration_distance_score_ = distance_error; + calibration_yaw_score_ = yaw_error; + } else { + RCLCPP_WARN( + this->get_logger(), + "The calibrated poses differ considerably with the initial calibration. This may be " + "either a " + "fault of the algorithm or a bad calibration initialization"); + } + } + // Log for all types RCLCPP_INFO( - this->get_logger(), "The 2D calibration pose was chosen as the output calibration pose"); - calibrated_radar_to_lidar_eigen_ = calibrated_2d_radar_to_lidar_transformation; - calibration_valid_ = true; - calibration_distance_score_ = calibrated_2d_distance_error; - calibration_yaw_score_ = calibrated_2d_yaw_error; - } else if ( - calibrated_rotation_translation_difference < - parameters_.max_initial_calibration_translation_error && - calibrated_rotation_rotation_difference < parameters_.max_initial_calibration_rotation_error) { - RCLCPP_WARN( - this->get_logger(), - "The pure rotation calibration pose was chosen as the output calibration pose. This may mean " - "you need to collect more points"); - calibrated_radar_to_lidar_eigen_ = calibrated_rotation_radar_to_lidar_transformation; - calibration_valid_ = true; - calibration_distance_score_ = calibrated_rotation_distance_error; - calibration_yaw_score_ = calibrated_rotation_yaw_error; - } else { - RCLCPP_WARN( - this->get_logger(), - "The calibrated poses differ considerably with the initial calibration. This may be either a " - "fault of the algorithm or a bad calibration initialization"); - } - - output_metrics_.push_back(static_cast(converged_tracks_.size())); - output_metrics_.push_back(static_cast(calibrated_2d_distance_error)); - output_metrics_.push_back(static_cast(calibrated_2d_yaw_error)); -} - -void ExtrinsicReflectorBasedCalibrator::findCombinations( - int n, int k, std::vector & curr, int first_num, - std::vector> & combinations) -{ - int curr_size = static_cast(curr.size()); - if (curr_size == k) { - combinations.push_back(curr); - return; + this->get_logger(), "Type: %s, distance error: %.4fm, yaw error: %.4f degrees", + toString(type).c_str(), distance_error, yaw_error); } - int need = k - curr_size; - int remain = n - first_num + 1; - int available = remain - need; - - for (int num = first_num; num <= first_num + available; num++) { - curr.push_back(num); - findCombinations(n, k, curr, num + 1, combinations); - curr.pop_back(); + if (track_index == converged_tracks_.size() - 1) { + output_metrics_.num_of_converged_tracks = converged_tracks_.size(); + for (const auto & converge_track : converged_tracks_) { + output_metrics_.detections.push_back(eigenToPointMsg(converge_track.radar_estimation)); + } } - - return; } -void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( - pcl::PointCloud::Ptr lidar_points_pcs, - pcl::PointCloud::Ptr radar_points_rcs) +void ExtrinsicReflectorBasedCalibrator::evaluateCombinations( + std::vector> & combinations, std::size_t num_of_samples, + TransformationResult transformation_result) { - // Note: pcs=parallel coordinate system rcs=radar coordinate system - int tracks_size = static_cast(converged_tracks_.size()); - if (tracks_size <= 3) return; - - pcl::PointCloud::Ptr crossval_lidar_points_pcs(new pcl::PointCloud); - pcl::PointCloud::Ptr crossval_radar_points_rcs(new pcl::PointCloud); - pcl::registration::TransformationEstimationSVD crossval_estimator; - Eigen::Matrix4f crossval_radar_to_radar_parallel_transformation; - Eigen::Isometry3d crossval_calibrated_2d_radar_to_radar_parallel_transformation; - Eigen::Isometry3d crossval_calibrated_2d_radar_to_lidar_transformation; - - for (int num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) { - crossval_lidar_points_pcs->reserve(num_of_samples); - crossval_radar_points_rcs->reserve(num_of_samples); - std::vector> combinations; - std::vector curr; - std::vector crossval_calibrated_2d_distance_error_vector; - std::vector crossval_calibrated_2d_yaw_error_vector; - double total_crossval_calibrated_2d_distance_error = 0.0; - double total_crossval_calibrated_2d_yaw_error = 0.0; - - findCombinations(tracks_size - 1, num_of_samples, curr, 0, combinations); - - RCLCPP_INFO( - this->get_logger(), - "The number of combinations is: %d, converged_tracks_size: %d, num_of_samples: %d", - static_cast(combinations.size()), tracks_size, num_of_samples); + // Initialize cross-validation estimator + TransformationEstimator crossval_estimator( + initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_, + radar_optimization_to_lidar_eigen_); + + // Prepare containers for cross-validation + 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); + crossval_converged_tracks.reserve(num_of_samples); + + // Containers to store results for each transformation type + std::unordered_map> distance_error_vectors; + std::unordered_map> yaw_error_vectors; + std::unordered_map total_distance_errors; + std::unordered_map total_yaw_errors; + + // Initialize metrics containers for all transformation types + for (const auto & [type, _] : transformation_result.calibrated_radar_to_lidar_transformations) { + distance_error_vectors[type] = {}; + yaw_error_vectors[type] = {}; + total_distance_errors[type] = 0.0; + total_yaw_errors[type] = 0.0; + } - // random select the combinations if the number of combinations is too large - if ( - combinations.size() > - static_cast(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); - } + for (const auto & combination : combinations) { + // Prepare the cross-validation data for the current combination + crossval_lidar_points_ocs->clear(); + crossval_radar_points_rcs->clear(); + crossval_converged_tracks.clear(); - for (const auto & combination : combinations) { - // clear the lidar radar pcs - crossval_lidar_points_pcs->clear(); - crossval_radar_points_rcs->clear(); - // calculate the transformation. - for (int j = 0; j < num_of_samples; j++) { - crossval_lidar_points_pcs->emplace_back(lidar_points_pcs->points[combination[j]]); - crossval_radar_points_rcs->emplace_back(radar_points_rcs->points[combination[j]]); - } - crossval_estimator.estimateRigidTransformation( - *crossval_lidar_points_pcs, *crossval_radar_points_rcs, - crossval_radar_to_radar_parallel_transformation); - crossval_calibrated_2d_radar_to_radar_parallel_transformation = - crossval_radar_to_radar_parallel_transformation.cast(); - crossval_calibrated_2d_radar_to_radar_parallel_transformation.translation().z() = - (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse()) - .translation() - .z(); - crossval_calibrated_2d_radar_to_lidar_transformation = - crossval_calibrated_2d_radar_to_radar_parallel_transformation * - radar_parallel_to_lidar_eigen_; - - // calculate the error. - auto [crossval_calibrated_2d_distance_error, crossval_calibrated_2d_yaw_error] = - computeCalibrationError(crossval_calibrated_2d_radar_to_lidar_transformation); - - total_crossval_calibrated_2d_distance_error += crossval_calibrated_2d_distance_error; - total_crossval_calibrated_2d_yaw_error += crossval_calibrated_2d_yaw_error; - crossval_calibrated_2d_distance_error_vector.push_back(crossval_calibrated_2d_distance_error); - crossval_calibrated_2d_yaw_error_vector.push_back(crossval_calibrated_2d_yaw_error); + for (std::size_t i : combination) { + crossval_lidar_points_ocs->emplace_back(transformation_result.lidar_points_ocs->points[i]); + crossval_radar_points_rcs->emplace_back(transformation_result.radar_points_rcs->points[i]); + crossval_converged_tracks.push_back(converged_tracks_[i]); } - auto calculate_std = [](std::vector & data, double mean) -> double { - double sum = 0.0; - for (size_t i = 0; i < data.size(); i++) { - sum += (data[i] - mean) * (data[i] - mean); + // Estimate transformations for each type + for (const auto & [type, _] : transformation_result.calibrated_radar_to_lidar_transformations) { + Eigen::Isometry3d calibrated_transformation; + + if (type == TransformationType::yaw_only_rotation_2d) { + auto begin = crossval_converged_tracks.begin(); + auto end = crossval_converged_tracks.end(); + auto [delta_cos, delta_sin] = get2DRotationDelta(begin, end, true); + crossval_estimator.set2DRotationDelta(delta_cos, delta_sin); + crossval_estimator.estimateYawOnlyTransformation(); + calibrated_transformation = crossval_estimator.getTransformation(); + } else if (type == TransformationType::svd_2d) { + crossval_estimator.setPoints(crossval_lidar_points_ocs, crossval_radar_points_rcs); + crossval_estimator.estimateSVDTransformation(type); + calibrated_transformation = crossval_estimator.getTransformation(); + } else if (type == TransformationType::zero_roll_3d) { + crossval_estimator.setPoints(crossval_lidar_points_ocs, crossval_radar_points_rcs); + crossval_estimator.estimateZeroRollTransformation(); + calibrated_transformation = crossval_estimator.getTransformation(); + } else if (type == TransformationType::svd_3d) { + crossval_estimator.setPoints(crossval_lidar_points_ocs, crossval_radar_points_rcs); + crossval_estimator.estimateSVDTransformation(type); + calibrated_transformation = crossval_estimator.getTransformation(); } - double variance = sum / data.size(); - return std::sqrt(variance); - }; - - double avg_crossval_calibrated_2d_distance_error = - total_crossval_calibrated_2d_distance_error / combinations.size(); - double avg_crossval_calibrated_2d_yaw_error = - total_crossval_calibrated_2d_yaw_error / combinations.size(); - output_metrics_.push_back(static_cast(num_of_samples)); - output_metrics_.push_back(static_cast(avg_crossval_calibrated_2d_distance_error)); - output_metrics_.push_back(static_cast(avg_crossval_calibrated_2d_yaw_error)); - - double std_crossval_calibrated_2d_distance_error = calculate_std( - crossval_calibrated_2d_distance_error_vector, avg_crossval_calibrated_2d_distance_error); - double std_crossval_calibrated_2d_yaw_error = - calculate_std(crossval_calibrated_2d_yaw_error_vector, avg_crossval_calibrated_2d_yaw_error); - output_metrics_.push_back(static_cast(std_crossval_calibrated_2d_distance_error)); - output_metrics_.push_back(static_cast(std_crossval_calibrated_2d_yaw_error)); - } -} - -void ExtrinsicReflectorBasedCalibrator::publishMetrics() -{ - // The final format of the output metrics is - // num of reflectors, calibration_distance_error, calibration_yaw_error, - // sample, avg_crossval_dis_error, avg_crossval_yaw_error, std_crossval_dis_error, - // std_crossval_yaw_error. sample, .... - std_msgs::msg::Float32MultiArray calibration_metrics_msg = std_msgs::msg::Float32MultiArray(); - calibration_metrics_msg.data = output_metrics_; - metrics_pub_->publish(calibration_metrics_msg); -} -void ExtrinsicReflectorBasedCalibrator::calibrateSensors() -{ - if (!checkInitialTransforms() || converged_tracks_.size() == 0) { - if (converged_tracks_.size() == 0) { - std_msgs::msg::Float32MultiArray calibration_metrics_msg = std_msgs::msg::Float32MultiArray(); - calibration_metrics_msg.data = {0, 0, 0, 0, 0}; - metrics_pub_->publish(calibration_metrics_msg); + // Compute errors for the transformation + auto begin = converged_tracks_.begin(); + auto end = converged_tracks_.end(); + auto [distance_error, yaw_error] = + computeCalibrationError(begin, end, transformation_type_, calibrated_transformation, false); + total_distance_errors[type] += distance_error; + total_yaw_errors[type] += yaw_error; + distance_error_vectors[type].push_back(distance_error); + yaw_error_vectors[type].push_back(yaw_error); } - return; } - output_metrics_.clear(); - - // Note: pcs=parallel coordinate system rcs=radar coordinate system - auto [lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum] = getPointsSetAndDelta(); - estimateTransformation(lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum); - crossValEvaluation(lidar_points_pcs, radar_points_rcs); - publishMetrics(); -} - -void ExtrinsicReflectorBasedCalibrator::visualizationMarkers( - const std::vector & lidar_detections, - const std::vector & radar_detections, - const std::vector> & matched_detections) -{ - visualization_msgs::msg::MarkerArray lidar_detections_marker_array; - - for (std::size_t detection_index = 0; detection_index < lidar_detections.size(); - detection_index++) { - const auto & detection_center = lidar_detections[detection_index]; - visualization_msgs::msg::Marker marker; - marker.header = lidar_header_; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.id = detection_index; - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.position.x = detection_center.x(); - marker.pose.position.y = detection_center.y(); - marker.pose.position.z = detection_center.z(); - marker.pose.orientation.w = 1.0; - marker.scale.x = parameters_.reflector_radius; - marker.scale.y = parameters_.reflector_radius; - marker.scale.z = parameters_.reflector_radius; - marker.color.a = 0.6; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - lidar_detections_marker_array.markers.push_back(marker); - } - - lidar_detections_pub_->publish(lidar_detections_marker_array); - - visualization_msgs::msg::MarkerArray radar_detections_marker_array; - - for (std::size_t detection_index = 0; detection_index < radar_detections.size(); - detection_index++) { - const auto & detection_center = radar_detections[detection_index]; - visualization_msgs::msg::Marker marker; - marker.header = radar_header_; - marker.id = detection_index; - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.ns = "center"; - marker.pose.position.x = detection_center.x(); - marker.pose.position.y = detection_center.y(); - marker.pose.position.z = detection_center.z(); - marker.pose.orientation.w = 1.0; - marker.scale.x = parameters_.reflector_radius; - marker.scale.y = parameters_.reflector_radius; - marker.scale.z = parameters_.reflector_radius; - marker.color.a = 0.6; - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - radar_detections_marker_array.markers.push_back(marker); - - geometry_msgs::msg::Point p1, p2; - p1.z -= 0.5; - p2.z += 0.5; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.ns = "line"; - marker.scale.x = 0.2 * parameters_.reflector_radius; - marker.scale.y = 0.2 * parameters_.reflector_radius; - marker.scale.z = 0.2 * parameters_.reflector_radius; - marker.points.push_back(p1); - marker.points.push_back(p2); - radar_detections_marker_array.markers.push_back(marker); - } - - radar_detections_pub_->publish(radar_detections_marker_array); - - visualization_msgs::msg::MarkerArray matches_marker_array; - - for (std::size_t match_index = 0; match_index < matched_detections.size(); match_index++) { - const auto & [lidar_detection, radar_detection] = matched_detections[match_index]; - const auto lidar_detection_transformed = initial_radar_to_lidar_eigen_ * lidar_detection; - - visualization_msgs::msg::Marker marker; - marker.header = radar_header_; - marker.id = match_index; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.ns = "match"; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.02; - marker.scale.y = 0.02; - marker.scale.z = 0.02; - marker.color.a = 0.6; - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.points.push_back(eigenToPointMsg(lidar_detection_transformed)); - marker.points.push_back(eigenToPointMsg(radar_detection)); - matches_marker_array.markers.push_back(marker); - } - - matches_markers_pub_->publish(matches_marker_array); -} - -void ExtrinsicReflectorBasedCalibrator::visualizeTrackMarkers() -{ - auto add_track_markers = [&]( - const Eigen::Vector3d & lidar_estimation, - const Eigen::Vector3d & radar_estimation_transformed, - const std::string ns, const std_msgs::msg::ColorRGBA & color, - std::vector & markers) { - visualization_msgs::msg::Marker marker; - - marker.header = lidar_header_; - marker.id = markers.size(); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.ns = ns; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.2 * parameters_.reflector_radius; - marker.scale.y = 0.2 * parameters_.reflector_radius; - marker.scale.z = 0.2 * parameters_.reflector_radius; - marker.color = color; - marker.points.push_back(eigenToPointMsg(radar_estimation_transformed)); - marker.points.push_back(eigenToPointMsg(lidar_estimation)); - markers.push_back(marker); - - marker.id = markers.size(); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.pose.position = eigenToPointMsg(radar_estimation_transformed); - marker.pose.orientation.w = 1.0; - marker.scale.x = parameters_.reflector_radius; - marker.scale.y = parameters_.reflector_radius; - marker.scale.z = parameters_.reflector_radius; - marker.points.clear(); - markers.push_back(marker); - - marker.id = markers.size(); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.scale.x = 0.2 * parameters_.reflector_radius; - marker.scale.y = 0.2 * parameters_.reflector_radius; - marker.scale.z = 0.2 * parameters_.reflector_radius; - marker.points.push_back(eigenToPointMsg(Eigen::Vector3d(0.0, 0.0, -0.5))); - marker.points.push_back(eigenToPointMsg(Eigen::Vector3d(0.0, 0.0, 0.5))); - markers.push_back(marker); - - marker.id = markers.size(); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.pose.position = eigenToPointMsg(lidar_estimation); - marker.pose.orientation.w = 1.0; - marker.scale.x = parameters_.reflector_radius; - marker.scale.y = parameters_.reflector_radius; - marker.scale.z = parameters_.reflector_radius; - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.points.clear(); - markers.push_back(marker); + // Calculate average and standard deviation for each transformation type + auto calculate_std = [](const std::vector & data, double mean) -> double { + double sum = 0.0; + for (double value : data) { + sum += (value - mean) * (value - mean); + } + double variance = sum / data.size(); + return std::sqrt(variance); }; - // Visualization - visualization_msgs::msg::MarkerArray tracking_marker_array; - std_msgs::msg::ColorRGBA initial_color; - initial_color.r = 1.0; - initial_color.a = 1.0; - - std_msgs::msg::ColorRGBA calibrated_color; - calibrated_color.g = 1.0; - calibrated_color.a = 1.0; - - for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { - auto & track = converged_tracks_[track_index]; - const auto & lidar_estimation = track.getLidarEstimation(); - const auto & radar_estimation = track.getRadarEstimation(); + for (const auto & [type, errors] : distance_error_vectors) { + double avg_distance_error = total_distance_errors[type] / combinations.size(); + double avg_yaw_error = total_yaw_errors[type] / combinations.size(); + double std_distance_error = calculate_std(errors, avg_distance_error); + double std_yaw_error = calculate_std(yaw_error_vectors[type], avg_yaw_error); - const auto initial_radar_estimation_transformed = - initial_radar_to_lidar_eigen_.inverse() * radar_estimation; - const auto calibrated_radar_estimation_transformed = - calibrated_radar_to_lidar_eigen_.inverse() * radar_estimation; - - add_track_markers( - lidar_estimation, initial_radar_estimation_transformed, "initial", initial_color, - tracking_marker_array.markers); - add_track_markers( - lidar_estimation, calibrated_radar_estimation_transformed, "calibrated", calibrated_color, - tracking_marker_array.markers); + // Log results + RCLCPP_INFO( + this->get_logger(), + "Type: %s, Avg Distance Error: %.4fm, Avg Yaw Error: %.4f degrees, " + "Std Distance Error: %.4fm, Std Yaw Error: %.4f degrees", + toString(type).c_str(), avg_distance_error, avg_yaw_error, std_distance_error, std_yaw_error); + + // Store in output metrics + output_metrics_.methods[type].avg_crossval_calibrated_distance_errors.push_back( + avg_distance_error); + output_metrics_.methods[type].avg_crossval_calibrated_yaw_errors.push_back(avg_yaw_error); + output_metrics_.methods[type].std_crossval_calibrated_distance_errors.push_back( + std_distance_error); + output_metrics_.methods[type].std_crossval_calibrated_yaw_errors.push_back(std_yaw_error); } - tracking_markers_pub_->publish(tracking_marker_array); -} - -void ExtrinsicReflectorBasedCalibrator::deleteTrackMarkers() -{ - visualization_msgs::msg::MarkerArray tracking_marker_array; - visualization_msgs::msg::Marker marker; - auto deleted_id_start = converged_tracks_.size() * MARKER_SIZE_PER_TRACK; - // delete the latest initial marker - std::string ns = "initial"; - for (int i = 0; i < 4; i++) { - marker.id = deleted_id_start + i; - marker.ns = ns; - marker.action = visualization_msgs::msg::Marker::DELETE; - tracking_marker_array.markers.push_back(marker); - } - // delete the latest calibrated marker - ns = "calibrated"; - for (int i = 4; i < 8; i++) { - marker.id = deleted_id_start + i; - marker.ns = ns; - marker.action = visualization_msgs::msg::Marker::DELETE; - tracking_marker_array.markers.push_back(marker); - } - - tracking_markers_pub_->publish(tracking_marker_array); + // Log number of samples + output_metrics_.num_of_samples.push_back(num_of_samples); } -void ExtrinsicReflectorBasedCalibrator::drawCalibrationStatusText() +void ExtrinsicReflectorBasedCalibrator::crossValEvaluation( + TransformationResult transformation_result) { - auto to_string_with_precision = [](const float value, const int n = 2) -> std::string { - std::ostringstream out; - out.precision(n); - out << std::fixed << value; - return out.str(); - }; - - visualization_msgs::msg::Marker text_marker; - - text_marker.id = 0; - text_marker.header = lidar_header_; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - text_marker.ns = "calibration_status"; - text_marker.scale.z = 0.3; - - // show the latest cross validation results which is located in the last two elements of the - // metrics vector show the latest calibration result, which is located in the 2nd and 3rd index of - // the metrics vector - constexpr double m_to_cm = 100.0; + auto tracks_size = converged_tracks_.size(); + if (tracks_size <= 3) return; - if (converged_tracks_.size() == 0) { - text_marker.text = " pairs=" + std::to_string(converged_tracks_.size()); - } else { - text_marker.text = - " pairs=" + std::to_string(converged_tracks_.size()) + - "\n average_distance_error[cm]=" + to_string_with_precision(output_metrics_[1] * m_to_cm) + - "\n average_yaw_error[deg]=" + to_string_with_precision(output_metrics_[2]); - - if (converged_tracks_.size() > 3) { - text_marker.text += - "\n crossval_distance_error[cm]=" + - to_string_with_precision(output_metrics_[output_metrics_.size() - 4] * m_to_cm) + - "\n crossval_yaw_error[deg]=" + - to_string_with_precision(output_metrics_[output_metrics_.size() - 3]); - } + for (std::size_t num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) { + std::vector> combinations; + selectCombinations( + tracks_size, num_of_samples, parameters_.max_number_of_combination_samples, combinations); + evaluateCombinations(combinations, num_of_samples, transformation_result); } - - text_marker.pose.position.x = 1.0; - text_marker.pose.position.y = 1.0; - text_marker.pose.position.z = 1.0; - text_marker.pose.orientation.x = 0.0; - text_marker.pose.orientation.y = 0.0; - text_marker.pose.orientation.z = 0.0; - text_marker.pose.orientation.w = 1.0; - - text_markers_pub_->publish(text_marker); } -geometry_msgs::msg::Point ExtrinsicReflectorBasedCalibrator::eigenToPointMsg( - const Eigen::Vector3d & p_eigen) +void ExtrinsicReflectorBasedCalibrator::publishMetrics() { - geometry_msgs::msg::Point p; - p.x = p_eigen.x(); - p.y = p_eigen.y(); - p.z = p_eigen.z(); - return p; + // Create the message + auto msg = tier4_calibration_msgs::msg::CalibrationMetrics(); + msg.num_of_converged_tracks = output_metrics_.num_of_converged_tracks; + msg.num_of_samples = output_metrics_.num_of_samples; + msg.detections = output_metrics_.detections; + + // Loop through methods to populate metrics dynamically + for (const auto & [type, metrics] : output_metrics_.methods) { + tier4_calibration_msgs::msg::MethodMetrics method_msg; + method_msg.method_name = toString(type); // Use a function to get the string representation + method_msg.calibrated_distance_errors = metrics.calibrated_distance_errors; + method_msg.calibrated_yaw_errors = metrics.calibrated_yaw_errors; + method_msg.avg_crossval_calibrated_distance_errors = + metrics.avg_crossval_calibrated_distance_errors; + method_msg.avg_crossval_calibrated_yaw_errors = metrics.avg_crossval_calibrated_yaw_errors; + method_msg.std_crossval_calibrated_distance_errors = + metrics.std_crossval_calibrated_distance_errors; + method_msg.std_crossval_calibrated_yaw_errors = metrics.std_crossval_calibrated_yaw_errors; + + // Add the method-specific metrics to the message + msg.method_metrics.push_back(method_msg); + } + + // Publish the message + metrics_pub_->publish(msg); } -double ExtrinsicReflectorBasedCalibrator::getYawError( - const Eigen::Vector3d & v1, const Eigen::Vector3d & v2) +void ExtrinsicReflectorBasedCalibrator::calibrateSensors() { - return std::abs(std::acos(v1.dot(v2) / (v1.norm() * v2.norm()))); + output_metrics_.clear(); + if (converged_tracks_.size() == 0) { + publishMetrics(); + return; + } + TransformationResult transformation_result; + for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) { + transformation_result = estimateTransformation(track_index); + evaluateTransformation(transformation_result, track_index); + } + crossValEvaluation(transformation_result); + publishMetrics(); } } // namespace marker_radar_lidar_calibrator 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/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp new file mode 100644 index 00000000..0b389326 --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp @@ -0,0 +1,190 @@ +// 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 +#include + +namespace marker_radar_lidar_calibrator +{ + +TransformationEstimator::TransformationEstimator( + Eigen::Isometry3d initial_radar_to_lidar_eigen, + Eigen::Isometry3d initial_radar_optimization_to_radar_eigen, + Eigen::Isometry3d radar_optimization_to_lidar_eigen) +{ + initial_radar_to_lidar_eigen_ = initial_radar_to_lidar_eigen; + initial_radar_optimization_to_radar_eigen_ = initial_radar_optimization_to_radar_eigen; + radar_optimization_to_lidar_eigen_ = radar_optimization_to_lidar_eigen; +} + +void TransformationEstimator::setPoints( + pcl::PointCloud::Ptr lidar_points_ocs, + pcl::PointCloud::Ptr radar_points_rcs) +{ + lidar_points_ocs_ = lidar_points_ocs; + radar_points_rcs_ = radar_points_rcs; +} + +void TransformationEstimator::set2DRotationDelta(double delta_cos, double delta_sin) +{ + delta_cos_ = delta_cos; + delta_sin_ = delta_sin; +} + +void TransformationEstimator::estimateYawOnlyTransformation() +{ + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate yaw only 2d transformation"); + Eigen::Matrix3d delta_rotation; + delta_rotation << delta_cos_, -delta_sin_, 0.0, delta_sin_, delta_cos_, 0.0, 0.0, 0.0, 1.0; + Eigen::Isometry3d delta_transformation = Eigen::Isometry3d::Identity(); + delta_transformation.linear() = delta_rotation; + + calibrated_radar_to_lidar_transformation_ = delta_transformation * initial_radar_to_lidar_eigen_; +} + +void TransformationEstimator::estimateSVDTransformation(TransformationType transformation_type) +{ + if (transformation_type == TransformationType::svd_2d) { + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate 2D SVD transformation"); + } else { + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate 3D SVD transformation"); + } + + 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); + Eigen::Isometry3d calibrated_radar_to_radar_optimization_transformation( + full_radar_to_radar_optimization_transformation.cast()); + + if (transformation_type == TransformationType::svd_2d) { + // Check that is is actually a 2D transformation + auto calibrated_radar_to_radar_optimization_rpy = autoware::universe_utils::getRPY( + tf2::toMsg(calibrated_radar_to_radar_optimization_transformation).orientation); + double calibrated_radar_to_radar_optimization_z = + calibrated_radar_to_radar_optimization_transformation.translation().z(); + double calibrated_radar_to_radar_optimization_roll = + calibrated_radar_to_radar_optimization_rpy.x; + double calibrated_radar_to_radar_optimization_pitch = + calibrated_radar_to_radar_optimization_rpy.y; + + if ( + calibrated_radar_to_radar_optimization_z != 0.0 || + calibrated_radar_to_radar_optimization_roll != 0.0 || + calibrated_radar_to_radar_optimization_pitch != 0.0) { + RCLCPP_ERROR( + rclcpp::get_logger("marker_radar_lidar_calibrator"), + "The estimated 2D translation was not really 2D. Continue at your own risk. z=%.3f " + "roll=%.3f " + "pitch=%.3f", + calibrated_radar_to_radar_optimization_z, calibrated_radar_to_radar_optimization_roll, + calibrated_radar_to_radar_optimization_pitch); + } + + calibrated_radar_to_radar_optimization_transformation.translation().z() = + (initial_radar_to_lidar_eigen_ * radar_optimization_to_lidar_eigen_.inverse()) + .translation() + .z(); + } + + calibrated_radar_to_lidar_transformation_ = + calibrated_radar_to_radar_optimization_transformation * radar_optimization_to_lidar_eigen_; +} + +void TransformationEstimator::estimateZeroRollTransformation() +{ + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), + "Estimate the 3D transformation by restricting the roll to zero"); + + ceres::Problem problem; + + Eigen::Vector3d translation = initial_radar_optimization_to_radar_eigen_.translation(); + Eigen::Matrix3d rotation = initial_radar_optimization_to_radar_eigen_.rotation(); + Eigen::Vector3d euler_angle = rotation.eulerAngles(0, 1, 2); + + // params: x, y, z, pitch, yaw + std::vector params = { + translation[0], translation[1], translation[2], euler_angle[1], euler_angle[2]}; + + std::string initial_params_str = std::accumulate( + std::next(params.begin()), params.end(), + std::to_string(params.front()), // Initialize with the first element + [](const std::string & a, const auto & b) { return a + " " + std::to_string(b); }); + std::string initial_params_msg = "initial params (x,y,z,pith,yaw): " + initial_params_str; + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", initial_params_msg.c_str()); + + 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]; + + Eigen::Vector4d radar_point_eigen(radar_point.x, radar_point.y, radar_point.z, 1); + Eigen::Vector4d lidar_point_eigen(lidar_point.x, lidar_point.y, lidar_point.z, 1); + + ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( + new SensorResidual(radar_point_eigen, lidar_point_eigen)); + problem.AddResidualBlock(cost_function, nullptr, params.data()); + } + + // Solve + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; // cSpell:ignore schur + options.minimizer_progress_to_stdout = true; + options.max_num_iterations = 500; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + std::string report = summary.FullReport(); + RCLCPP_INFO(rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", report.c_str()); + + std::string calibrated_params_str = std::accumulate( + std::next(params.begin()), params.end(), + std::to_string(params.front()), // Initialize with the first element + [](const std::string & a, const auto & b) { return a + " " + std::to_string(b); }); + std::string calibrated_params_msg = + "calibrated params (x,y,z,pitch,yaw): " + calibrated_params_str; + RCLCPP_INFO( + rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", calibrated_params_msg.c_str()); + + Eigen::Isometry3d calibrated_3d_radar_optimization_to_radar_transformation = + Eigen::Isometry3d::Identity(); + calibrated_3d_radar_optimization_to_radar_transformation.pretranslate( + Eigen::Vector3d(params[0], params[1], params[2])); + Eigen::Quaterniond q( + Eigen::AngleAxisd(params[4], Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(params[3], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())); + calibrated_3d_radar_optimization_to_radar_transformation.rotate(q); + + calibrated_radar_to_lidar_transformation_ = + calibrated_3d_radar_optimization_to_radar_transformation.inverse() * + radar_optimization_to_lidar_eigen_; +} + +Eigen::Isometry3d TransformationEstimator::getTransformation() +{ + return calibrated_radar_to_lidar_transformation_; +} + +} // namespace marker_radar_lidar_calibrator diff --git a/calibrators/marker_radar_lidar_calibrator/src/utils.cpp b/calibrators/marker_radar_lidar_calibrator/src/utils.cpp new file mode 100644 index 00000000..80e82756 --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/src/utils.cpp @@ -0,0 +1,240 @@ +// 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 "marker_radar_lidar_calibrator/types.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace marker_radar_lidar_calibrator +{ +std::string toString(TransformationType type) +{ + switch (type) { + case TransformationType::svd_2d: + return "svd_2d"; + case TransformationType::yaw_only_rotation_2d: + return "yaw_only_rotation_2d"; + case TransformationType::svd_3d: + return "svd_3d"; + case TransformationType::zero_roll_3d: + return "zero_roll_3d"; + default: + return "unknown"; + } +} + +std::string toStringWithPrecision(const float value, const int n) +{ + std::ostringstream out; + out.precision(n); + out << std::fixed << value; + return out.str(); +} + +geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen) +{ + geometry_msgs::msg::Point p; + p.x = p_eigen.x(); + p.y = p_eigen.y(); + p.z = p_eigen.z(); + return p; +} + +void updateTrackIds(std::vector & converged_tracks) +{ + for (size_t i = 0; i < converged_tracks.size(); ++i) { + converged_tracks[i].id = i + 1; // Reassign IDs starting from 1 + } +} + +std::pair computeCalibrationError( + std::vector::iterator & begin, std::vector::iterator & end, + const TransformationType transformation_type, const Eigen::Isometry3d & radar_to_lidar_isometry, + const bool record_error_in_track) +{ + double total_distance_error = 0.0; + double total_yaw_error = 0.0; + + for (auto track = begin; track != end; ++track) { + auto lidar_estimation_transformed = radar_to_lidar_isometry * track->lidar_estimation; + + auto distance_error = + getDistanceError(transformation_type, lidar_estimation_transformed, track->radar_estimation); + auto yaw_error = getYawError(lidar_estimation_transformed, track->radar_estimation); + + if (record_error_in_track) { + track->distance_error = distance_error; + track->yaw_error = yaw_error * 180.0 / (M_PI); + } + total_distance_error += distance_error; + total_yaw_error += yaw_error; + } + + total_distance_error /= static_cast(std::distance(begin, end)); + total_yaw_error *= 180.0 / (M_PI * static_cast(std::distance(begin, end))); + + return std::make_pair(total_distance_error, total_yaw_error); +} + +double getDistanceError( + TransformationType transformation_type, Eigen::Vector3d v1, Eigen::Vector3d v2) +{ + if ( + transformation_type == TransformationType::svd_2d || + transformation_type == TransformationType::yaw_only_rotation_2d) { + v1.z() = 0.0; + v2.z() = 0.0; + } + return (v1 - v2).norm(); +} + +double getYawError(Eigen::Vector3d v1, Eigen::Vector3d v2) +{ + v1.z() = 0.0; + v2.z() = 0.0; + return std::abs(std::acos(v1.dot(v2) / (v1.norm() * v2.norm()))); +} + +size_t combination_count(const size_t n, const size_t k) +{ + if (k > n) return 0; + size_t result = 1; + for (size_t i = 0; i < k; ++i) { + result *= (n - i); + result /= (i + 1); + } + return result; +} + +void generateAllCombinations( + const std::size_t n, const std::size_t k, std::vector> & combinations) +{ + std::vector nums(n); + for (std::size_t i = 0; i < n; ++i) { + nums[i] = i; + } + + std::vector bitmask(k, true); + bitmask.resize(n, false); + + do { + std::vector selected; + for (std::size_t i = 0; i < n; ++i) { + if (bitmask[i]) selected.push_back(nums[i]); + } + combinations.push_back(selected); + } while (std::prev_permutation(bitmask.begin(), bitmask.end())); +} + +void selectCombinations( + const std::size_t n, const std::size_t k, const std::size_t max_number_of_combination_samples, + std::vector> & combinations) +{ + bool use_random_sample = (n > 20) && (k >= 5 && k <= 15); + if (!use_random_sample && max_number_of_combination_samples >= combination_count(n, k)) { + generateAllCombinations(n, k, combinations); + } else { + std::random_device rd; + std::mt19937 mt(rd()); + + std::vector nums(n); + for (std::size_t i = 0; i < n; ++i) { + nums[i] = i; + } + + std::size_t count = 0; + while (count < max_number_of_combination_samples) { + std::vector selected; + std::sample(nums.begin(), nums.end(), std::back_inserter(selected), k, mt); + combinations.push_back(selected); + count++; + } + } +} + +void parseHeader( + std::ifstream & file, const std::string & header_name, std_msgs::msg::Header & header) +{ + std::string line; + + // Parse the header section. + while (std::getline(file, line)) { + if (!line.empty()) break; // Skip blank lines. + } + + if (line != header_name) { + throw std::runtime_error("Failed to find " + header_name + " section."); + } + + if (!std::getline(file, line) || line.rfind("stamp_sec ", 0) != 0) { + throw std::runtime_error("Missing or invalid stamp_sec in " + header_name + " section."); + } + header.stamp.sec = std::stoi(line.substr(10)); + + if (!std::getline(file, line) || line.rfind("stamp_nanosec ", 0) != 0) { + throw std::runtime_error("Missing or invalid stamp_nanosec in " + header_name + " section."); + } + header.stamp.nanosec = std::stoi(line.substr(14)); + + if (!std::getline(file, line) || line.rfind("frame_id ", 0) != 0) { + throw std::runtime_error("Missing or invalid frame_id in " + header_name + " section."); + } + header.frame_id = line.substr(9); +} + +void parseConvergedTracks(std::ifstream & file, std::vector & converged_tracks) +{ + std::string line; + + // Skip the header line. + if (!std::getline(file, line)) { + throw std::runtime_error("File is empty or missing the header line."); + } + + // Parse the point cloud data. + while (std::getline(file, line) && !line.empty()) { + if (line.find("matrix:") != std::string::npos) break; // Stop if a matrix section starts. + + std::istringstream stream(line); + std::vector values; + double value; + + while (stream >> value) { // Parse values. + values.push_back(value); + } + + if (values.size() != 6) { + throw std::runtime_error("Invalid number of values in line: " + line); + } + + Track track; + track.id = converged_tracks.size() + 1; + track.lidar_estimation = Eigen::Vector3d(values[0], values[1], values[2]); + track.radar_estimation = Eigen::Vector3d(values[3], values[4], values[5]); + + converged_tracks.emplace_back(std::move(track)); + } +} + +} // 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 new file mode 100644 index 00000000..8090bfe6 --- /dev/null +++ b/calibrators/marker_radar_lidar_calibrator/src/visualization.cpp @@ -0,0 +1,293 @@ +// 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 +{ + +void Visualization::setParameters(VisualizationParameters params) { params_ = params; } + +DetectionMarkers Visualization::visualizeDetectionMarkers( + const std::vector & lidar_detections, + const std::vector & radar_detections, + const std::vector> & matched_detections) +{ + 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]; + visualization_msgs::msg::Marker marker; + marker.header = params_.lidar_header; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.id = detection_index; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = detection_center.x(); + marker.pose.position.y = detection_center.y(); + marker.pose.position.z = detection_center.z(); + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.05; + marker.scale.y = 0.05; + marker.scale.z = 0.05; + marker.color.a = 0.6; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + detection_markers.lidar_detections_marker_array.markers.push_back(marker); + } + + // Radar makers + for (std::size_t detection_index = 0; detection_index < radar_detections.size(); + detection_index++) { + const auto & detection_center = radar_detections[detection_index]; + visualization_msgs::msg::Marker marker; + marker.header = params_.radar_header; + marker.id = detection_index; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.ns = "center"; + marker.pose.position.x = detection_center.x(); + marker.pose.position.y = detection_center.y(); + marker.pose.position.z = detection_center.z(); + marker.pose.orientation.w = 1.0; + marker.scale.x = params_.reflector_radius; + marker.scale.y = params_.reflector_radius; + marker.scale.z = params_.reflector_radius; + marker.color.a = 0.6; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + detection_markers.radar_detections_marker_array.markers.push_back(marker); + + // For 2D radar detection to represent that it has no z values. + if ( + params_.transformation_type == TransformationType::svd_2d || + params_.transformation_type == TransformationType::yaw_only_rotation_2d) { + geometry_msgs::msg::Point p1, p2; + p1.z -= 0.5; + p2.z += 0.5; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.ns = "line"; + marker.scale.x = 0.2 * params_.reflector_radius; + marker.scale.y = 0.2 * params_.reflector_radius; + marker.scale.z = 0.2 * params_.reflector_radius; + marker.points.push_back(p1); + marker.points.push_back(p2); + detection_markers.radar_detections_marker_array.markers.push_back(marker); + } + } + + for (std::size_t match_index = 0; match_index < matched_detections.size(); match_index++) { + const auto & [lidar_detection, radar_detection] = matched_detections[match_index]; + const auto lidar_detection_transformed = params_.initial_radar_to_lidar_eigen * lidar_detection; + + visualization_msgs::msg::Marker marker; + marker.header = params_.radar_header; + marker.id = match_index; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.ns = "match"; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.02; + marker.scale.y = 0.02; + marker.scale.z = 0.02; + marker.color.a = 0.6; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 1.0; + marker.points.push_back(eigenToPointMsg(lidar_detection_transformed)); + marker.points.push_back(eigenToPointMsg(radar_detection)); + detection_markers.matches_marker_array.markers.push_back(marker); + } + + return detection_markers; +} + +visualization_msgs::msg::MarkerArray Visualization::visualizeTrackMarkers( + const std::vector & converged_tracks, + const Eigen::Isometry3d & calibrated_radar_to_lidar_eigen) +{ + auto add_track_markers = [&]( + const Eigen::Vector3d & lidar_estimation, + const Eigen::Vector3d & radar_estimation_transformed, Track track, + const std::string ns, const std_msgs::msg::ColorRGBA & color, + std::vector & markers) { + visualization_msgs::msg::Marker marker; + + marker.header = params_.lidar_header; + marker.id = markers.size(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.ns = ns; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.2 * params_.reflector_radius; + marker.scale.y = 0.2 * params_.reflector_radius; + marker.scale.z = 0.2 * params_.reflector_radius; + marker.color = color; + marker.points.push_back(eigenToPointMsg(radar_estimation_transformed)); + marker.points.push_back(eigenToPointMsg(lidar_estimation)); + markers.push_back(marker); + + marker.id = markers.size(); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.pose.position = eigenToPointMsg(radar_estimation_transformed); + marker.pose.orientation.w = 1.0; + marker.scale.x = params_.reflector_radius; + marker.scale.y = params_.reflector_radius; + marker.scale.z = params_.reflector_radius; + marker.points.clear(); + markers.push_back(marker); + + // For 2D radar detection to represent that it has no z values. + if ( + params_.transformation_type == TransformationType::svd_2d || + params_.transformation_type == TransformationType::yaw_only_rotation_2d) { + marker.id = markers.size(); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = 0.2 * params_.reflector_radius; + marker.scale.y = 0.2 * params_.reflector_radius; + marker.scale.z = 0.2 * params_.reflector_radius; + marker.points.push_back(eigenToPointMsg(Eigen::Vector3d(0.0, 0.0, -0.5))); + marker.points.push_back(eigenToPointMsg(Eigen::Vector3d(0.0, 0.0, 0.5))); + markers.push_back(marker); + } + + marker.id = markers.size(); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.pose.position = eigenToPointMsg(lidar_estimation); + marker.pose.orientation.w = 1.0; + marker.scale.x = params_.reflector_radius; + marker.scale.y = params_.reflector_radius; + marker.scale.z = params_.reflector_radius; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.points.clear(); + markers.push_back(marker); + + if (ns == "calibrated") { + marker.id = markers.size(); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.pose.position = eigenToPointMsg(lidar_estimation + Eigen::Vector3d(0, 0, 1)); + marker.scale.z = 0.3; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + marker.text = "\n ID=" + std::to_string(track.id) + + "\n dist_err=" + toStringWithPrecision(track.distance_error * m_to_cm, 2) + + "\n yaw_err=" + toStringWithPrecision(track.yaw_error, 2); + markers.push_back(marker); + } + }; + + // Visualization + visualization_msgs::msg::MarkerArray tracking_marker_array; + std_msgs::msg::ColorRGBA initial_color; + initial_color.r = 1.0; + initial_color.a = 1.0; + + std_msgs::msg::ColorRGBA calibrated_color; + calibrated_color.g = 1.0; + calibrated_color.a = 1.0; + + for (const auto & track : converged_tracks) { + const auto initial_radar_estimation_transformed = + params_.initial_radar_to_lidar_eigen.inverse() * track.radar_estimation; + const auto calibrated_radar_estimation_transformed = + calibrated_radar_to_lidar_eigen.inverse() * track.radar_estimation; + + add_track_markers( + track.lidar_estimation, initial_radar_estimation_transformed, track, "initial", initial_color, + tracking_marker_array.markers); + add_track_markers( + track.lidar_estimation, calibrated_radar_estimation_transformed, track, "calibrated", + calibrated_color, tracking_marker_array.markers); + } + + return tracking_marker_array; +} + +visualization_msgs::msg::MarkerArray Visualization::deleteTrackMarkers( + const size_t converged_tracks_size) +{ + visualization_msgs::msg::MarkerArray tracking_marker_array; + visualization_msgs::msg::Marker marker; + + for (size_t i = 0; i < converged_tracks_size * params_.marker_size_per_track; i++) { + marker.id = i; + marker.ns = "initial"; + marker.action = visualization_msgs::msg::Marker::DELETE; + tracking_marker_array.markers.push_back(marker); + + marker.ns = "calibrated"; + marker.action = visualization_msgs::msg::Marker::DELETE; + tracking_marker_array.markers.push_back(marker); + } + + return tracking_marker_array; +} + +visualization_msgs::msg::Marker Visualization::drawCalibrationStatusText( + const size_t converged_tracks_size, TransformationType type, CalibrationErrorMetrics metrics) +{ + visualization_msgs::msg::Marker text_marker; + + text_marker.id = 0; + text_marker.header = params_.lidar_header; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.ns = "calibration_status"; + text_marker.scale.z = 0.3; + + text_marker.text = toString(type) + "\npairs=" + std::to_string(converged_tracks_size); + if (converged_tracks_size) { + // Display average errors + text_marker.text += + "\naverage_distance_error[cm]=" + + toStringWithPrecision(metrics.calibrated_distance_errors.back() * m_to_cm, 2) + + "\naverage_yaw_error[deg]=" + toStringWithPrecision(metrics.calibrated_yaw_errors.back(), 2); + + // Display cross-validation errors + if (converged_tracks_size > 3) { + text_marker.text += + "\ncrossval_distance_error[cm]=" + + toStringWithPrecision(metrics.avg_crossval_calibrated_distance_errors.back() * m_to_cm, 2) + + "\ncrossval_yaw_error[deg]=" + + toStringWithPrecision(metrics.avg_crossval_calibrated_yaw_errors.back(), 2); + } + } + text_marker.pose.position.x = 1.0; + text_marker.pose.position.y = 1.0; + text_marker.pose.position.z = 1.0; + text_marker.pose.orientation.x = 0.0; + text_marker.pose.orientation.y = 0.0; + text_marker.pose.orientation.z = 0.0; + text_marker.pose.orientation.w = 1.0; + + return text_marker; +} + +} // namespace marker_radar_lidar_calibrator diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt index 315d433c..40ab9e7c 100644 --- a/common/tier4_calibration_msgs/CMakeLists.txt +++ b/common/tier4_calibration_msgs/CMakeLists.txt @@ -17,8 +17,12 @@ ament_auto_find_build_dependencies() rosidl_generate_interfaces(${PROJECT_NAME} "msg/CalibrationPoints.msg" "msg/CalibrationResult.msg" + "msg/CalibrationMetrics.msg" + "msg/MethodMetrics.msg" "msg/Files.msg" "srv/Empty.srv" + "srv/DeleteLidarRadarPair.srv" + "srv/FileSrv.srv" "srv/FilesSrv.srv" "srv/FilesListSrv.srv" "srv/CalibrationDatabase.srv" diff --git a/common/tier4_calibration_msgs/msg/CalibrationMetrics.msg b/common/tier4_calibration_msgs/msg/CalibrationMetrics.msg new file mode 100644 index 00000000..4c970876 --- /dev/null +++ b/common/tier4_calibration_msgs/msg/CalibrationMetrics.msg @@ -0,0 +1,4 @@ +int32 num_of_converged_tracks +int32[] num_of_samples +geometry_msgs/Point[] detections +MethodMetrics[] method_metrics diff --git a/common/tier4_calibration_msgs/msg/MethodMetrics.msg b/common/tier4_calibration_msgs/msg/MethodMetrics.msg new file mode 100644 index 00000000..af98c6da --- /dev/null +++ b/common/tier4_calibration_msgs/msg/MethodMetrics.msg @@ -0,0 +1,7 @@ +string method_name +float64[] calibrated_distance_errors +float64[] calibrated_yaw_errors +float64[] avg_crossval_calibrated_distance_errors +float64[] avg_crossval_calibrated_yaw_errors +float64[] std_crossval_calibrated_distance_errors +float64[] std_crossval_calibrated_yaw_errors diff --git a/common/tier4_calibration_msgs/srv/DeleteLidarRadarPair.srv b/common/tier4_calibration_msgs/srv/DeleteLidarRadarPair.srv new file mode 100644 index 00000000..dfbe79d9 --- /dev/null +++ b/common/tier4_calibration_msgs/srv/DeleteLidarRadarPair.srv @@ -0,0 +1,4 @@ +int32 pair_id +--- +bool success +string message diff --git a/common/tier4_calibration_msgs/srv/FileSrv.srv b/common/tier4_calibration_msgs/srv/FileSrv.srv new file mode 100644 index 00000000..8d9af9fa --- /dev/null +++ b/common/tier4_calibration_msgs/srv/FileSrv.srv @@ -0,0 +1,3 @@ +string file +--- +bool success diff --git a/docs/images/marker_radar_lidar_calibrator/add1.jpg b/docs/images/marker_radar_lidar_calibrator/add1.jpg deleted file mode 100644 index 969c9f52..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/add1.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/add2.jpg b/docs/images/marker_radar_lidar_calibrator/add2.jpg deleted file mode 100644 index d07df1d2..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/add2.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/add2.png b/docs/images/marker_radar_lidar_calibrator/add2.png new file mode 100644 index 00000000..b1c826ca Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/add2.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/add2d_1.png b/docs/images/marker_radar_lidar_calibrator/add2d_1.png new file mode 100644 index 00000000..8da56c86 Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/add2d_1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/add3d_1.png b/docs/images/marker_radar_lidar_calibrator/add3d_1.png new file mode 100644 index 00000000..b08b97dc Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/add3d_1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/delete1.jpg b/docs/images/marker_radar_lidar_calibrator/delete1.jpg deleted file mode 100644 index 2c4be6ea..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/delete1.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/delete1.png b/docs/images/marker_radar_lidar_calibrator/delete1.png new file mode 100644 index 00000000..c3a980c1 Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/delete1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/delete2.jpg b/docs/images/marker_radar_lidar_calibrator/delete2.jpg deleted file mode 100644 index 2d677cce..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/delete2.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/delete2.png b/docs/images/marker_radar_lidar_calibrator/delete2.png new file mode 100644 index 00000000..5657e3ce Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/delete2.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/end_calibration1.jpg b/docs/images/marker_radar_lidar_calibrator/end_calibration1.jpg deleted file mode 100644 index 5b957f7d..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/end_calibration1.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/end_calibration1.png b/docs/images/marker_radar_lidar_calibrator/end_calibration1.png new file mode 100644 index 00000000..96f22dce Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/end_calibration1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/end_calibration2.jpg b/docs/images/marker_radar_lidar_calibrator/end_calibration2.jpg deleted file mode 100644 index c6319aff..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/end_calibration2.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/end_calibration2.png b/docs/images/marker_radar_lidar_calibrator/end_calibration2.png new file mode 100644 index 00000000..44adbd5e Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/end_calibration2.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.jpg b/docs/images/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.jpg deleted file mode 100644 index b41e0079..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/menu2.jpg b/docs/images/marker_radar_lidar_calibrator/menu2.jpg deleted file mode 100644 index 350a9720..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/menu2.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/menu2.png b/docs/images/marker_radar_lidar_calibrator/menu2.png new file mode 100644 index 00000000..a0232d5c Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/menu2.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/metric_plotter1.jpg b/docs/images/marker_radar_lidar_calibrator/metric_plotter1.jpg deleted file mode 100644 index 6f171cfe..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/metric_plotter1.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/metric_plotter1.png b/docs/images/marker_radar_lidar_calibrator/metric_plotter1.png new file mode 100644 index 00000000..a9e0fb68 Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/metric_plotter1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz1.jpg b/docs/images/marker_radar_lidar_calibrator/rviz1.jpg deleted file mode 100644 index 55c1e079..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/rviz1.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz1.png b/docs/images/marker_radar_lidar_calibrator/rviz1.png new file mode 100644 index 00000000..9c43bbb5 Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/rviz1.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz2.jpg b/docs/images/marker_radar_lidar_calibrator/rviz2.jpg deleted file mode 100644 index 2102f2f2..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/rviz2.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz2.png b/docs/images/marker_radar_lidar_calibrator/rviz2.png new file mode 100644 index 00000000..1d97ac3a Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/rviz2.png differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz3.jpg b/docs/images/marker_radar_lidar_calibrator/rviz3.jpg deleted file mode 100644 index 90c716fd..00000000 Binary files a/docs/images/marker_radar_lidar_calibrator/rviz3.jpg and /dev/null differ diff --git a/docs/images/marker_radar_lidar_calibrator/rviz3.png b/docs/images/marker_radar_lidar_calibrator/rviz3.png new file mode 100644 index 00000000..7c250fed Binary files /dev/null and b/docs/images/marker_radar_lidar_calibrator/rviz3.png differ diff --git a/docs/tutorials/marker_radar_lidar_calibrator.md b/docs/tutorials/marker_radar_lidar_calibrator.md index a50a2ee0..0646a892 100644 --- a/docs/tutorials/marker_radar_lidar_calibrator.md +++ b/docs/tutorials/marker_radar_lidar_calibrator.md @@ -15,6 +15,8 @@ Please download the data (rosbag) from this [link](https://drive.google.com/driv The rosbag includes three different topics: `/sensing/radar/front_center/objects_raw`, `/sensing/lidar/front_lower/pointcloud_raw`, and `/tf_static`. +Please note that in this rosbag, the data recorded from the ARS408 radar does not include elevation angles for detections. To estimate the transformation between the radar and LiDAR sensors, we will use the `svd_2d` method. + ## Environment preparation ### Overall calibration environment @@ -50,7 +52,7 @@ A menu titled `Launcher configuration` should appear in the UI, and the user may For this tutorial, we modify the default value of `radar_name` from `front_left` to `front_center`. After configuring the parameters, click `Launch`.

- menu2 + menu2

The following UI should be displayed, and when the `Calibrate` button becomes available, click it to start the calibration process. @@ -67,19 +69,19 @@ Once the user starts playing the tutorial rosbag, the pointcloud should appear i Note that the user should remove the radar reflector from the calibration area before this step. If any moving object enters the calibration area at this point, the area that these objects pass through will be marked as background and thus will not be usable for calibration in later steps.

- rviz1 + rviz1

Once the user clicks the button, it will become unavailable in the UI until the process finishes. For a more detailed status of the background extraction process, the user can check the console logs.

- rviz2 + rviz2

Once the background is extracted, the `Add lidar-radar pair` button will become enabled, as shown in the following image. After this, the user can start moving radar reflectors into the calibration area.

- rviz3 + rviz3

Another way to confirm that the background model extraction finished, is to check the console output. The following text should be displayed upon completion: @@ -120,18 +122,27 @@ After the background model has been extracted, the user can carry the radar refl In the tutorial rosbag, the user will see that both the human and the radar reflector (with a tripod) are identified as foreground objects in the image below. -In the image, the colored points represent different lidar foreground clusters. The purple lines indicate radar foreground detections, appearing as lines due to the radar's lack of elevation data, making the z-axis unknown. The blue point is the estimated center of the radar reflector derived from the lidar pointcloud. There is no blue point on the human cluster because the calibrator filters out clusters where the highest point in the cluster exceeds the specified threshold. +In the left image below, the colored points represent different lidar foreground clusters. The purple lines indicate radar foreground detections, appearing as lines due to the radar's lack of elevation data, making the z-axis unknown. The blue point is the estimated center of the radar reflector derived from the lidar pointcloud. There is no blue point on the human cluster because the calibrator filters out clusters where the highest point in the cluster exceeds the specified threshold. -

- add1 -

+On the other hand, if the radar provides elevation data, the detections appear as points, as shown in the right image below. + + + + + + + + + + +
2d3d

Radar without elevation.

Radar with elevation.

When a purple line connects the purple point (the radar estimation of the reflector) and the blue point (the lidar estimation of the reflector), the user can press the `Add lidar-radar pair` button to register them as a pair. The line represents that the detections in each modality recognize each other as their best match, thus forming a valid pair. If this does not happen, the initial calibration may be too far from the real value for the pairing heuristic to succeed. Afterward, if the pair that the user added converges, it will be added to the data used for calibration. Additionally, the colors of the markers will change: the white point indicates the lidar estimation, the red point marks the initial radar estimation, and the green point signifies the calibrated radar estimation.

- add2 + add2

As described in the [Step 3: Matching and filtering](../../calibrators/marker_radar_lidar_calibrator/README.md#step-3-matching-and-filtering) in the general documentation, we rely on the initial calibration to pair each lidar detection with its closest radar detection, and vice versa. Below, we show examples of good and bad initial calibration. @@ -157,8 +168,9 @@ To test this feature, the user can click the previous button to delete the lates - - + + @@ -177,10 +189,10 @@ The console should also show the following text. This package also provides a metric plotter for real-time visualization to help users determine whether enough samples have been collected and identify potential errors in sampling or the presence of outliers.

- metric_plotter1 + metric_plotter1

-The top subplots display the cross-validation errors, while the bottom subplot shows the average errors in the calibration procedure. Plotting for the average errors begins after three pairs have been collected. For the cross-validation errors, plotting starts after four pairs have been collected. +The plots not only display the average errors and cross-validation errors for the selected algorithm but also provide metrics for an alternative method to facilitate result comparison. For example, if the user selects `svd_2d`, the plotter will display errors for both `yaw_only_rotation_2d` and `svd_2d`. Similarly, if the user selects `roll_zero_3d`, errors for both `roll_zero_3d` and `svd_3d` will be shown. Average errors are plotted after three data pairs have been collected, whereas cross-validation errors begin plotting after four pairs have been gathered. The cross-validation error is computed as follows: At any points in time, N pairs have been collected (5 in the previous example), and the cross-validation error is computed for every number of pairs K between 3 and N-1. For every value of K, all potential combinations of size K (NCK or N choose K) are computed, calibration is attempted using those K pairs, and the calibration error is computed over the remaining N - K pairs. Finally, the value of the cross-validation error at `x=K` is the averaged calibration error with the area shown representing the standard deviation of said calibration error. @@ -188,23 +200,24 @@ Note that the value of `x=N-1` represents the leave-on-out cross-validation stra The previous process is valid for both cross-validation distance and angle errors, and the users can use these values to determine when to stop the calibration process. When the cross-validation converges both in mean and has a low standard deviation, it can be considered a good point to stop calibrating. The particular criteria is left up to the user, since depending on the use cases, the required accuracy is also affected. +Additionally, the plotter displays the distribution of the range, pitch, and yaw of the detections. This helps users assess whether they have collected a sufficient variety of calibration data. + ### Sending the calibration result to the sensor calibration manager The user can click the `Send calibration` button once it is enabled. However, it is recommended to stop the calibration when the curve in the cross-validation error has converged. Therefore, in this tutorial, we run the calibration process until the bag is finished. Once the calibration has ended, the console should show similar message to the following ones: ```text -[marker_radar_lidar_calibrator]: Initial calibration error: detection2detection.distance=0.3279m yaw=1.5119 degrees -[marker_radar_lidar_calibrator]: Final calibration error: detection2detection.distance=0.0576m yaw=0.1642 degrees -[marker_radar_lidar_calibrator]: Final calibration error (rotation only): detection2detection.distance=0.0634m yaw=0.1774 degrees -[marker_radar_lidar_calibrator]: The 2D calibration pose was chosen as the output calibration pose +[marker_radar_lidar_calibrator]: Initial calibration error: detection2detection.distance=0.3254m yaw=1.4958 degrees (evaluateTransformation()) +[marker_radar_lidar_calibrator]: Type: svd_2d, distance error: 0.0614m, yaw error: 0.1638 degrees (evaluateTransformation()) +[marker_radar_lidar_calibrator]: Type: yaw_only_rotation_2d, distance error: 0.0639m, yaw error: 0.1646 degrees (evaluateTransformation()) ``` Once the `Send calibration` button is clicked, the result will be sent to the sensor calibration manager. Afterward, no pairs can be added or deleted, as shown in the image below. Please make sure you want to end the calibration process before clicking the button.
delete1delete2delete1delete2

Before deletion.

- - + + diff --git a/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml index 2393b0e7..591082ba 100644 --- a/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml +++ b/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml @@ -2,11 +2,23 @@ - + + + + + + - + + + + + + + + @@ -15,8 +27,10 @@ - + - + + + diff --git a/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml index e4034e2e..85c02f72 100644 --- a/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml +++ b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml @@ -9,13 +9,28 @@ + + + + + + + + + + + + + - + - + + + @@ -24,8 +39,10 @@ - + - + + + diff --git a/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml index 10b47c0a..80ed5000 100644 --- a/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml +++ b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml @@ -9,14 +9,29 @@ + + + + + + + + + + + + + - - - - - - + + + + + + + + @@ -27,7 +42,9 @@ - + + + @@ -42,8 +59,10 @@ - + - + + + diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py index 9a9010f9..46510ba5 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py @@ -33,11 +33,13 @@ class MarkerRadarLidarCalibrator(CalibratorBase): def __init__(self, ros_interface: RosInterface, **kwargs): super().__init__(ros_interface) - self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_optimization_frame = kwargs["radar_optimization_frame"] self.radar_frame = kwargs["radar_frame"] self.lidar_frame = kwargs["lidar_frame"] - self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + self.required_frames.extend( + [self.radar_optimization_frame, self.radar_frame, self.lidar_frame] + ) self.add_calibrator( service_name="calibrate_radar_lidar", @@ -47,15 +49,17 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - lidar_to_radar_parallel_transform = self.get_transform_matrix( - self.lidar_frame, self.radar_parallel_frame + lidar_to_radar_optimization_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_optimization_frame ) - radar_parallel_to_radar_transform = np.linalg.inv( + radar_optimization_to_radar_transform = np.linalg.inv( calibration_transforms[self.radar_frame][self.lidar_frame] - @ lidar_to_radar_parallel_transform + @ lidar_to_radar_optimization_transform ) - results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + results = { + self.radar_optimization_frame: {self.radar_frame: radar_optimization_to_radar_transform} + } return results diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py index 0c59f005..bd1968cb 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py @@ -33,11 +33,13 @@ class MarkerRadarLidarCalibrator(CalibratorBase): def __init__(self, ros_interface: RosInterface, **kwargs): super().__init__(ros_interface) - self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_optimization_frame = kwargs["radar_optimization_frame"] self.radar_frame = kwargs["radar_frame"] self.lidar_frame = kwargs["lidar_frame"] - self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + self.required_frames.extend( + [self.radar_optimization_frame, self.radar_frame, self.lidar_frame] + ) self.add_calibrator( service_name="calibrate_radar_lidar", @@ -47,15 +49,17 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - lidar_to_radar_parallel_transform = self.get_transform_matrix( - self.lidar_frame, self.radar_parallel_frame + lidar_to_radar_optimization_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_optimization_frame ) - radar_parallel_to_radar_transform = np.linalg.inv( + radar_optimization_to_radar_transform = np.linalg.inv( calibration_transforms[self.radar_frame][self.lidar_frame] - @ lidar_to_radar_parallel_transform + @ lidar_to_radar_optimization_transform ) - results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + results = { + self.radar_optimization_frame: {self.radar_frame: radar_optimization_to_radar_transform} + } return results diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py index ca0b78a7..0f304874 100644 --- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py +++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py @@ -33,11 +33,24 @@ class MarkerRadarLidarCalibrator(CalibratorBase): def __init__(self, ros_interface: RosInterface, **kwargs): super().__init__(ros_interface) - self.radar_parallel_frame = kwargs["radar_parallel_frame"] + self.radar_optimization_frame = kwargs["radar_optimization_frame"] + self.radar_parent_frame = kwargs["radar_parent_frame"] self.radar_frame = kwargs["radar_frame"] self.lidar_frame = kwargs["lidar_frame"] - self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame]) + if self.radar_optimization_frame == self.radar_parent_frame: + self.required_frames.extend( + [self.radar_optimization_frame, self.radar_frame, self.lidar_frame] + ) + else: + self.required_frames.extend( + [ + self.radar_optimization_frame, + self.radar_parent_frame, + self.radar_frame, + self.lidar_frame, + ] + ) self.add_calibrator( service_name="calibrate_radar_lidar", @@ -47,15 +60,29 @@ def __init__(self, ros_interface: RosInterface, **kwargs): ) def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]): - lidar_to_radar_parallel_transform = self.get_transform_matrix( - self.lidar_frame, self.radar_parallel_frame + lidar_to_radar_optimization_transform = self.get_transform_matrix( + self.lidar_frame, self.radar_optimization_frame ) - radar_parallel_to_radar_transform = np.linalg.inv( + radar_optimization_to_radar_transform = np.linalg.inv( calibration_transforms[self.radar_frame][self.lidar_frame] - @ lidar_to_radar_parallel_transform + @ lidar_to_radar_optimization_transform ) - results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}} + if self.radar_optimization_frame == self.radar_parent_frame: + results = { + self.radar_optimization_frame: { + self.radar_frame: radar_optimization_to_radar_transform + } + } + else: + radar_parent_to_radar_optimization_transform = self.get_transform_matrix( + self.radar_parent_frame, self.radar_optimization_frame + ) + + radar_parent_to_radar_transform = ( + radar_parent_to_radar_optimization_transform @ radar_optimization_to_radar_transform + ) + results = {self.radar_parent_frame: {self.radar_frame: radar_parent_to_radar_transform}} return results
end_calibration1end_calibration2end_calibration1end_calibration2

Rosbag finished.