From 868601304b3d6f31dc2ade3ac352628bb87b56bc Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 26 Jan 2024 16:38:48 +0900 Subject: [PATCH] refactor(ndt_scan_matcher): rename de-grounded Signed-off-by: Yamato Ando --- localization/ndt_scan_matcher/README.md | 12 +++++------- .../config/ndt_scan_matcher.param.yaml | 5 ++--- .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 3 +-- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 9 ++++----- 4 files changed, 12 insertions(+), 17 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 95a7cebdc01c8..ca5ea45a082e1 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -31,13 +31,13 @@ One optional function is regularization. Please see the regularization chapter i | `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | | `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | | `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | +| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | | `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | @@ -228,20 +228,18 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample | single file | at once (standard) | | multiple files | dynamically | -## Scan matching score based on de-grounded LiDAR scan +## Scan matching score based on no ground LiDAR scan ### Abstract -This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. +This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters - - | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | -| `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | +| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground LiDAR scan (FALSE by default) | | `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | ## 2D real-time covariance estimation diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5a4941bfec62..d4c49b7e8eec5 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -95,9 +95,8 @@ # Radius of input LiDAR range (used for diagnostics of dynamic map loading) lidar_radius: 100.0 - # cspell: ignore degrounded - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false + # A flag for using scan matching score based on no ground LiDAR scan + estimate_scores_by_no_ground_points: false # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index e2503c20aef6e..2883aa761444d 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -220,8 +220,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - // cspell: ignore degrounded - bool estimate_scores_for_degrounded_scan_; + bool estimate_scores_by_no_ground_points_; double z_margin_for_ground_removal_; // The execution time which means probably NDT cannot matches scans properly diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 0382d805b7276..0278a62341981 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,7 +64,6 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), @@ -160,8 +159,8 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("initial_estimate_particles_num"); n_startup_trials_ = this->declare_parameter("n_startup_trials"); - estimate_scores_for_degrounded_scan_ = - this->declare_parameter("estimate_scores_for_degrounded_scan"); + estimate_scores_by_no_ground_points_ = + this->declare_parameter("estimate_scores_by_no_ground_points"); z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); @@ -526,8 +525,8 @@ void NDTScanMatcher::callback_sensor_points( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); - // whether use de-grounded points calculate score - if (estimate_scores_for_degrounded_scan_) { + // whether use no ground points to calculate score + if (estimate_scores_by_no_ground_points_) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud);