test(autoware_localization_util): add tests

Signed-off-by: NorahXiong <norah.xiong@autocore.ai>

Author: NorahXiong

localization/autoware_localization_util/test/test_tpe.cpp

@@ -73,3 +73,154 @@ TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphe
   }
   ASSERT_LT(mean_scores[0], mean_scores[1]);
 }
+
+// Additional tests for TreeStructuredParzenEstimator
+TEST(TreeStructuredParzenEstimatorTest, MinimizationTest)
+{
+  auto quadratic_function = [](const TreeStructuredParzenEstimator::Input & input) {
+    return input[0] * input[0] + input[1] * input[1];
+  };
+
+  constexpr int64_t k_trials_num = 100;
+  std::vector<double> sample_mean{0.0, 0.0};
+  std::vector<double> sample_stddev{1.0, 1.0};
+
+  double best_score = std::numeric_limits<double>::max();
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MINIMIZE, k_trials_num / 2, sample_mean,
+    sample_stddev);
+  
+  for (int64_t trial = 0; trial < k_trials_num; trial++) {
+    const TreeStructuredParzenEstimator::Input input = estimator.get_next_input();
+    const double score = quadratic_function(input);
+    estimator.add_trial({input, score});
+    best_score = std::min(best_score, score);
+  }
+
+  EXPECT_LT(best_score, 0.1);  // Should find a solution close to 0
+}
+
+TEST(TreeStructuredParzenEstimatorTest, SingleDimensionTest)
+{
+  auto linear_function = [](const TreeStructuredParzenEstimator::Input & input) {
+    return input[0];
+  };
+
+  constexpr int64_t k_trials_num = 50;
+  std::vector<double> sample_mean{0.0};
+  std::vector<double> sample_stddev{1.0};
+
+  double best_score = std::numeric_limits<double>::lowest();
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MAXIMIZE, k_trials_num / 2, sample_mean,
+    sample_stddev);
+  
+  for (int64_t trial = 0; trial < k_trials_num; trial++) {
+    const TreeStructuredParzenEstimator::Input input = estimator.get_next_input();
+    const double score = linear_function(input);
+    estimator.add_trial({input, score});
+    best_score = std::max(best_score, score);
+  }
+
+  EXPECT_GT(best_score, 2.0);  // Should find a value in the right tail
+}
+
+TEST(TreeStructuredParzenEstimatorTest, EmptyTrials)
+{
+  std::vector<double> sample_mean{0.0, 0.0};
+  std::vector<double> sample_stddev{1.0, 1.0};
+
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MAXIMIZE, 10, sample_mean, sample_stddev);
+  
+  // Should not crash when getting input without any trials
+  const TreeStructuredParzenEstimator::Input input = estimator.get_next_input();
+  EXPECT_EQ(input.size(), 2u);
+}
+
+TEST(TreeStructuredParzenEstimatorTest, SingleTrial)
+{
+  std::vector<double> sample_mean{0.0, 0.0};
+  std::vector<double> sample_stddev{1.0, 1.0};
+
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MAXIMIZE, 0, sample_mean, sample_stddev);
+  
+  // Add one trial
+  const TreeStructuredParzenEstimator::Input first_input{1.0, 1.0};
+  estimator.add_trial({first_input, 1.0});
+  
+  // Should be able to get next input
+  const TreeStructuredParzenEstimator::Input next_input = estimator.get_next_input();
+  EXPECT_EQ(next_input.size(), 2u);
+}
+
+TEST(TreeStructuredParzenEstimatorTest, LogGaussianPdf)
+{
+  TreeStructuredParzenEstimator::Input input{1.0, 2.0};
+  TreeStructuredParzenEstimator::Input mu{1.0, 2.0};
+  TreeStructuredParzenEstimator::Input sigma{1.0, 1.0};
+  
+  double log_pdf = TreeStructuredParzenEstimator::log_gaussian_pdf(input, mu, sigma);
+  EXPECT_NEAR(log_pdf, -1.8378770664093453, 1e-6);  // Expected value calculated manually
+  
+  // Test with different sigma
+  TreeStructuredParzenEstimator::Input sigma2{0.5, 0.5};
+  log_pdf = TreeStructuredParzenEstimator::log_gaussian_pdf(input, mu, sigma2);
+  EXPECT_NEAR(log_pdf, -2.8378770664093453, 1e-6);
+}
+
+TEST(TreeStructuredParzenEstimatorTest, DifferentSampleStddev)
+{
+  std::vector<double> sample_mean{0.0, 0.0, 0.0};
+  std::vector<double> sample_stddev{1.0, 0.1, 10.0};  // Different scales for different dimensions
+
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MAXIMIZE, 10, sample_mean, sample_stddev);
+  
+  // Add some trials
+  for (int i = 0; i < 5; ++i) {
+    TreeStructuredParzenEstimator::Input input{0.1 * i, 0.01 * i, 1.0 * i};
+    estimator.add_trial({input, static_cast<double>(i)});
+  }
+  
+  // Get next input and verify it's within reasonable bounds
+  const TreeStructuredParzenEstimator::Input next_input = estimator.get_next_input();
+  EXPECT_NEAR(next_input[0], 0.0, 2.0);  // stddev = 1.0
+  EXPECT_NEAR(next_input[1], 0.0, 0.2);  // stddev = 0.1
+  EXPECT_NEAR(next_input[2], 0.0, 20.0); // stddev = 10.0
+}
+
+TEST(TreeStructuredParzenEstimatorTest, ComputeLogLikelihoodRatio)
+{
+  std::vector<double> sample_mean{0.0, 0.0};
+  std::vector<double> sample_stddev{1.0, 1.0};
+
+  TreeStructuredParzenEstimator estimator(
+    TreeStructuredParzenEstimator::Direction::MAXIMIZE, 0, sample_mean, sample_stddev);
+  
+  // Add some trials
+  for (int i = 0; i < 10; ++i) {
+    TreeStructuredParzenEstimator::Input input{0.1 * i, 0.1 * i};
+    estimator.add_trial({input, static_cast<double>(i)});
+  }
+  
+  // Test likelihood ratio for a point
+  TreeStructuredParzenEstimator::Input test_point{0.5, 0.5};
+  double ratio = estimator.compute_log_likelihood_ratio(test_point);
+  
+  // The exact value depends on the random sampling, but we can check it's finite
+  EXPECT_TRUE(std::isfinite(ratio));
+}
+
+TEST(TreeStructuredParzenEstimatorTest, FixedIndexTest)
+{
+  // Test that the enum values are as expected
+  EXPECT_EQ(TreeStructuredParzenEstimator::TRANS_X, 0);
+  EXPECT_EQ(TreeStructuredParzenEstimator::TRANS_Y, 1);
+  EXPECT_EQ(TreeStructuredParzenEstimator::TRANS_Z, 2);
+  EXPECT_EQ(TreeStructuredParzenEstimator::ANGLE_X, 3);
+  EXPECT_EQ(TreeStructuredParzenEstimator::ANGLE_Y, 4);
+  EXPECT_EQ(TreeStructuredParzenEstimator::ANGLE_Z, 5);
+  EXPECT_EQ(TreeStructuredParzenEstimator::INDEX_NUM, 6);
+}