fix(ndt_scan_matcher): improved tpe

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -48,7 +48,7 @@
       # 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
+      n_startup_trials: 100
 
 
     validation:

localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json

@@ -14,7 +14,7 @@
         "n_startup_trials": {
           "type": "number",
           "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,
+          "default": 100,
           "minimum": 1
         }
       },

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -22,8 +22,6 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/transform/transforms.hpp>
 
-#include <boost/math/special_functions/erf.hpp>
-
 #include <pcl_conversions/pcl_conversions.h>
 
 #ifdef ROS_DISTRO_GALACTIC
@@ -988,95 +986,69 @@
   const double stddev_roll = std::sqrt(covariance(3, 3));
   const double stddev_pitch = std::sqrt(covariance(4, 4));
 
-  // Let phi be the cumulative distribution function of the standard normal distribution.
-  // It has the following relationship with the error function (erf).
-  //   phi(x) = 1/2 (1 + erf(x / sqrt(2)))
-  // so, 2 * phi(x) - 1 = erf(x / sqrt(2)).
-  // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in
-  // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good
-  // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps).
-  const double sqrt2 = std::sqrt(2);
-  auto uniform_to_normal = [&sqrt2](const double uniform) {
-    assert(-1.0 <= uniform && uniform <= 1.0);
-    constexpr double epsilon = 1.0e-6;
-    const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon);
-    return boost::math::erf_inv(clamped) * sqrt2;
-  };
-
-  auto normal_to_uniform = [&sqrt2](const double normal) {
-    return boost::math::erf(normal / sqrt2);
+  // Since only yaw is uniformly sampled, we define the mean and standard deviation for the others.
+  const std::vector<double> sample_mean{
+    initial_pose_with_cov.pose.pose.position.x,  // trans_x
+    initial_pose_with_cov.pose.pose.position.y,  // trans_y
+    initial_pose_with_cov.pose.pose.position.z,  // trans_z
+    base_rpy.x,                                  // angle_x
+    base_rpy.y                                   // angle_y
   };
+  const std::vector<double> sample_stddev{stddev_x, stddev_y, stddev_z, stddev_roll, stddev_pitch};
 
   // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions.
-  // The last dimension (yaw) is a loop variable.
-  // Although roll and pitch are also angles, they are considered non-looping variables that follow
-  // a normal distribution with a small standard deviation. This assumes that the initial pose of
-  // the ego vehicle is aligned with the ground to some extent about roll and pitch.
-  const std::vector<bool> is_loop_variable = {false, false, false, false, false, true};
   TreeStructuredParzenEstimator tpe(
     TreeStructuredParzenEstimator::Direction::MAXIMIZE,
-    param_.initial_pose_estimation.n_startup_trials, is_loop_variable);
+    param_.initial_pose_estimation.n_startup_trials, sample_mean, sample_stddev);
 
   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
   visualization_msgs::msg::MarkerArray marker_array;
   constexpr int64_t publish_num = 20;
   const int64_t publish_interval = param_.initial_pose_estimation.particles_num / publish_num;
 
   for (int64_t i = 0; i < param_.initial_pose_estimation.particles_num; i++) {
     const TreeStructuredParzenEstimator::Input input = tpe.get_next_input();
 
     geometry_msgs::msg::Pose initial_pose;
-    initial_pose.position.x =
-      initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x;
-    initial_pose.position.y =
-      initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y;
-    initial_pose.position.z =
-      initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z;
+    initial_pose.position.x = input[0];
+    initial_pose.position.y = input[1];
+    initial_pose.position.z = input[2];
     geometry_msgs::msg::Vector3 init_rpy;
-    init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll;
-    init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch;
-    init_rpy.z = base_rpy.z + input[5] * M_PI;
+    init_rpy.x = input[3];
+    init_rpy.y = input[4];
+    init_rpy.z = input[5];
     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();
 
     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, i);
     if (
       (i + 1) % publish_interval == 0 || (i + 1) == param_.initial_pose_estimation.particles_num) {
       ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array);
       marker_array.markers.clear();
     }
 
     const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose);
     const geometry_msgs::msg::Vector3 rpy = get_rpy(pose);
 
-    const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x;
-    const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y;
-    const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z;
-    const double diff_roll = rpy.x - base_rpy.x;
-    const double diff_pitch = rpy.y - base_rpy.y;
-    const double diff_yaw = rpy.z - base_rpy.z;
-
-    // Only yaw is a loop_variable, so only simple normalization is performed.
-    // All other variables are converted from normal distribution to uniform distribution.
-    TreeStructuredParzenEstimator::Input result(is_loop_variable.size());
-    result[0] = normal_to_uniform(diff_x / stddev_x);
-    result[1] = normal_to_uniform(diff_y / stddev_y);
-    result[2] = normal_to_uniform(diff_z / stddev_z);
-    result[3] = normal_to_uniform(diff_roll / stddev_roll);
-    result[4] = normal_to_uniform(diff_pitch / stddev_pitch);
-    result[5] = diff_yaw / M_PI;
+    TreeStructuredParzenEstimator::Input result(6);
+    result[0] = pose.position.x;
+    result[1] = pose.position.y;
+    result[2] = pose.position.z;
+    result[3] = rpy.x;
+    result[4] = rpy.y;
+    result[5] = rpy.z;
     tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability});
 
     auto sensor_points_in_map_ptr = std::make_shared<pcl::PointCloud<PointSource>>();

localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp

@@ -44,37 +44,40 @@ class TreeStructuredParzenEstimator
     MAXIMIZE = 1,
   };
 
+  enum Index {
+    TRANS_X = 0,
+    TRANS_Y = 1,
+    TRANS_Z = 2,
+    ANGLE_X = 3,
+    ANGLE_Y = 4,
+    ANGLE_Z = 5,
+    INDEX_NUM = 6,
+  };
+
   TreeStructuredParzenEstimator() = delete;
   TreeStructuredParzenEstimator(
-    const Direction direction, const int64_t n_startup_trials, std::vector<bool> is_loop_variable);
+    const Direction direction, const int64_t n_startup_trials,
+    const std::vector<double> & sample_mean, const std::vector<double> & sample_stddev);
   void add_trial(const Trial & trial);
   Input get_next_input() const;
 
 private:
-  static constexpr double BASE_STDDEV_COEFF = 0.2;
   static constexpr double MAX_GOOD_RATE = 0.10;
-  static constexpr double MAX_VALUE = 1.0;
-  static constexpr double MIN_VALUE = -1.0;
-  static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE;
   static constexpr int64_t N_EI_CANDIDATES = 100;
-  static constexpr double PRIOR_WEIGHT = 0.0;
 
   static std::mt19937_64 engine;
-  static std::uniform_real_distribution<double> dist_uniform;
-  static std::normal_distribution<double> dist_normal;
 
   double compute_log_likelihood_ratio(const Input & input) const;
   double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const;
-  static std::vector<double> get_weights(const int64_t n);
-  static double normalize_loop_variable(const double value);
 
   std::vector<Trial> trials_;
   int64_t above_num_;
   const Direction direction_;
   const int64_t n_startup_trials_;
   const int64_t input_dimension_;
-  const std::vector<bool> is_loop_variable_;
-  const Input base_stddev_;
+  const std::vector<double> sample_mean_;
+  const std::vector<double> sample_stddev_;
+  Input base_stddev_;
 };
 
 #endif  // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_