From b85ea0aaa598b76eefb1609958af291525b551f6 Mon Sep 17 00:00:00 2001
From: kechxu <kechxu89@gmail.com>
Date: Thu, 8 Mar 2018 10:21:50 -0800
Subject: [PATCH] Prediction: integrate validation checker

---
 modules/prediction/common/prediction_gflags.cc         |  2 ++
 modules/prediction/common/prediction_gflags.h          |  1 +
 modules/prediction/common/validation_checker.cc        |  7 ++++++-
 modules/prediction/predictor/lane_sequence/BUILD       |  1 +
 .../predictor/lane_sequence/lane_sequence_predictor.cc | 10 ++++++++++
 5 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc
index 032b28b321..c105b6a780 100644
--- a/modules/prediction/common/prediction_gflags.cc
+++ b/modules/prediction/common/prediction_gflags.cc
@@ -118,6 +118,8 @@ DEFINE_bool(enable_lane_sequence_acc, false,
 DEFINE_bool(enable_trim_prediction_trajectory, false,
             "If trim the prediction trajectory to avoid crossing"
             "protected adc planning trajectory.");
+DEFINE_bool(enable_trajectory_validation_check, false,
+            "If check the validity of prediction trajectory.");
 DEFINE_double(distance_beyond_junction, 0.5,
               "If the obstacle is in junction more than this threshold,"
               "consider it in junction.");
diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h
index fe817881d0..4f0f60a765 100644
--- a/modules/prediction/common/prediction_gflags.h
+++ b/modules/prediction/common/prediction_gflags.h
@@ -86,6 +86,7 @@ DECLARE_double(lane_sequence_threshold);
 DECLARE_double(lane_change_dist);
 DECLARE_bool(enable_lane_sequence_acc);
 DECLARE_bool(enable_trim_prediction_trajectory);
+DECLARE_bool(enable_trajectory_validation_check);
 DECLARE_double(distance_beyond_junction);
 DECLARE_double(adc_trajectory_search_length);
 DECLARE_double(virtual_lane_radius);
diff --git a/modules/prediction/common/validation_checker.cc b/modules/prediction/common/validation_checker.cc
index aed6cac90b..bec760e299 100644
--- a/modules/prediction/common/validation_checker.cc
+++ b/modules/prediction/common/validation_checker.cc
@@ -38,8 +38,13 @@ bool ValidationChecker::ValidCentripedalAcceleration(
     const auto& second_point = trajectory_points[i + 1];
     double theta_diff = std::abs(second_point.path_point().theta() -
                                  first_point.path_point().theta());
+    double time_diff = std::abs(second_point.relative_time() -
+                                first_point.relative_time());
+    if (time_diff < FLAGS_double_precision) {
+      continue;
+    }
     double v = (first_point.v() + second_point.v()) / 2.0;
-    double centripedal_acc = v * theta_diff;
+    double centripedal_acc = v * theta_diff / time_diff;
     max_centripedal_acc = std::max(max_centripedal_acc, centripedal_acc);
   }
   return max_centripedal_acc < FLAGS_centripedal_acc_threshold;
diff --git a/modules/prediction/predictor/lane_sequence/BUILD b/modules/prediction/predictor/lane_sequence/BUILD
index 212b4f9820..8246043e5f 100644
--- a/modules/prediction/predictor/lane_sequence/BUILD
+++ b/modules/prediction/predictor/lane_sequence/BUILD
@@ -14,6 +14,7 @@ cc_library(
         "//modules/prediction/common:prediction_gflags",
         "//modules/prediction/common:prediction_map",
         "//modules/prediction/common:prediction_util",
+        "//modules/prediction/common:validation_checker",
         "//modules/prediction/predictor/sequence:sequence_predictor",
         "//modules/prediction/proto:lane_graph_proto",
         "@eigen//:eigen",
diff --git a/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc b/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc
index 53c7fac3ba..792eae2690 100644
--- a/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc
+++ b/modules/prediction/predictor/lane_sequence/lane_sequence_predictor.cc
@@ -24,6 +24,7 @@
 #include "modules/prediction/common/prediction_gflags.h"
 #include "modules/prediction/common/prediction_map.h"
 #include "modules/prediction/common/prediction_util.h"
+#include "modules/prediction/common/validation_checker.h"
 
 namespace apollo {
 namespace prediction {
@@ -82,6 +83,15 @@ void LaneSequencePredictor::Predict(Obstacle* obstacle) {
         *obstacle, sequence, FLAGS_prediction_duration,
         FLAGS_prediction_period, &points);
 
+    if (points.empty()) {
+      continue;
+    }
+
+    if (FLAGS_enable_trajectory_validation_check &&
+        !ValidationChecker::ValidCentripedalAcceleration(points)) {
+      continue;
+    }
+
     Trajectory trajectory = GenerateTrajectory(points);
     trajectory.set_probability(sequence.probability());
     trajectories_.push_back(std::move(trajectory));
-- 
GitLab