perception: remove fp & tuning parameter

modules/perception/model/probabilistic_fusion.config

@@ -16,7 +16,7 @@ model_configs {
     }
     float_params {
         name: "max_radar_invisible_period"
-        value: 0.50
+        value: 0.25
     }
     float_params {
         name: "max_radar_confident_angle"

modules/perception/obstacle/fusion/probabilistic_fusion/pbf_track.cc

@@ -287,7 +287,7 @@ bool PbfTrack::AbleToPublish() {
               s_max_radar_confident_angle_) {
         if (fused_object_->object->velocity.dot(
                 fused_object_->object->direction) < 0.3) {
-          // fused_object_->object->velocity.setZero();
+          fused_object_->object->velocity.setZero();
           return false;
         }
         return true;

modules/perception/obstacle/radar/modest/conti_radar_util.h

@@ -18,6 +18,7 @@
 #define MODULES_PERCEPTION_OBSTACLE_RADAR_MODEST_CONTI_RADAR_UTIL_H
 
 #include "modules/perception/obstacle/radar/interface/base_radar_detector.h"
+#include "modules/perception/obstacle/common/geometry_util.h"
 
 namespace apollo {
 namespace perception {
@@ -26,6 +27,20 @@ class ContiRadarUtil {
  public:
   static bool IsFp(const ContiRadarObs &contiobs, const ContiParams &params,
                    const int delay_frames, const int tracking_times);
+
+  static bool IsConflict(const Eigen::Vector3f& main_velocity,
+          const Eigen::Vector3f& velocity) {
+    Eigen::Vector3f vector_temp1 = main_velocity;
+    Eigen::Vector3f vector_temp2 = velocity;
+    if (vector_temp1.head(2).norm() > 1e-5 && vector_temp2.head(2).norm() > 1e-5) {
+        double theta = VectorTheta2dXy(vector_temp1, vector_temp2);
+        if ((theta > 1.0 / 4.0 * M_PI && theta < 3.0 / 4.0 * M_PI) ||
+            (theta > -3.0 / 4.0 * M_PI && theta < -1.0 / 4.0 * M_PI)) {
+          return true;
+        }
+    }
+    return false;
+  }
 };
 
 }  // namespace perception

modules/perception/obstacle/radar/modest/modest_radar_detector_test.cc

@@ -53,8 +53,8 @@ TEST(ModestRadarDetectorTest, modest_radar_detector_test) {
   Eigen::Matrix4d radar2world_pose;
   radar2world_pose << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
   Eigen::Vector3f main_car_velocity;
-  main_car_velocity[0] = -1.0;
-  main_car_velocity[1] = 0.0;
+  main_car_velocity[0] = 0.3;
+  main_car_velocity[1] = 0.4;
   main_car_velocity[2] = 0.0;
   options.radar2world_pose = &radar2world_pose;
   options.car_linear_speed = main_car_velocity;
@@ -77,8 +77,8 @@ TEST(ModestRadarDetectorTest, modest_radar_detector_test) {
   EXPECT_EQ(objects.size(), 1);
   EXPECT_TRUE(fabs(objects[0]->center(0) - 0.0) < 1e-5);
   EXPECT_TRUE(fabs(objects[0]->center(1) - 0.0) < 1e-5);
-  EXPECT_TRUE(fabs(objects[0]->velocity(0) - 2.0) < 1e-5);
-  EXPECT_TRUE(fabs(objects[0]->velocity(1) - 4.0) < 1e-5);
+  EXPECT_TRUE(fabs(objects[0]->velocity(0) - 3.3) < 1e-5);
+  EXPECT_TRUE(fabs(objects[0]->velocity(1) - 4.4) < 1e-5);
   EXPECT_TRUE(objects[0]->type == UNKNOWN);
   objects.resize(0);
   header->set_timestamp_sec(123456789.074);
@@ -92,8 +92,8 @@ TEST(ModestRadarDetectorTest, modest_radar_detector_test) {
   EXPECT_EQ(objects.size(), 1);
   EXPECT_TRUE(fabs(objects[0]->center(0) - location(0)) < 1e-2);
   EXPECT_TRUE(fabs(objects[0]->center(1) - location(1)) < 1e-2);
-  EXPECT_TRUE(fabs(objects[0]->velocity(0) - 2.0) < 1e-2);
-  EXPECT_TRUE(fabs(objects[0]->velocity(1) - 4.0) < 1e-2);
+  EXPECT_TRUE(fabs(objects[0]->velocity(0) - 3.3) < 1e-2);
+  EXPECT_TRUE(fabs(objects[0]->velocity(1) - 4.4) < 1e-2);
   EXPECT_TRUE(objects[0]->type == UNKNOWN);
   delete radar_detector;
 }

modules/perception/obstacle/radar/modest/object_builder.cc

@@ -66,9 +66,14 @@ void ObjectBuilder::Build(const ContiRadar &raw_obstacles,
     Eigen::Matrix<double, 3, 1> velocity_r;
     Eigen::Matrix<double, 3, 1> velocity_w;
     velocity_r << raw_obstacles.contiobs(i).longitude_vel(),
-        raw_obstacles.contiobs(i).lateral_vel(), 0.0;
+                  raw_obstacles.contiobs(i).lateral_vel(), 
+                  0.0;
     velocity_w = radar_pose.topLeftCorner(3, 3) * velocity_r;
 
+    Eigen::Vector3f ref_velocity(main_velocity(0), main_velocity(1), 0.0);
+    if (ContiRadarUtil::IsConflict(ref_velocity, velocity_w.cast<float>())) {
+        object_ptr->is_background = true;
+    }
     //  calculate the absolute velodity
     object_ptr->velocity(0) = velocity_w[0] + main_velocity(0);
     object_ptr->velocity(1) = velocity_w[1] + main_velocity(1);
@@ -77,11 +82,6 @@ void ObjectBuilder::Build(const ContiRadar &raw_obstacles,
     object_ptr->length = 1.0;
     object_ptr->width = 1.0;
     object_ptr->height = 1.0;
-    //  int cls = raw_obstacles.contiobs(i).obstacle_class();
-    //  if (cls == CONTI_CAR) {
-    //    object_ptr->length = raw_obstacles.contiobs(i).length();
-    //    object_ptr->width = raw_obstacles.contiobs(i).width();
-    //  }
     object_ptr->type = UNKNOWN;
 
     Eigen::Matrix3d dist_rms;