Prediction: integrate validation checker

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.");

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);

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;

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",

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));