feat: add fusion policy test

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

perception/probabilistic_occupancy_grid_map/CMakeLists.txt

@@ -68,7 +68,10 @@ rclcpp_components_register_node(grid_map_fusion
 if(BUILD_TESTING)
   ament_add_gtest(costmap_unit_tests
     test/cost_value_test.cpp)
+  ament_add_gtest(fusion_policy_unit_tests
+    test/fusion_policy_test.cpp)
   target_include_directories(costmap_unit_tests PRIVATE "include")
+  target_include_directories(fusion_policy_unit_tests PRIVATE "include")
 endif()
 
 ament_auto_package(

perception/probabilistic_occupancy_grid_map/include/fusion/single_frame_fusion_policy.hpp

@@ -20,6 +20,9 @@
 #include <stdexcept>
 #include <vector>
 
+/*min and max prob threshold to prevent log-odds to diverge*/
+#define EPSILON_PROB 0.03
+
 namespace fusion_policy
 {
 
@@ -123,7 +126,8 @@ double logOddsFusion(const std::vector<double> & probabilities)
 {
   double log_odds = 0.0;
   for (auto & probability : probabilities) {
-    log_odds += std::log(probability / (1.0 - probability));
+    const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probability));
+    log_odds += std::log(p / (1.0 - p));
   }
   return 1.0 / (1.0 + std::exp(-log_odds));
 }
@@ -148,7 +152,8 @@ double logOddsFusion(const std::vector<double> & probabilities, const std::vecto
 
   double log_odds = 0.0;
   for (size_t i = 0; i < probabilities.size(); i++) {
-    log_odds += weights[i] * std::log(probabilities[i] / (1.0 - probabilities[i]));
+    const double p = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, probabilities[i]));
+    log_odds += weights[i] * std::log(p / (1.0 - p));
   }
   return 1.0 / (1.0 + std::exp(-log_odds));
 }

perception/probabilistic_occupancy_grid_map/test/fusion_policy_test.cpp

@@ -0,0 +1,75 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "cost_value.hpp"
+#include "fusion/single_frame_fusion_policy.hpp"
+#include "gtest/gtest.h"
+
+// Test the log-odds update rule
+TEST(FusionPolicyTest, TestLogOddsUpdateRule)
+{
+  using fusion_policy::log_odds_fusion::logOddsFusion;
+  const double MARGIN = 0.03;
+  const double OCCUPIED = 1.0 - MARGIN;
+  const double FREE = 0.0 + MARGIN;
+  const double UNKNOWN = 0.5;
+  const double EPSILON = 1e-3;
+
+  // same case
+  std::vector<double> case1_1 = {OCCUPIED, OCCUPIED};
+  std::vector<double> case1_2 = {FREE, FREE};
+  std::vector<double> case1_3 = {UNKNOWN, UNKNOWN};
+  EXPECT_GE(logOddsFusion(case1_1), OCCUPIED);
+  EXPECT_LE(logOddsFusion(case1_2), FREE);
+  EXPECT_NEAR(logOddsFusion(case1_3), UNKNOWN, EPSILON);
+
+  // with unknown
+  std::vector<double> case2_1 = {OCCUPIED, UNKNOWN};
+  std::vector<double> case2_2 = {FREE, UNKNOWN};
+  EXPECT_NEAR(logOddsFusion(case2_1), OCCUPIED, EPSILON);
+  EXPECT_NEAR(logOddsFusion(case2_2), FREE, EPSILON);
+
+  // with conflict
+  std::vector<double> case3_1 = {OCCUPIED, FREE};
+  EXPECT_NEAR(logOddsFusion(case3_1), UNKNOWN, EPSILON);
+}
+
+// Test the dempster-shafer update rule
+TEST(FusionPolicyTest, TestDempsterShaferUpdateRule)
+{
+  using fusion_policy::dempster_shafer_fusion::dempsterShaferFusion;
+  const double MARGIN = 0.03;
+  const double OCCUPIED = 1.0 - MARGIN;
+  const double FREE = 0.0 + MARGIN;
+  const double UNKNOWN = 0.5;
+  const double EPSILON = 1e-3;
+
+  // same case
+  std::vector<double> case1_1 = {OCCUPIED, OCCUPIED};
+  std::vector<double> case1_2 = {FREE, FREE};
+  std::vector<double> case1_3 = {UNKNOWN, UNKNOWN};
+  EXPECT_GE(dempsterShaferFusion(case1_1), OCCUPIED);
+  EXPECT_LE(dempsterShaferFusion(case1_2), FREE);
+  EXPECT_NEAR(dempsterShaferFusion(case1_3), UNKNOWN, EPSILON);
+
+  // with unknown
+  std::vector<double> case2_1 = {OCCUPIED, UNKNOWN};
+  std::vector<double> case2_2 = {FREE, UNKNOWN};
+  EXPECT_NEAR(dempsterShaferFusion(case2_1), OCCUPIED, EPSILON);
+  EXPECT_NEAR(dempsterShaferFusion(case2_2), FREE, EPSILON);
+
+  // with conflict
+  std::vector<double> case3_1 = {OCCUPIED, FREE};
+  EXPECT_NEAR(dempsterShaferFusion(case3_1), UNKNOWN, EPSILON);
+}