From 898fb73ab986bb4d3ca42f9d7b947ced13ad257b Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Thu, 13 Jun 2024 11:55:00 +0900 Subject: [PATCH] refactor(tree_structured_parzen_estimator): apply static analysis (#7364) * refactor based on linter Signed-off-by: a-maumau * fix to match the change in TreeStructuredParzenEstimator Signed-off-by: a-maumau * style(pre-commit): autofix * apply suggestion Signed-off-by: a-maumau * revert const declare Signed-off-by: a-maumau * revert const declare Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: SakodaShintaro --- .../tree_structured_parzen_estimator.hpp | 15 +++++---- .../src/tree_structured_parzen_estimator.cpp | 32 +++++++++---------- .../test/test_tpe.cpp | 32 ++++++++----------- 3 files changed, 38 insertions(+), 41 deletions(-) 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 30d36e7150113..3853fed2c3943 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 @@ -56,19 +56,20 @@ class TreeStructuredParzenEstimator TreeStructuredParzenEstimator() = delete; TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, - const std::vector & sample_mean, const std::vector & sample_stddev); + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev); void add_trial(const Trial & trial); - Input get_next_input() const; + [[nodiscard]] Input get_next_input() const; private: - static constexpr double MAX_GOOD_RATE = 0.10; - static constexpr int64_t N_EI_CANDIDATES = 100; + static constexpr double max_good_rate = 0.10; + static constexpr int64_t n_ei_candidates = 100; static std::mt19937_64 engine; - double compute_log_likelihood_ratio(const Input & input) const; - double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const; + [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; + [[nodiscard]] static double log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma); std::vector trials_; int64_t above_num_; 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 c81962c14f61c..f7cdcbacd3e04 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 @@ -23,14 +23,14 @@ std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, - const std::vector & sample_mean, const std::vector & sample_stddev) + const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, + std::vector sample_stddev) : above_num_(0), direction_(direction), n_startup_trials_(n_startup_trials), input_dimension_(INDEX_NUM), - sample_mean_(sample_mean), - sample_stddev_(sample_stddev) + sample_mean_(std::move(sample_mean)), + sample_stddev_(std::move(sample_stddev)) { if (sample_mean_.size() != ANGLE_Z) { std::cerr << "sample_mean size is invalid" << std::endl; @@ -56,8 +56,9 @@ void TreeStructuredParzenEstimator::add_trial(const Trial & trial) std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); }); - above_num_ = - std::min({static_cast(10), static_cast(trials_.size() * MAX_GOOD_RATE)}); + above_num_ = std::min( + {static_cast(10), + static_cast(static_cast(trials_.size()) * max_good_rate)}); } TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const @@ -88,7 +89,7 @@ TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_inp Input best_input; double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < N_EI_CANDIDATES; i++) { + for (int64_t i = 0; i < n_ei_candidates; i++) { Input input(input_dimension_); input[TRANS_X] = dist_normal_trans_x(engine); input[TRANS_Y] = dist_normal_trans_y(engine); @@ -107,7 +108,7 @@ TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_inp double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const { - const int64_t n = trials_.size(); + const auto n = static_cast(trials_.size()); // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to // select best sample. @@ -117,11 +118,11 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & 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 w = 1.0 / above_num_; + const double w = 1.0 / static_cast(above_num_); const double log_w = std::log(w); above_logs.push_back(log_p + log_w); } else { - const double w = 1.0 / (n - above_num_); + const double w = 1.0 / static_cast(n - above_num_); const double log_w = std::log(w); below_logs.push_back(log_p + log_w); } @@ -129,10 +130,9 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & 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; - for (const double log_v : log_vec) { - sum += std::exp(log_v - max); - } + double sum = std::accumulate( + log_vec.begin(), log_vec.end(), 0.0, + [max](double total, double log_v) { return total + std::exp(log_v - max); }); return max + std::log(sum); }; @@ -147,14 +147,14 @@ double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & } double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) const + const Input & input, const Input & mu, const Input & sigma) { const double log_2pi = std::log(2.0 * M_PI); auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); }; - const int64_t n = input.size(); + const auto n = static_cast(input.size()); double result = 0.0; for (int64_t i = 0; i < n; i++) { diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp index f8a697878d6a3..357c86289ef1f 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -20,7 +20,7 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe { auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { double value = 0.0; - const int64_t n = input.size(); + const auto n = static_cast(input.size()); for (int64_t i = 0; i < n; i++) { const double v = input[i] * 10; value += v * v; @@ -28,26 +28,23 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe return value; }; - constexpr int64_t kOuterTrialsNum = 20; - constexpr int64_t kInnerTrialsNum = 200; + constexpr int64_t k_outer_trials_num = 20; + constexpr int64_t k_inner_trials_num = 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 sample_mean(5, 0.0); + std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - 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"); + for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { + const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); std::vector scores; - for (int64_t i = 0; i < kOuterTrialsNum; i++) { + for (int64_t i = 0; i < k_outer_trials_num; 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++) { + for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); const double score = -sphere_function(input); estimator.add_trial({input, score}); @@ -57,13 +54,12 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe } const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / scores.size(); + const double mean = sum / static_cast(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()); + double sq_sum = std::accumulate( + scores.begin(), scores.end(), 0.0, + [mean](double total, double score) { return total + (score - mean) * (score - mean); }); + const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; }