feat(ndt_scan_matcher): add grid_search initial pose estimation

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -42,6 +42,12 @@
 
 
     initial_pose_estimation:
+      # method: 0=RANDOM_SEARCH(including TPE), 1=GRID_SEARCH
+      method: 1
+
+      #
+      # Parameters for RANDOM_SEARCH
+      #
       # The number of particles to estimate initial pose
       particles_num: 200
 
@@ -50,6 +56,24 @@
       # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search.
       n_startup_trials: 20
 
+      #
+      # Parameters for GRID_SEARCH
+      #
+      # The number of grid search
+      grid_num_x: 3
+      grid_num_y: 3
+      grid_num_z: 1
+      grid_num_roll: 1
+      grid_num_pitch: 1
+      grid_num_yaw: 80
+
+      # The range of grid search
+      grid_search_range_x: 3.0 # [m] (Search range: -3.0m ~ 3.0m)
+      grid_search_range_y: 3.0 # [m] (Search range: -3.0m ~ 3.0m)
+      grid_search_range_z: 0.0 # [m]
+      grid_search_range_roll: 0.0  # [rad]
+      grid_search_range_pitch: 0.0 # [rad]
+      grid_search_range_yaw: 3.14159265359 # [rad] (Search range: -pi ~ pi)
 
     validation:
       # Tolerance of timestamp difference between current time and sensor pointcloud. [sec]

localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp

@@ -23,6 +23,11 @@
 #include <string>
 #include <vector>
 
+enum class InitialPoseEstimationMethod {
+  RANDOM_SEARCH = 0,
+  GRID_SEARCH = 1,
+};
+
 enum class ConvergedParamType {
   TRANSFORM_PROBABILITY = 0,
   NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD = 1
@@ -47,8 +52,23 @@ struct HyperParameters
 
   struct InitialPoseEstimation
   {
+    InitialPoseEstimationMethod method;
+    // Parameters for RANDOM_SEARCH
     int64_t particles_num;
     int64_t n_startup_trials;
+    // Parameters for GRID_SEARCH
+    int64_t grid_num_x;
+    int64_t grid_num_y;
+    int64_t grid_num_z;
+    int64_t grid_num_roll;
+    int64_t grid_num_pitch;
+    int64_t grid_num_yaw;
+    double grid_search_range_x;
+    double grid_search_range_y;
+    double grid_search_range_z;
+    double grid_search_range_roll;
+    double grid_search_range_pitch;
+    double grid_search_range_yaw;
   } initial_pose_estimation;
 
   struct Validation
@@ -109,10 +129,38 @@ struct HyperParameters
     ndt.regularization_scale_factor =
       static_cast<float>(node->declare_parameter<float>("ndt.regularization.scale_factor"));
 
+    const int64_t initial_pose_estimation_method_tmp =
+      node->declare_parameter<int64_t>("initial_pose_estimation.method");
+    initial_pose_estimation.method =
+      static_cast<InitialPoseEstimationMethod>(initial_pose_estimation_method_tmp);
     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");
+    initial_pose_estimation.grid_num_x =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_x");
+    initial_pose_estimation.grid_num_y =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_y");
+    initial_pose_estimation.grid_num_z =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_z");
+    initial_pose_estimation.grid_num_roll =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_roll");
+    initial_pose_estimation.grid_num_pitch =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_pitch");
+    initial_pose_estimation.grid_num_yaw =
+      node->declare_parameter<int64_t>("initial_pose_estimation.grid_num_yaw");
+    initial_pose_estimation.grid_search_range_x =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_x");
+    initial_pose_estimation.grid_search_range_y =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_y");
+    initial_pose_estimation.grid_search_range_z =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_z");
+    initial_pose_estimation.grid_search_range_roll =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_roll");
+    initial_pose_estimation.grid_search_range_pitch =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_pitch");
+    initial_pose_estimation.grid_search_range_yaw =
+      node->declare_parameter<double>("initial_pose_estimation.grid_search_range_yaw");
 
     validation.lidar_topic_timeout_sec =
       node->declare_parameter<double>("validation.lidar_topic_timeout_sec");

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -92,7 +92,9 @@ class NDTScanMatcher : public rclcpp::Node
   void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
-  geometry_msgs::msg::PoseWithCovarianceStamped align_pose(
+  geometry_msgs::msg::PoseWithCovarianceStamped align_pose_by_random_search(
+    const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
+  geometry_msgs::msg::PoseWithCovarianceStamped align_pose_by_grid_search(
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
 
   void transform_sensor_measurement(

localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json

@@ -5,6 +5,12 @@
     "initial_pose_estimation": {
       "type": "object",
       "properties": {
+        "method": {
+          "type": "number",
+          "description": "The method to estimate the initial pose. 0:RANDOM_SEARCH, 1:GRID_SEARCH",
+          "default": 1,
+          "enum": [0, 1]
+        },
         "particles_num": {
           "type": "number",
           "description": "The number of particles to estimate initial pose.",
@@ -16,9 +22,97 @@
           "description": "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.",
           "default": 20,
           "minimum": 1
+        },
+        "grid_num_x": {
+          "type": "number",
+          "description": "The number of grid search in x-axis.",
+          "default": 3,
+          "minimum": 1
+        },
+        "grid_num_y": {
+          "type": "number",
+          "description": "The number of grid search in y-axis.",
+          "default": 3,
+          "minimum": 1
+        },
+        "grid_num_z": {
+          "type": "number",
+          "description": "The number of grid search in z-axis.",
+          "default": 1,
+          "minimum": 1
+        },
+        "grid_num_roll": {
+          "type": "number",
+          "description": "The number of grid search in roll-axis.",
+          "default": 1,
+          "minimum": 1
+        },
+        "grid_num_pitch": {
+          "type": "number",
+          "description": "The number of grid search in pitch-axis.",
+          "default": 1,
+          "minimum": 1
+        },
+        "grid_num_yaw": {
+          "type": "number",
+          "description": "The number of grid search in yaw-axis.",
+          "default": 80,
+          "minimum": 1
+        },
+        "grid_search_range_x": {
+          "type": "number",
+          "description": "The range of grid search in x-axis. [m]",
+          "default": 3.0,
+          "minimum": 0.0
+        },
+        "grid_search_range_y": {
+          "type": "number",
+          "description": "The range of grid search in y-axis. [m]",
+          "default": 3.0,
+          "minimum": 0.0
+        },
+        "grid_search_range_z": {
+          "type": "number",
+          "description": "The range of grid search in z-axis. [m]",
+          "default": 0.0,
+          "minimum": 0.0
+        },
+        "grid_search_range_roll": {
+          "type": "number",
+          "description": "The range of grid search in roll-axis. [rad]",
+          "default": 0.0,
+          "minimum": 0.0
+        },
+        "grid_search_range_pitch": {
+          "type": "number",
+          "description": "The range of grid search in pitch-axis. [rad]",
+          "default": 0.0,
+          "minimum": 0.0
+        },
+        "grid_search_range_yaw": {
+          "type": "number",
+          "description": "The range of grid search in yaw-axis. [rad]",
+          "default": 3.0,
+          "minimum": 3.14159265359
         }
       },
-      "required": ["particles_num", "n_startup_trials"],
+      "required": [
+        "method",
+        "particles_num",
+        "n_startup_trials",
+        "grid_num_x",
+        "grid_num_y",
+        "grid_num_z",
+        "grid_num_roll",
+        "grid_num_pitch",
+        "grid_num_yaw",
+        "grid_search_range_x",
+        "grid_search_range_y",
+        "grid_search_range_z",
+        "grid_search_range_roll",
+        "grid_search_range_pitch",
+        "grid_search_range_yaw"
+      ],
       "additionalProperties": false
     }
   }

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -880,12 +880,23 @@
     return;
   }
 
-  res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
+  if (param_.initial_pose_estimation.method == InitialPoseEstimationMethod::RANDOM_SEARCH) {
+    res->pose_with_covariance = align_pose_by_random_search(initial_pose_msg_in_map_frame);
+  } else if (param_.initial_pose_estimation.method == InitialPoseEstimationMethod::GRID_SEARCH) {
+    res->pose_with_covariance = align_pose_by_grid_search(initial_pose_msg_in_map_frame);
+  } else {
+    res->success = false;
+    RCLCPP_ERROR(
+      get_logger(),
+      "Unknown initial pose estimation method. "
+      "Please check initial_pose_estimation.method in ndt_scan_matcher.param.yaml.");
+    return;
+  }
   res->success = true;
   res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose(
+geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_random_search(
   const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov)
 {
   output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov);
@@ -1011,3 +1022,105 @@
 
   return result_pose_with_cov_msg;
 }
+
+geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose_by_grid_search(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov)
+{
+  output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov);
+
+  const geometry_msgs::msg::Point base_xyz = initial_pose_with_cov.pose.pose.position;
+  const geometry_msgs::msg::Vector3 base_rpy = get_rpy(initial_pose_with_cov);
+
+  std::vector<Particle> particle_array;
+  auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
+
+  // publish the estimated poses in 20 times to see the progress and to avoid dropping data
+  const int64_t publish_interval = 20;
+  visualization_msgs::msg::MarkerArray marker_array;
+
+  // calculate unit of grid search
+  const double unit_x = 2.0 * param_.initial_pose_estimation.grid_search_range_x /
+                        std::max(param_.initial_pose_estimation.grid_num_x - 1, (int64_t)1);
+  const double unit_y = 2.0 * param_.initial_pose_estimation.grid_search_range_y /
+                        std::max(param_.initial_pose_estimation.grid_num_y - 1, (int64_t)1);
+  const double unit_z = 2.0 * param_.initial_pose_estimation.grid_search_range_z /
+                        std::max(param_.initial_pose_estimation.grid_num_z - 1, (int64_t)1);
+  const double unit_roll = 2.0 * param_.initial_pose_estimation.grid_num_roll /
+                           std::max(param_.initial_pose_estimation.grid_num_roll - 1, (int64_t)1);
+  const double unit_pitch = 2.0 * param_.initial_pose_estimation.grid_num_pitch /
+                            std::max(param_.initial_pose_estimation.grid_num_pitch - 1, (int64_t)1);
+  const double unit_yaw = 2.0 * param_.initial_pose_estimation.grid_num_yaw /
+                          std::max(param_.initial_pose_estimation.grid_num_yaw - 1, (int64_t)1);
+
+  int64_t total_i = 0;
+  for (int64_t ix = 0; ix < param_.initial_pose_estimation.grid_num_x; ix++) {
+    for (int64_t iy = 0; iy < param_.initial_pose_estimation.grid_num_y; iy++) {
+      for (int64_t iz = 0; iz < param_.initial_pose_estimation.grid_num_z; iz++) {
+        for (int64_t i_roll = 0; i_roll < param_.initial_pose_estimation.grid_num_roll; i_roll++) {
+          for (int64_t i_pitch = 0; i_pitch < param_.initial_pose_estimation.grid_num_pitch;
+               i_pitch++) {
+            for (int64_t i_yaw = 0; i_yaw < param_.initial_pose_estimation.grid_num_yaw; i_yaw++) {
+              geometry_msgs::msg::Pose initial_pose;
+              initial_pose.position.x =
+                base_xyz.x - param_.initial_pose_estimation.grid_search_range_x + ix * unit_x;
+              initial_pose.position.y =
+                base_xyz.y - param_.initial_pose_estimation.grid_search_range_y + iy * unit_y;
+              initial_pose.position.z =
+                base_xyz.z - param_.initial_pose_estimation.grid_search_range_z + iz * unit_z;
+              geometry_msgs::msg::Vector3 init_rpy;
+              init_rpy.x = base_rpy.x - param_.initial_pose_estimation.grid_search_range_roll +
+                           i_roll * unit_roll;
+              init_rpy.y = base_rpy.y - param_.initial_pose_estimation.grid_search_range_pitch +
+                           i_pitch * unit_pitch;
+              init_rpy.z = base_rpy.z - param_.initial_pose_estimation.grid_search_range_yaw +
+                           i_yaw * unit_yaw;
+              tf2::Quaternion tf_quaternion;
+              tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z);
+              initial_pose.orientation = tf2::toMsg(tf_quaternion);
+
+              const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose);
+              ndt_ptr_->align(*output_cloud, initial_pose_matrix);
+              const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
+
+              const Particle particle(
+                initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability,
+                ndt_result.iteration_num);
+              particle_array.push_back(particle);
+              push_debug_markers(
+                marker_array, get_clock()->now(), param_.frame.map_frame, particle, total_i);
+              if (
+                (total_i + 1) % publish_interval == 0 ||
+                (total_i + 1) == param_.initial_pose_estimation.particles_num) {
+                ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array);
+                marker_array.markers.clear();
+              }
+
+              auto sensor_points_in_map_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
+              tier4_autoware_utils::transformPointCloud(
+                *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose);
+              publish_point_cloud(
+                initial_pose_with_cov.header.stamp, param_.frame.map_frame,
+                sensor_points_in_map_ptr);
+
+              total_i++;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  const auto best_particle_ptr = std::max_element(
+    std::begin(particle_array), std::end(particle_array),
+    [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; });
+
+  geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg;
+  result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp;
+  result_pose_with_cov_msg.header.frame_id = param_.frame.map_frame;
+  result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
+
+  output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg);
+  RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score);
+
+  return result_pose_with_cov_msg;
+}