From 73d3b38bc5db635fbf082b7d87ae31034cebb78c Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 13 May 2024 09:54:41 +0900 Subject: [PATCH 1/4] Improved tpe Signed-off-by: Shintaro Sakoda --- .../config/ndt_scan_matcher.param.yaml | 2 +- .../schema/sub/initial_pose_estimation.json | 2 +- .../src/ndt_scan_matcher_core.cpp | 72 +++----- .../tree_structured_parzen_estimator.hpp | 27 +-- .../src/tree_structured_parzen_estimator.cpp | 171 +++++++----------- .../test/test_tpe.cpp | 52 +++--- 6 files changed, 131 insertions(+), 195 deletions(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 241892e67b66c..ec80a0ef79c69 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/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: diff --git a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json b/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json index 9817f3145bbd3..20250d05782f9 100644 --- a/localization/ndt_scan_matcher/schema/sub/initial_pose_estimation.json +++ b/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 } }, diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 1e3188a15acc9..8e7685180c1f9 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -22,8 +22,6 @@ #include #include -#include - #include #ifdef ROS_DISTRO_GALACTIC @@ -988,34 +986,20 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( 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 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 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 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_array; auto output_cloud = std::make_shared>(); @@ -1029,16 +1013,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( 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); @@ -1061,22 +1042,13 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( 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>(); diff --git a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp index b7b522b4e6b76..30d36e7150113 100644 --- a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp +++ b/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 is_loop_variable); + const Direction direction, const int64_t n_startup_trials, + const std::vector & sample_mean, const std::vector & 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 dist_uniform; - static std::normal_distribution 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 get_weights(const int64_t n); - static double normalize_loop_variable(const double value); std::vector trials_; int64_t above_num_; const Direction direction_; const int64_t n_startup_trials_; const int64_t input_dimension_; - const std::vector is_loop_variable_; - const Input base_stddev_; + const std::vector sample_mean_; + const std::vector sample_stddev_; + Input base_stddev_; }; #endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp index 99c70a844f331..ec22e26567721 100644 --- a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp +++ b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp @@ -21,19 +21,33 @@ // random number generator std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); -std::uniform_real_distribution TreeStructuredParzenEstimator::dist_uniform( - TreeStructuredParzenEstimator::MIN_VALUE, TreeStructuredParzenEstimator::MAX_VALUE); -std::normal_distribution TreeStructuredParzenEstimator::dist_normal(0.0, 1.0); TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable) + const Direction direction, const int64_t n_startup_trials, + const std::vector & sample_mean, const std::vector & 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 @@ void TreeStructuredParzenEstimator::add_trial(const Trial & trial) return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); }); above_num_ = - std::min(static_cast(25), static_cast(trials_.size() * MAX_GOOD_RATE)); + std::min({static_cast(10), static_cast(trials_.size() * MAX_GOOD_RATE)}); } TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const { + std::normal_distribution dist_normal_trans_x( + sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); + std::normal_distribution dist_normal_trans_y( + sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); + std::normal_distribution dist_normal_trans_z( + sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); + std::normal_distribution dist_normal_angle_x( + sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); + std::normal_distribution dist_normal_angle_y( + sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); + std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); + if (static_cast(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::lowest(); - const double coeff = BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); - std::vector weights = get_weights(above_num_); - weights.push_back(PRIOR_WEIGHT); - std::discrete_distribution 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 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & std::vector above_logs; std::vector 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 above_weights = get_weights(above_num_); - std::vector 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 & log_vec) { const double max = *std::max_element(log_vec.begin(), log_vec.end()); double sum = 0.0; @@ -157,7 +138,10 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & 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. + // TODO: It's theoretically incorrect, consider it again later. + const double r = above - below * 5.0; return r; } @@ -174,44 +158,21 @@ double TreeStructuredParzenEstimator::log_gaussian_pdf( 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 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 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; } diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp index 32eb66e70fb16..a5106cc7ceb0b 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -32,34 +32,34 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe constexpr int64_t kInnerTrialsNum = 100; std::cout << std::fixed; std::vector mean_scores; - for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 10}) { - const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + const int64_t n_startup_trials = kInnerTrialsNum / 10; + const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); - std::vector scores; - for (int64_t i = 0; i < kOuterTrialsNum; i++) { - double best_score = std::numeric_limits::lowest(); - const std::vector is_loop_variable(6, false); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, is_loop_variable); - for (int64_t trial = 0; trial < kInnerTrialsNum; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); + std::vector scores; + for (int64_t i = 0; i < kOuterTrialsNum; i++) { + double best_score = std::numeric_limits::lowest(); + const std::vector sample_mean(5, 0.0); + const std::vector sample_stddev(5, 2.0); + TreeStructuredParzenEstimator estimator( + 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); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); } + scores.push_back(best_score); + } - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / scores.size(); - mean_scores.push_back(mean); - double sq_sum = 0.0; - for (const double score : scores) { - sq_sum += (score - mean) * (score - mean); - } - const double stddev = std::sqrt(sq_sum / scores.size()); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / scores.size(); + mean_scores.push_back(mean); + double sq_sum = 0.0; + for (const double score : scores) { + sq_sum += (score - mean) * (score - mean); } - ASSERT_LT(mean_scores[0], mean_scores[1]); + const double stddev = std::sqrt(sq_sum / scores.size()); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; } From e51a74a65ae3d9d938820bcea3bc5c27a0047df8 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 13 May 2024 10:42:48 +0900 Subject: [PATCH 2/4] Added name in TODO Signed-off-by: Shintaro Sakoda --- .../src/tree_structured_parzen_estimator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp index ec22e26567721..c81962c14f61c 100644 --- a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp +++ b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp @@ -140,7 +140,8 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & const double below = log_sum_exp(below_logs); // Multiply by a constant so that the score near the "below sample" becomes lower. - // TODO: It's theoretically incorrect, consider it again later. + // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again + // later. const double r = above - below * 5.0; return r; } From acd71c40d9dca92551f96823f107e8cb37e29ebd Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 14 May 2024 10:04:08 +0900 Subject: [PATCH 3/4] Fixed tpe test Signed-off-by: Shintaro Sakoda --- .../test/test_tpe.cpp | 58 ++++++++++--------- 1 file changed, 32 insertions(+), 26 deletions(-) diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp index a5106cc7ceb0b..f38c5091e4183 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -28,38 +28,44 @@ 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 mean_scores; const int64_t n_startup_trials = kInnerTrialsNum / 10; const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); - std::vector scores; - for (int64_t i = 0; i < kOuterTrialsNum; i++) { - double best_score = std::numeric_limits::lowest(); - const std::vector sample_mean(5, 0.0); - const std::vector sample_stddev(5, 2.0); - TreeStructuredParzenEstimator estimator( - 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); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); + const std::vector sample_mean(5, 0.0); + const std::vector 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 scores; + for (int64_t i = 0; i < kOuterTrialsNum; i++) { + double best_score = std::numeric_limits::lowest(); + TreeStructuredParzenEstimator estimator( + 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); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); } - scores.push_back(best_score); - } - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / scores.size(); - mean_scores.push_back(mean); - double sq_sum = 0.0; - for (const double score : scores) { - sq_sum += (score - mean) * (score - mean); - } - const double stddev = std::sqrt(sq_sum / scores.size()); + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / scores.size(); + mean_scores.push_back(mean); + double sq_sum = 0.0; + for (const double score : scores) { + sq_sum += (score - mean) * (score - mean); + } + const double stddev = std::sqrt(sq_sum / scores.size()); - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); } From 606d528812fecddafa4eb5e59b16f5111af1c30e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 May 2024 01:06:10 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- localization/tree_structured_parzen_estimator/test/test_tpe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp index f38c5091e4183..f8a697878d6a3 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -36,7 +36,7 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); const std::vector sample_mean(5, 0.0); - const std::vector sample_stddev {1.0, 1.0, 0.1, 0.1, 0.1}; + const std::vector 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");