feat: add noise on priority map generation to widen detection area

Signed-off-by: Taekjin LEE <[email protected]>

refactor: optimize CenterPointTRT::initPriorityMap()

refactor: optimize CenterPointTRT::initPriorityMap()

perception/lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -22,9 +22,9 @@
 #include <algorithm>
 #include <iostream>
 #include <memory>
+#include <random>
 #include <string>
 #include <vector>
-#include <random>
 
 namespace centerpoint
 {
@@ -70,7 +70,7 @@ void CenterPointTRT::initPriorityMap()
 
   // random number generator, but fixed seed for reproducibility
   std::mt19937 generator(0);
-  std::normal_distribution<float> normal_distribution(0.0, 1000.0);
+  std::normal_distribution<float> normal_distribution(0.0, 1.0);
 
   // assign priority score map
   for (unsigned int i = 0; i < mask_size_; ++i) {
@@ -79,15 +79,16 @@ void CenterPointTRT::initPriorityMap()
 
     const float pos_x = x * config_.voxel_size_x_;
     const float pos_y = y * config_.voxel_size_y_;
-
-    // cross section area, weighted to the front by sigmoid function
-    float score_a = abs((pos_x - 15.0) * pos_y) * ( 1/(1+exp(pos_x*0.3)) + 1.0);
-    // ellipse area in the front
-    float score_b = sqrt((pos_x-150)*(pos_x-150) + pos_y*pos_y) + sqrt((pos_x-10)*(pos_x-10) + pos_y*pos_y) - 50;
+
+    // absolute rectangular hyperbola to focus on crosssection
+    // multiply sigmoid function to prioritize the front area
+    const float score_a = abs((pos_x - 15.0) * pos_y) * (1 / (1 + exp(pos_x * 0.3)) + 0.75);
+    // ellipse area focus on (10,0) and (150,0)
+    const float score_b = sqrt((pos_x - 150) * (pos_x - 150) + pos_y * pos_y) +
+                          sqrt((pos_x - 10) * (pos_x - 10) + pos_y * pos_y) - 140;
     // total score with weight
-    float score = -score_a - score_b * 15;
-    // add random noise, normal distribution
-    score += normal_distribution(generator);
+    const float score =
+      sqrt(score_a * 0.1 + score_b * 2.5) + normal_distribution(generator) * score_b * 0.01;
 
     priority_score_map.push_back(std::make_pair(score, i));
   }
@@ -98,7 +99,7 @@ void CenterPointTRT::initPriorityMap()
   std::sort(
     priority_score_map.begin(), priority_score_map.end(),
     [](const std::pair<float, unsigned int> & a, const std::pair<float, unsigned int> & b) {
-      return a.first > b.first;
+      return a.first < b.first;
     });
 
   for (unsigned int i = 0; i < mask_size_; ++i) {