Merge branch 'feat/cuda_probabilistic_occupancy_grid_map' of github.c…

…om:knzo25/autoware.universe into feat/cuda_probabilistic_occupancy_grid_map
autowarefoundation · Jan 23, 2025 · cdbdd2d · cdbdd2d
2 parents ea46130 + a33d3d9
commit cdbdd2d
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/...ware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu b/...ware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu
@@ -33,7 +33,8 @@ __host__ __device__ __forceinline__ double convertCharToProbability(const std::u
 
 __host__ __device__ __forceinline__ std::uint8_t convertProbabilityToChar(const double value)
 {
-  return static_cast<std::uint8_t>(value * 255.0);
+  return static_cast<std::uint8_t>(std::max(0.0, std::min(1.0, value)) * 255.0);
+  ;
 }
 
 __host__ __device__ __forceinline__ double logOddsFusion(const double p1, const double p2)
@@ -43,7 +44,7 @@ __host__ __device__ __forceinline__ double logOddsFusion(const double p1, const
   const double p1_norm = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, p1));
   log_odds += std::log(p1_norm / (1.0 - p1_norm));
 
-  const double p2_norm = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, p1));
+  const double p2_norm = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, p2));
   log_odds += std::log(p2_norm / (1.0 - p2_norm));
 
   return 1.0 / (1.0 + std::exp(-log_odds));