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_

localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -21,19 +21,33 @@
 
 // random number generator
 std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}());
-std::uniform_real_distribution<double> TreeStructuredParzenEstimator::dist_uniform(
-  TreeStructuredParzenEstimator::MIN_VALUE, TreeStructuredParzenEstimator::MAX_VALUE);
-std::normal_distribution<double> TreeStructuredParzenEstimator::dist_normal(0.0, 1.0);
 
 TreeStructuredParzenEstimator::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)
 : above_num_(0),
   direction_(direction),
   n_startup_trials_(n_startup_trials),
-  input_dimension_(is_loop_variable.size()),
-  is_loop_variable_(is_loop_variable),
-  base_stddev_(input_dimension_, VALUE_WIDTH)
+  input_dimension_(INDEX_NUM),
+  sample_mean_(sample_mean),
+  sample_stddev_(sample_stddev)
 {
+  if (sample_mean_.size() != ANGLE_Z) {
+    std::cerr << "sample_mean size is invalid" << std::endl;
+    throw std::runtime_error("sample_mean size is invalid");
+  }
+  if (sample_stddev_.size() != ANGLE_Z) {
+    std::cerr << "sample_stddev size is invalid" << std::endl;
+    throw std::runtime_error("sample_stddev size is invalid");
+  }
+  // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher.
+  base_stddev_.resize(input_dimension_);
+  base_stddev_[TRANS_X] = 0.25;                // [m]
+  base_stddev_[TRANS_Y] = 0.25;                // [m]
+  base_stddev_[TRANS_Z] = 0.25;                // [m]
+  base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI;  // [rad]
+  base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI;  // [rad]
+  base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI;  // [rad]
 }
 
 void TreeStructuredParzenEstimator::add_trial(const Trial & trial)
@@ -43,47 +57,45 @@
     return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score);
   });
   above_num_ =
-    std::min(static_cast<int64_t>(25), static_cast<int64_t>(trials_.size() * MAX_GOOD_RATE));
+    std::min({static_cast<int64_t>(10), static_cast<int64_t>(trials_.size() * MAX_GOOD_RATE)});
 }
 
 TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const
 {
+  std::normal_distribution<double> dist_normal_trans_x(
+    sample_mean_[TRANS_X], sample_stddev_[TRANS_X]);
+  std::normal_distribution<double> dist_normal_trans_y(
+    sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]);
+  std::normal_distribution<double> dist_normal_trans_z(
+    sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]);
+  std::normal_distribution<double> dist_normal_angle_x(
+    sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]);
+  std::normal_distribution<double> dist_normal_angle_y(
+    sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]);
+  std::uniform_real_distribution<double> dist_uniform_angle_z(-M_PI, M_PI);
+
   if (static_cast<int64_t>(trials_.size()) < n_startup_trials_ || above_num_ == 0) {
     // Random sampling based on prior until the number of trials reaches `n_startup_trials_`.
     Input input(input_dimension_);
-    for (int64_t j = 0; j < input_dimension_; j++) {
-      input[j] = dist_uniform(engine);
-    }
+    input[TRANS_X] = dist_normal_trans_x(engine);
+    input[TRANS_Y] = dist_normal_trans_y(engine);
+    input[TRANS_Z] = dist_normal_trans_z(engine);
+    input[ANGLE_X] = dist_normal_angle_x(engine);
+    input[ANGLE_Y] = dist_normal_angle_y(engine);
+    input[ANGLE_Z] = dist_uniform_angle_z(engine);
     return input;
   }
 
   Input best_input;
   double best_log_likelihood_ratio = std::numeric_limits<double>::lowest();
-  const double coeff = BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_));
-  std::vector<double> weights = get_weights(above_num_);
-  weights.push_back(PRIOR_WEIGHT);
-  std::discrete_distribution<int64_t> dist(weights.begin(), weights.end());
   for (int64_t i = 0; i < N_EI_CANDIDATES; i++) {
-    Input mu, sigma;
-    const int64_t index = dist(engine);
-    if (index == above_num_) {
-      mu = Input(input_dimension_, 0.0);
-      sigma = base_stddev_;
-    } else {
-      mu = trials_[index].input;
-      sigma = base_stddev_;
-      for (int64_t j = 0; j < input_dimension_; j++) {
-        sigma[j] *= coeff;
-      }
-    }
-    // sample from the normal distribution
     Input input(input_dimension_);
-    for (int64_t j = 0; j < input_dimension_; j++) {
-      input[j] = mu[j] + dist_normal(engine) * sigma[j];
-      input[j] =
-        (is_loop_variable_[j] ? normalize_loop_variable(input[j])
-                              : std::clamp(input[j], MIN_VALUE, MAX_VALUE));
-    }
+    input[TRANS_X] = dist_normal_trans_x(engine);
+    input[TRANS_Y] = dist_normal_trans_y(engine);
+    input[TRANS_Z] = dist_normal_trans_z(engine);
+    input[ANGLE_X] = dist_normal_angle_x(engine);
+    input[ANGLE_Y] = dist_normal_angle_y(engine);
+    input[ANGLE_Z] = dist_uniform_angle_z(engine);
     const double log_likelihood_ratio = compute_log_likelihood_ratio(input);
     if (log_likelihood_ratio > best_log_likelihood_ratio) {
       best_log_likelihood_ratio = log_likelihood_ratio;
@@ -102,50 +114,19 @@
   std::vector<double> above_logs;
   std::vector<double> below_logs;
 
-  // Scott's rule
-  const double coeff_above =
-    BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_));
-  const double coeff_below =
-    BASE_STDDEV_COEFF * std::pow(n - above_num_, -1.0 / (4 + input_dimension_));
-  Input sigma_above = base_stddev_;
-  Input sigma_below = base_stddev_;
-  for (int64_t j = 0; j < input_dimension_; j++) {
-    sigma_above[j] *= coeff_above;
-    sigma_below[j] *= coeff_below;
-  }
-
-  std::vector<double> above_weights = get_weights(above_num_);
-  std::vector<double> below_weights = get_weights(n - above_num_);
-  std::reverse(below_weights.begin(), below_weights.end());  // below_weights is ascending order
-
-  // calculate the sum of weights to normalize
-  double above_sum = std::accumulate(above_weights.begin(), above_weights.end(), 0.0);
-  double below_sum = std::accumulate(below_weights.begin(), below_weights.end(), 0.0);
-
-  // above includes prior
-  above_sum += PRIOR_WEIGHT;
-
   for (int64_t i = 0; i < n; i++) {
+    const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_);
     if (i < above_num_) {
-      const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_above);
-      const double w = above_weights[i] / above_sum;
+      const double w = 1.0 / above_num_;
       const double log_w = std::log(w);
       above_logs.push_back(log_p + log_w);
     } else {
-      const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_below);
-      const double w = below_weights[i - above_num_] / below_sum;
+      const double w = 1.0 / (n - above_num_);
       const double log_w = std::log(w);
       below_logs.push_back(log_p + log_w);
     }
   }
 
-  // prior
-  if (PRIOR_WEIGHT > 0.0) {
-    const double log_p = log_gaussian_pdf(input, Input(input_dimension_, 0.0), base_stddev_);
-    const double log_w = std::log(PRIOR_WEIGHT / above_sum);
-    above_logs.push_back(log_p + log_w);
-  }
-
   auto log_sum_exp = [](const std::vector<double> & log_vec) {
     const double max = *std::max_element(log_vec.begin(), log_vec.end());
     double sum = 0.0;
@@ -157,7 +138,11 @@
 
   const double above = log_sum_exp(above_logs);
   const double below = log_sum_exp(below_logs);
-  const double r = above - below;
+
+  // Multiply by a constant so that the score near the "below sample" becomes lower.
+  // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again
+  // later.
+  const double r = above - below * 5.0;
   return r;
 }
 
@@ -174,44 +159,21 @@
   double result = 0.0;
   for (int64_t i = 0; i < n; i++) {
     double diff = input[i] - mu[i];
-    if (is_loop_variable_[i]) {
-      diff = normalize_loop_variable(diff);
+    if (i == ANGLE_Z) {
+      // Normalize the loop variable to [-pi, pi)
+      while (diff >= M_PI) {
+        diff -= 2 * M_PI;
+      }
+      while (diff < -M_PI) {
+        diff += 2 * M_PI;
+      }
     }
-    result += log_gaussian_pdf_1d(diff, sigma[i]);
-  }
-  return result;
-}
-
-std::vector<double> TreeStructuredParzenEstimator::get_weights(const int64_t n)
-{
-  // See optuna
-  // https://github.com/optuna/optuna/blob/4bfab78e98bf786f6a2ce6e593a26e3f8403e08d/optuna/samplers/_tpe/sampler.py#L50-L58
-  std::vector<double> weights;
-  constexpr int64_t WEIGHT_ALPHA = 25;
-  if (n == 0) {
-    return weights;
-  } else if (n < WEIGHT_ALPHA) {
-    weights.resize(n, 1.0);
-  } else {
-    weights.resize(n);
-    const double unit = (1.0 - 1.0 / n) / (n - WEIGHT_ALPHA);
-    for (int64_t i = 0; i < n; i++) {
-      weights[i] = (i < WEIGHT_ALPHA ? 1.0 : 1.0 - unit * (i - WEIGHT_ALPHA));
+    // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x,
+    // angle_y.
+    if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) {
+      continue;
     }
-  }
-
-  return weights;
-}
-
-double TreeStructuredParzenEstimator::normalize_loop_variable(const double value)
-{
-  // Normalize the loop variable to [-1, 1)
-  double result = value;
-  while (result >= MAX_VALUE) {
-    result -= VALUE_WIDTH;
-  }
-  while (result < MIN_VALUE) {
-    result += VALUE_WIDTH;
+    result += log_gaussian_pdf_1d(diff, sigma[i]);
   }
   return result;
 }

localization/tree_structured_parzen_estimator/test/test_tpe.cpp

@@ -28,19 +28,25 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe
     return value;
   };
 
-  constexpr int64_t kOuterTrialsNum = 10;
-  constexpr int64_t kInnerTrialsNum = 100;
+  constexpr int64_t kOuterTrialsNum = 20;
+  constexpr int64_t kInnerTrialsNum = 200;
   std::cout << std::fixed;
   std::vector<double> mean_scores;
-  for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 10}) {
+  const int64_t n_startup_trials = kInnerTrialsNum / 10;
+  const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE");
+
+  const std::vector<double> sample_mean(5, 0.0);
+  const std::vector<double> sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1};
+
+  for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 2}) {
     const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE");
 
     std::vector<double> scores;
     for (int64_t i = 0; i < kOuterTrialsNum; i++) {
       double best_score = std::numeric_limits<double>::lowest();
-      const std::vector<bool> is_loop_variable(6, false);
       TreeStructuredParzenEstimator estimator(
-        TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, is_loop_variable);
+        TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean,
+        sample_stddev);
       for (int64_t trial = 0; trial < kInnerTrialsNum; trial++) {
         const TreeStructuredParzenEstimator::Input input = estimator.get_next_input();
         const double score = -sphere_function(input);