refactor(ndt_scan_matcher): delete default parameters from ndt_scan_matcher_core.cpp

localization/ndt_scan_matcher/README.md

@@ -54,19 +54,25 @@ One optional function is regularization. Please see the regularization chapter i
 
 ### Core Parameters
 
-| Name                                    | Type   | Description                                                                                     |
-| --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- |
-| `base_frame`                            | string | Vehicle reference frame                                                                         |
-| `ndt_base_frame`                        | string | NDT reference frame                                                                             |
-| `input_sensor_points_queue_size`        | int    | Subscriber queue size                                                                           |
-| `trans_epsilon`                         | double | The maximum difference between two consecutive transformations in order to consider convergence |
-| `step_size`                             | double | The newton line search maximum step length                                                      |
-| `resolution`                            | double | The ND voxel grid resolution [m]                                                                |
-| `max_iterations`                        | int    | The number of iterations required to calculate alignment                                        |
-| `converged_param_type`                  | int    | The type of indicators for scan matching score (0: TP, 1: NVTL)                                 |
-| `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result                                   |
-| `lidar_topic_timeout_sec`               | double | Tolerance of timestamp difference between current time and sensor pointcloud                    |
-| `num_threads`                           | int    | Number of threads used for parallel computing                                                   |
+| Name                                                      | Type                   | Description                                                                                        |
+| --------------------------------------------------------- | ---------------------- | -------------------------------------------------------------------------------------------------- |
+| `base_frame`                                              | string                 | Vehicle reference frame                                                                            |
+| `ndt_base_frame`                                          | string                 | NDT reference frame                                                                                |
+| `map_frame`                                               | string                 | map frame                                                                                          |
+| `input_sensor_points_queue_size`                          | int                    | Subscriber queue size                                                                              |
+| `trans_epsilon`                                           | double                 | The max difference between two consecutive transformations to consider convergence                 |
+| `step_size`                                               | double                 | The newton line search maximum step length                                                         |
+| `resolution`                                              | double                 | The ND voxel grid resolution [m]                                                                   |
+| `max_iterations`                                          | int                    | The number of iterations required to calculate alignment                                           |
+| `converged_param_type`                                    | int                    | The type of indicators for scan matching score (0: TP, 1: NVTL)                                    |
+| `converged_param_transform_probability`                   | double                 | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0)   |
+| `converged_param_nearest_voxel_transformation_likelihood` | double                 | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) |
+| `initial_estimate_particles_num`                          | int                    | The number of particles to estimate initial pose                                                   |
+| `lidar_topic_timeout_sec`                                 | double                 | Tolerance of timestamp difference between current time and sensor pointcloud                       |
+| `initial_pose_timeout_sec`                                | int                    | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]                |
+| `initial_pose_distance_tolerance_m`                       | double                 | Tolerance of distance difference between two initial poses used for linear interpolation. [m]      |
+| `num_threads`                                             | int                    | Number of threads used for parallel computing                                                      |
+| `output_pose_covariance`                                  | std::array<double, 36> | The covariance of output pose                                                                      |
 
 (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)
 
@@ -238,7 +244,7 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample
 
 ### 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.
+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.
 [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044).
 
 ### Parameters

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -9,6 +9,9 @@
     # NDT reference frame
     ndt_base_frame: "ndt_base_link"
 
+    # map frame
+    map_frame: "map"
+
     # Subscriber queue size
     input_sensor_points_queue_size: 1
 
@@ -53,7 +56,7 @@
     num_threads: 4
 
     # The covariance of output pose
-    # Do note that this covariance matrix is empirically derived
+    # Note that this covariance matrix is empirically derived
     output_pose_covariance:
       [
         0.0225, 0.0,   0.0,   0.0,      0.0,      0.0,
@@ -67,7 +70,7 @@
     # Regularization switch
     regularization_enabled: false
 
-    # regularization scale factor
+    # Regularization scale factor
     regularization_scale_factor: 0.01
 
     # Dynamic map loading distance
@@ -86,5 +89,5 @@
     # If lidar_point.z - base_link.z <= this threshold , the point will be removed
     z_margin_for_ground_removal: 0.8
 
-    # The execution time which means probably NDT cannot matches scans properly
-    critical_upper_bound_exe_time_ms: 100 # [ms]
+    # The execution time which means probably NDT cannot matches scans properly. [ms]
+    critical_upper_bound_exe_time_ms: 100

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -84,38 +84,26 @@
   tf2_broadcaster_(*this),
   ndt_ptr_(new NormalDistributionsTransform),
   state_ptr_(new std::map<std::string, std::string>),
-  base_frame_("base_link"),
-  ndt_base_frame_("ndt_base_link"),
-  map_frame_("map"),
-  converged_param_type_(ConvergedParamType::TRANSFORM_PROBABILITY),
-  converged_param_transform_probability_(4.5),
-  converged_param_nearest_voxel_transformation_likelihood_(2.3),
-  initial_estimate_particles_num_(100),
-  lidar_topic_timeout_sec_(1.0),
-  initial_pose_timeout_sec_(1.0),
-  initial_pose_distance_tolerance_m_(10.0),
-  inversion_vector_threshold_(-0.9),
-  oscillation_threshold_(10),
-  output_pose_covariance_(),
-  regularization_enabled_(declare_parameter<bool>("regularization_enabled")),
-  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)),
-  critical_upper_bound_exe_time_ms_(100)
+  inversion_vector_threshold_(-0.9),  // Not necessary to extract to ndt_scan_matcher.param.yaml
+  oscillation_threshold_(10),         // Not necessary to extract to ndt_scan_matcher.param.yaml
+  regularization_enabled_(declare_parameter<bool>("regularization_enabled"))
 {
   (*state_ptr_)["state"] = "Initializing";
   is_activated_ = false;
 
   int points_queue_size = this->declare_parameter<int>("input_sensor_points_queue_size");
   points_queue_size = std::max(points_queue_size, 0);
   RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size);
 
   base_frame_ = this->declare_parameter<std::string>("base_frame");
   RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str());
 
   ndt_base_frame_ = this->declare_parameter<std::string>("ndt_base_frame");
   RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str());
 
+  map_frame_ = this->declare_parameter<std::string>("map_frame");
+  RCLCPP_INFO(get_logger(), "map_frame_id: %s", map_frame_.c_str());
+
   pclomp::NdtParams ndt_params{};
   ndt_params.trans_epsilon = this->declare_parameter<double>("trans_epsilon");
   ndt_params.step_size = this->declare_parameter<double>("step_size");
@@ -141,8 +129,9 @@
     this->declare_parameter<double>("converged_param_nearest_voxel_transformation_likelihood");
 
   lidar_topic_timeout_sec_ = this->declare_parameter<double>("lidar_topic_timeout_sec");
+
   critical_upper_bound_exe_time_ms_ =
-    this->declare_parameter("critical_upper_bound_exe_time_ms", critical_upper_bound_exe_time_ms_);
+    this->declare_parameter<int>("critical_upper_bound_exe_time_ms");
 
   initial_pose_timeout_sec_ = this->declare_parameter<double>("initial_pose_timeout_sec");
 
@@ -157,6 +146,11 @@
 
   initial_estimate_particles_num_ = this->declare_parameter<int>("initial_estimate_particles_num");
 
+  estimate_scores_for_degrounded_scan_ =
+    this->declare_parameter<bool>("estimate_scores_for_degrounded_scan");
+
+  z_margin_for_ground_removal_ = this->declare_parameter<double>("z_margin_for_ground_removal");
+
   rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group;
   initial_pose_callback_group =
     this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);