refactor(ndt_scan_matcher): hierarchize parameters (autowarefoundatio…

…n#6208)

* refactor(ndt_scan_matcher): hierarchize parameters

Signed-off-by: Yamato Ando <[email protected]>

* style(pre-commit): autofix

* add new lines

Signed-off-by: Yamato Ando <[email protected]>

* fix typo

Signed-off-by: Yamato Ando <[email protected]>

---------

Signed-off-by: Yamato Ando <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -1,102 +1,114 @@
 /**:
   ros__parameters:
-    # Vehicle reference frame
-    base_frame: "base_link"
+    frame:
+      # Vehicle reference frame
+      base_frame: "base_link"
 
-    # NDT reference frame
-    ndt_base_frame: "ndt_base_link"
+      # NDT reference frame
+      ndt_base_frame: "ndt_base_link"
 
-    # map frame
-    map_frame: "map"
+      # Map frame
+      map_frame: "map"
 
-    # Subscriber queue size
-    input_sensor_points_queue_size: 1
 
-    # The maximum difference between two consecutive
-    # transformations in order to consider convergence
-    trans_epsilon: 0.01
+    ndt:
+      # The maximum difference between two consecutive
+      # transformations in order to consider convergence
+      trans_epsilon: 0.01
 
-    # The newton line search maximum step length
-    step_size: 0.1
+      # The newton line search maximum step length
+      step_size: 0.1
 
-    # The ND voxel grid resolution
-    resolution: 2.0
+      # The ND voxel grid resolution
+      resolution: 2.0
 
-    # The number of iterations required to calculate alignment
-    max_iterations: 30
+      # The number of iterations required to calculate alignment
+      max_iterations: 30
 
-    # Converged param type
-    # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
-    converged_param_type: 1
+      # Number of threads used for parallel computing
+      num_threads: 4
 
-    # If converged_param_type is 0
-    # Threshold for deciding whether to trust the estimation result
-    converged_param_transform_probability: 3.0
+      regularization:
+        enable: false
 
-    # If converged_param_type is 1
-    # Threshold for deciding whether to trust the estimation result
-    converged_param_nearest_voxel_transformation_likelihood: 2.3
+        # Regularization scale factor
+        scale_factor: 0.01
 
-    # The number of particles to estimate initial pose
-    initial_estimate_particles_num: 200
 
-    # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator).
-    # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0.
-    # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
-    n_startup_trials: 20
+    initial_pose_estimation:
+      # The number of particles to estimate initial pose
+      particles_num: 200
 
-    # Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
-    lidar_topic_timeout_sec: 1.0
+      # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator).
+      # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0.
+      # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
+      n_startup_trials: 20
 
-    # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
-    initial_pose_timeout_sec: 1.0
 
-    # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
-    initial_pose_distance_tolerance_m: 10.0
+    validation:
+      # Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
+      lidar_topic_timeout_sec: 1.0
 
-    # The execution time which means probably NDT cannot matches scans properly. [ms]
-    critical_upper_bound_exe_time_ms: 100.0
+      # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
+      initial_pose_timeout_sec: 1.0
 
-    # Number of threads used for parallel computing
-    num_threads: 4
+      # Tolerance of distance difference between two initial poses used for linear interpolation. [m]
+      initial_pose_distance_tolerance_m: 10.0
 
-    # The covariance of output pose
-    # Note that this covariance matrix is empirically derived
-    output_pose_covariance:
-      [
-        0.0225, 0.0,   0.0,   0.0,      0.0,      0.0,
-        0.0,   0.0225, 0.0,   0.0,      0.0,      0.0,
-        0.0,   0.0,   0.0225, 0.0,      0.0,      0.0,
-        0.0,   0.0,   0.0,   0.000625, 0.0,      0.0,
-        0.0,   0.0,   0.0,   0.0,      0.000625, 0.0,
-        0.0,   0.0,   0.0,   0.0,      0.0,      0.000625,
-      ]
+      # The execution time which means probably NDT cannot matches scans properly. [ms]
+      critical_upper_bound_exe_time_ms: 100.0
 
-    # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
-    use_covariance_estimation: false
 
-    # Offset arrangement in covariance estimation [m]
-    # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
-    initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
-    initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
+    score_estimation:
+      # Converged param type
+      # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD
+      converged_param_type: 1
 
-    # Regularization switch
-    regularization_enabled: false
+      # If converged_param_type is 0
+      # Threshold for deciding whether to trust the estimation result
+      converged_param_transform_probability: 3.0
 
-    # Regularization scale factor
-    regularization_scale_factor: 0.01
+      # If converged_param_type is 1
+      # Threshold for deciding whether to trust the estimation result
+      converged_param_nearest_voxel_transformation_likelihood: 2.3
 
-    # Dynamic map loading distance
-    dynamic_map_loading_update_distance: 20.0
+      # Scan matching score based on no ground LiDAR scan
+      no_ground_points:
+        enable: false
 
-    # Dynamic map loading loading radius
-    dynamic_map_loading_map_radius: 150.0
+        # If lidar_point.z - base_link.z <= this threshold , the point will be removed
+        z_margin_for_ground_removal: 0.8
 
-    # Radius of input LiDAR range (used for diagnostics of dynamic map loading)
-    lidar_radius: 100.0
 
-    # A flag for using scan matching score based on no ground LiDAR scan
-    estimate_scores_by_no_ground_points: false
+    covariance:
+      # The covariance of output pose
+      # Note that this covariance matrix is empirically derived
+      output_pose_covariance:
+        [
+          0.0225, 0.0,   0.0,   0.0,      0.0,      0.0,
+          0.0,   0.0225, 0.0,   0.0,      0.0,      0.0,
+          0.0,   0.0,   0.0225, 0.0,      0.0,      0.0,
+          0.0,   0.0,   0.0,   0.000625, 0.0,      0.0,
+          0.0,   0.0,   0.0,   0.0,      0.000625, 0.0,
+          0.0,   0.0,   0.0,   0.0,      0.0,      0.000625,
+        ]
 
-    # If lidar_point.z - base_link.z <= this threshold , the point will be removed
-    z_margin_for_ground_removal: 0.8
+      # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value)
+      covariance_estimation:
+        enable: false
+
+        # Offset arrangement in covariance estimation [m]
+        # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements.
+        initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0]
+        initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0]
+
+
+    dynamic_map_loading:
+      # Dynamic map loading distance
+      update_distance: 20.0
+
+      # Dynamic map loading loading radius
+      map_radius: 150.0
+
+      # Radius of input LiDAR range (used for diagnostics of dynamic map loading)
+      lidar_radius: 100.0

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

@@ -0,0 +1,173 @@
+// Copyright 2024 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use node 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 NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
+#define NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <multigrid_pclomp/multigrid_ndt_omp.h>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+enum class ConvergedParamType {
+  TRANSFORM_PROBABILITY = 0,
+  NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
+};
+
+struct HyperParameters
+{
+  struct Frame
+  {
+    std::string base_frame;
+    std::string ndt_base_frame;
+    std::string map_frame;
+  } frame;
+
+  pclomp::NdtParams ndt;
+  bool ndt_regularization_enable;
+
+  struct InitialPoseEstimation
+  {
+    int64_t particles_num;
+    int64_t n_startup_trials;
+  } initial_pose_estimation;
+
+  struct Validation
+  {
+    double lidar_topic_timeout_sec;
+    double initial_pose_timeout_sec;
+    double initial_pose_distance_tolerance_m;
+    double critical_upper_bound_exe_time_ms;
+  } validation;
+
+  struct ScoreEstimation
+  {
+    ConvergedParamType converged_param_type;
+    double converged_param_transform_probability;
+    double converged_param_nearest_voxel_transformation_likelihood;
+    struct NoGroundPoints
+    {
+      bool enable;
+      double z_margin_for_ground_removal;
+    } no_ground_points;
+  } score_estimation;
+
+  struct Covariance
+  {
+    std::array<double, 36> output_pose_covariance;
+
+    struct CovarianceEstimation
+    {
+      bool enable;
+      std::vector<Eigen::Vector2d> initial_pose_offset_model;
+    } covariance_estimation;
+  } covariance;
+
+  struct DynamicMapLoading
+  {
+    double update_distance;
+    double map_radius;
+    double lidar_radius;
+  } dynamic_map_loading;
+
+public:
+  explicit HyperParameters(rclcpp::Node * node)
+  {
+    frame.base_frame = node->declare_parameter<std::string>("frame.base_frame");
+    frame.ndt_base_frame = node->declare_parameter<std::string>("frame.ndt_base_frame");
+    frame.map_frame = node->declare_parameter<std::string>("frame.map_frame");
+
+    ndt.trans_epsilon = node->declare_parameter<double>("ndt.trans_epsilon");
+    ndt.step_size = node->declare_parameter<double>("ndt.step_size");
+    ndt.resolution = node->declare_parameter<double>("ndt.resolution");
+    ndt.max_iterations = static_cast<int>(node->declare_parameter<int64_t>("ndt.max_iterations"));
+    ndt.num_threads = static_cast<int>(node->declare_parameter<int64_t>("ndt.num_threads"));
+    ndt.num_threads = std::max(ndt.num_threads, 1);
+    ndt_regularization_enable = node->declare_parameter<bool>("ndt.regularization.enable");
+    ndt.regularization_scale_factor =
+      static_cast<float>(node->declare_parameter<float>("ndt.regularization.scale_factor"));
+
+    initial_pose_estimation.particles_num =
+      node->declare_parameter<int64_t>("initial_pose_estimation.particles_num");
+    initial_pose_estimation.n_startup_trials =
+      node->declare_parameter<int64_t>("initial_pose_estimation.n_startup_trials");
+
+    validation.lidar_topic_timeout_sec =
+      node->declare_parameter<double>("validation.lidar_topic_timeout_sec");
+    validation.initial_pose_timeout_sec =
+      node->declare_parameter<double>("validation.initial_pose_timeout_sec");
+    validation.initial_pose_distance_tolerance_m =
+      node->declare_parameter<double>("validation.initial_pose_distance_tolerance_m");
+    validation.critical_upper_bound_exe_time_ms =
+      node->declare_parameter<double>("validation.critical_upper_bound_exe_time_ms");
+
+    const int64_t converged_param_type_tmp =
+      node->declare_parameter<int64_t>("score_estimation.converged_param_type");
+    score_estimation.converged_param_type =
+      static_cast<ConvergedParamType>(converged_param_type_tmp);
+    score_estimation.converged_param_transform_probability =
+      node->declare_parameter<double>("score_estimation.converged_param_transform_probability");
+    score_estimation.converged_param_nearest_voxel_transformation_likelihood =
+      node->declare_parameter<double>(
+        "score_estimation.converged_param_nearest_voxel_transformation_likelihood");
+    score_estimation.no_ground_points.enable =
+      node->declare_parameter<bool>("score_estimation.no_ground_points.enable");
+    score_estimation.no_ground_points.z_margin_for_ground_removal = node->declare_parameter<double>(
+      "score_estimation.no_ground_points.z_margin_for_ground_removal");
+
+    std::vector<double> output_pose_covariance =
+      node->declare_parameter<std::vector<double>>("covariance.output_pose_covariance");
+    for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) {
+      covariance.output_pose_covariance[i] = output_pose_covariance[i];
+    }
+    covariance.covariance_estimation.enable =
+      node->declare_parameter<bool>("covariance.covariance_estimation.enable");
+    if (covariance.covariance_estimation.enable) {
+      std::vector<double> initial_pose_offset_model_x =
+        node->declare_parameter<std::vector<double>>(
+          "covariance.covariance_estimation.initial_pose_offset_model_x");
+      std::vector<double> initial_pose_offset_model_y =
+        node->declare_parameter<std::vector<double>>(
+          "covariance.covariance_estimation.initial_pose_offset_model_y");
+
+      if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) {
+        const size_t size = initial_pose_offset_model_x.size();
+        covariance.covariance_estimation.initial_pose_offset_model.resize(size);
+        for (size_t i = 0; i < size; i++) {
+          covariance.covariance_estimation.initial_pose_offset_model[i].x() =
+            initial_pose_offset_model_x[i];
+          covariance.covariance_estimation.initial_pose_offset_model[i].y() =
+            initial_pose_offset_model_y[i];
+        }
+      } else {
+        RCLCPP_WARN(
+          node->get_logger(),
+          "Invalid initial pose offset model parameters. Disable covariance estimation.");
+        covariance.covariance_estimation.enable = false;
+      }
+    }
+
+    dynamic_map_loading.update_distance =
+      node->declare_parameter<double>("dynamic_map_loading.update_distance");
+    dynamic_map_loading.map_radius =
+      node->declare_parameter<double>("dynamic_map_loading.map_radius");
+    dynamic_map_loading.lidar_radius =
+      node->declare_parameter<double>("dynamic_map_loading.lidar_radius");
+  }
+};
+
+#endif  // NDT_SCAN_MATCHER__HYPER_PARAMETERS_HPP_

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -16,6 +16,7 @@
 #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_
 
 #include "localization_util/util_func.hpp"
+#include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/particle.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -47,7 +48,8 @@ class MapUpdateModule
 public:
   MapUpdateModule(
     rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
-    std::shared_ptr<NormalDistributionsTransform> ndt_ptr);
+    std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
+    HyperParameters::DynamicMapLoading param);
 
 private:
   friend class NDTScanMatcher;
@@ -70,9 +72,8 @@ class MapUpdateModule
   rclcpp::Clock::SharedPtr clock_;
 
   std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
-  const double dynamic_map_loading_update_distance_;
-  const double dynamic_map_loading_map_radius_;
-  const double lidar_radius_;
+
+  HyperParameters::DynamicMapLoading param_;
 };
 
 #endif  // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_