fix(ndt_scan_matcher): using de-ground points calculate ndt match sco…

…re (autowarefoundation#2312)

Signed-off-by: Cynthia Liu <cynthia.liu@autocore.ai>

localization/ndt_scan_matcher/README.md

@@ -32,9 +32,11 @@ 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                                                                            |
 | `initial_pose_with_covariance`    | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching                                                                                         |
 | `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                                                                     |
 | `iteration_num`                   | `tier4_debug_msgs::msg::Int32Stamped`           | [debug topic] number of scan matching iterations                                                                                         |
 | `initial_to_result_distance`      | `tier4_debug_msgs::msg::Float32Stamped`         | [debug topic] distance difference between the initial point and the convergence point [m]                                                |
 | `initial_to_result_distance_old`  | `tier4_debug_msgs::msg::Float32Stamped`         | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] |
@@ -169,3 +171,17 @@ The color of the trajectory indicates the error (meter) from the reference traje
 - The right figure shows that the regularization suppresses the longitudinal error.
 
 <img src="./media/trajectory_without_regularization.png" alt="drawing" width="300"/> <img src="./media/trajectory_with_regularization.png" alt="drawing" width="300"/>
+
+## Scan matching score based on de-grounded LiDAR scan
+
+### Abstract
+
+This is a function that using de-grounded LiDAR scan estimate scan matching score.This score can more accurately reflect the current localization performance.
+[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) |
+| `z_margin_for_ground_removal`         | double | Z-value margin for removal ground points                                              |

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -65,3 +65,9 @@
 
     # Regularization scale factor
     regularization_scale_factor: 0.01
+
+    # A flag for using scan matching score based on de-grounded LiDAR scan
+    estimate_scores_for_degrounded_scan: false
+
+    # If lidar_point.z - base_link.z <= this threshold , the point will be removed
+    z_margin_for_ground_removal: 0.8

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -107,7 +107,7 @@ class NDTScanMatcher : public rclcpp::Node
     const bool is_converged);
   void publish_point_cloud(
     const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
-    const std::shared_ptr<const pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr);
+    const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr);
   void publish_marker(
     const rclcpp::Time & sensor_ros_time, const std::vector<geometry_msgs::msg::Pose> & pose_array);
   void publish_initial_to_result_distances(
@@ -134,6 +134,7 @@ class NDTScanMatcher : public rclcpp::Node
     regularization_pose_sub_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_aligned_pose_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr no_ground_points_aligned_pose_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ndt_pose_pub_;
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     ndt_pose_with_covariance_pub_;
@@ -143,6 +144,10 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr transform_probability_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
     nearest_voxel_transformation_likelihood_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    no_ground_transform_probability_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
+    no_ground_nearest_voxel_transformation_likelihood_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Int32Stamped>::SharedPtr iteration_num_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr
     initial_to_result_distance_pub_;
@@ -195,6 +200,9 @@ class NDTScanMatcher : public rclcpp::Node
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
   std::unique_ptr<MapModule> map_module_;
   std::unique_ptr<PoseInitializationModule> pose_init_module_;
+
+  bool estimate_scores_for_degrounded_scan_;
+  double z_margin_for_ground_removal_;
 };
 
 #endif  // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -93,7 +93,10 @@ NDTScanMatcher::NDTScanMatcher()
   initial_pose_distance_tolerance_m_(10.0),
   inversion_vector_threshold_(-0.9),
   oscillation_threshold_(10),
-  regularization_enabled_(declare_parameter("regularization_enabled", false))
+  regularization_enabled_(declare_parameter("regularization_enabled", false)),
+  estimate_scores_for_degrounded_scan_(
+    declare_parameter("estimate_scores_for_degrounded_scan", false)),
+  z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8))
 {
   (*state_ptr_)["state"] = "Initializing";
   is_activated_ = false;
@@ -174,6 +177,8 @@ NDTScanMatcher::NDTScanMatcher()
 
   sensor_aligned_pose_pub_ =
     this->create_publisher<sensor_msgs::msg::PointCloud2>("points_aligned", 10);
+  no_ground_points_aligned_pose_pub_ =
+    this->create_publisher<sensor_msgs::msg::PointCloud2>("points_aligned_no_ground", 10);
   ndt_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("ndt_pose", 10);
   ndt_pose_with_covariance_pub_ =
     this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -187,6 +192,12 @@ NDTScanMatcher::NDTScanMatcher()
   nearest_voxel_transformation_likelihood_pub_ =
     this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
       "nearest_voxel_transformation_likelihood", 10);
+  no_ground_transform_probability_pub_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
+      "no_ground_transform_probability", 10);
+  no_ground_nearest_voxel_transformation_likelihood_pub_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
+      "no_ground_nearest_voxel_transformation_likelihood", 10);
   iteration_num_pub_ =
     this->create_publisher<tier4_debug_msgs::msg::Int32Stamped>("iteration_num", 10);
   initial_to_result_distance_pub_ =
@@ -418,11 +429,42 @@ void NDTScanMatcher::callback_sensor_points(
     sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(),
     interpolator.get_new_pose());
 
-  auto sensor_points_mapTF_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
+  pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_mapTF_ptr(
+    new pcl::PointCloud<PointSource>);
   pcl::transformPointCloud(
     *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, ndt_result.pose);
   publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr);
 
+  // whether use de-grounded points calculate score
+  if (estimate_scores_for_degrounded_scan_) {
+    // remove ground
+    pcl::shared_ptr<pcl::PointCloud<PointSource>> no_ground_points_mapTF_ptr(
+      new pcl::PointCloud<PointSource>);
+    for (std::size_t i = 0; i < sensor_points_mapTF_ptr->size(); i++) {
+      if (
+        sensor_points_mapTF_ptr->points[i].z - matrix4f_to_pose(ndt_result.pose).position.z >
+        z_margin_for_ground_removal_) {
+        no_ground_points_mapTF_ptr->points.push_back(sensor_points_mapTF_ptr->points[i]);
+      }
+    }
+    // pub remove-ground points
+    sensor_msgs::msg::PointCloud2 no_ground_points_mapTF_msg;
+    pcl::toROSMsg(*no_ground_points_mapTF_ptr, no_ground_points_mapTF_msg);
+    no_ground_points_mapTF_msg.header.stamp = sensor_ros_time;
+    no_ground_points_mapTF_msg.header.frame_id = map_frame_;
+    no_ground_points_aligned_pose_pub_->publish(no_ground_points_mapTF_msg);
+    // calculate score
+    const float no_ground_transform_probability =
+      ndt_ptr_->calculateTransformationProbability(*no_ground_points_mapTF_ptr);
+    const float no_ground_nearest_voxel_transformation_likelihood =
+      ndt_ptr_->calculateNearestVoxelTransformationLikelihood(*no_ground_points_mapTF_ptr);
+    // pub score
+    no_ground_transform_probability_pub_->publish(
+      make_float32_stamped(sensor_ros_time, no_ground_transform_probability));
+    no_ground_nearest_voxel_transformation_likelihood_pub_->publish(
+      make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
+  }
+
   (*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability);
   (*state_ptr_)["nearest_voxel_transformation_likelihood"] =
     std::to_string(ndt_result.nearest_voxel_transformation_likelihood);
@@ -485,7 +527,7 @@ void NDTScanMatcher::publish_pose(
 
 void NDTScanMatcher::publish_point_cloud(
   const rclcpp::Time & sensor_ros_time, const std::string & frame_id,
-  const std::shared_ptr<const pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr)
+  const pcl::shared_ptr<pcl::PointCloud<PointSource>> & sensor_points_mapTF_ptr)
 {
   sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg;
   pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg);