Prediction: implement simplified lane sequence

modules/prediction/common/prediction_gflags.cc

@@ -162,6 +162,8 @@ DEFINE_double(distance_threshold_to_junction_exit, 1.0,
               "Threshold of distance to junction exit");
 DEFINE_double(angle_threshold_to_junction_exit, M_PI * 0.25,
               "Threshold of angle to junction exit");
+DEFINE_uint32(sample_size_for_average_lane_curvature, 10,
+              "The sample size to compute average lane curvature");
 
 // Validation checker
 DEFINE_double(centripetal_acc_coeff, 0.5,

modules/prediction/common/prediction_gflags.h

@@ -102,6 +102,7 @@ DECLARE_double(heading_filter_param);
 DECLARE_uint64(max_num_lane_point);
 DECLARE_double(distance_threshold_to_junction_exit);
 DECLARE_double(angle_threshold_to_junction_exit);
+DECLARE_uint32(sample_size_for_average_lane_curvature);
 
 // Validation checker
 DECLARE_double(centripetal_acc_coeff);

modules/prediction/common/prediction_map.cc

@@ -51,6 +51,10 @@ double PredictionMap::HeadingOnLane(
 double PredictionMap::CurvatureOnLane(const std::string& lane_id,
                                       const double s) {
   std::shared_ptr<const hdmap::LaneInfo> lane_info = LaneById(lane_id);
+  if (lane_info == nullptr) {
+    AERROR << "Null lane_info ptr found";
+    return 0.0;
+  }
   return lane_info->Curvature(s);
 }
 

modules/prediction/common/road_graph.cc

@@ -178,30 +178,77 @@ void RoadGraph::ComputeLaneSequence(
     // Sort the successor lane_segments from left to right.
     std::vector<std::shared_ptr<const hdmap::LaneInfo>> successor_lanes;
     for (const auto& successor_lane_id : lane_info_ptr->lane().successor_id()) {
-      successor_lanes.push_back(
+      successor_lanes.emplace_back(
           PredictionMap::LaneById(successor_lane_id.id()));
     }
     std::sort(successor_lanes.begin(), successor_lanes.end(), IsAtLeft);
 
-    if (consider_divide) {
-      if (successor_lanes.size() > 1) {
-        // Run recursion function to perform DFS.
-        for (size_t i = 0; i < successor_lanes.size(); i++) {
-          ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lanes[i],
-                              graph_search_horizon - 1,  false, lane_segments,
-                              lane_graph_ptr);
+    if (!successor_lanes.empty()) {
+      if (consider_divide) {
+        if (successor_lanes.size() > 1) {
+          // Run recursion function to perform DFS.
+          for (size_t i = 0; i < successor_lanes.size(); i++) {
+            ComputeLaneSequence(
+                successor_accumulated_s, 0.0, successor_lanes[i],
+                graph_search_horizon - 1,  false, lane_segments,
+                lane_graph_ptr);
+          }
+        } else {
+          ComputeLaneSequence(
+              successor_accumulated_s, 0.0, successor_lanes[0],
+              graph_search_horizon - 1,  true, lane_segments,
+              lane_graph_ptr);
         }
       } else {
-        ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lanes[0],
-                            graph_search_horizon - 1,  true, lane_segments,
-                            lane_graph_ptr);
+        auto selected_successor_lane =
+            LaneWithSmallestAverageCurvature(successor_lanes);
+          ComputeLaneSequence(
+              successor_accumulated_s, 0.0, selected_successor_lane,
+              graph_search_horizon - 1,  false, lane_segments,
+              lane_graph_ptr);
       }
-    } else {
-      // TODO(kechxu) select a successor lane
     }
   }
   lane_segments->pop_back();
 }
 
+std::shared_ptr<const hdmap::LaneInfo>
+RoadGraph::LaneWithSmallestAverageCurvature(
+    const std::vector<std::shared_ptr<const hdmap::LaneInfo>>& lane_infos)
+const {
+  CHECK(!lane_infos.empty());
+  size_t sample_size = FLAGS_sample_size_for_average_lane_curvature;
+  std::shared_ptr<const hdmap::LaneInfo> selected_lane_info = lane_infos[0];
+  double smallest_curvature = AverageCurvature(
+      selected_lane_info->id().id(), sample_size);
+  for (size_t i = 1; i < lane_infos.size(); ++i) {
+    std::shared_ptr<const hdmap::LaneInfo> lane_info = lane_infos[i];
+    double curvature = AverageCurvature(lane_info->id().id(), sample_size);
+    if (curvature < smallest_curvature) {
+      smallest_curvature = curvature;
+      selected_lane_info = lane_info;
+    }
+  }
+  return selected_lane_info;
+}
+
+double RoadGraph::AverageCurvature(
+    const std::string& lane_id, const size_t sample_size) const {
+  CHECK_GT(sample_size, 0);
+  std::shared_ptr<const hdmap::LaneInfo> lane_info_ptr =
+      PredictionMap::LaneById(lane_id);
+  if (lane_info_ptr == nullptr) {
+    return 0.0;
+  }
+  double lane_length = lane_info_ptr->total_length();
+  double s_gap = lane_length / static_cast<double>(sample_size);
+  double curvature_sum = 0.0;
+  for (size_t i = 0; i < sample_size; ++i) {
+    double s = s_gap * static_cast<double>(i);
+    curvature_sum += PredictionMap::CurvatureOnLane(lane_id, s);
+  }
+  return curvature_sum / static_cast<double>(sample_size);
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/common/road_graph.h

@@ -17,6 +17,7 @@
 #pragma once
 
 #include <memory>
+#include <string>
 #include <vector>
 
 #include "modules/prediction/proto/lane_graph.pb.h"
@@ -78,6 +79,25 @@ class RoadGraph {
       std::vector<LaneSegment>* const lane_segments,
       LaneGraph* const lane_graph_ptr) const;
 
+  /**
+   * @brief Get the pointer to the lane with the smallest average curvature
+   * @param The vector of lane infos
+   * @return The pointer to the lane with the smallest average curvature
+   */
+  std::shared_ptr<const hdmap::LaneInfo>
+  LaneWithSmallestAverageCurvature(
+      const std::vector<std::shared_ptr<const hdmap::LaneInfo>>& lane_infos)
+  const;
+
+  /**
+   * @brief Get the average curvature along a lane with the ID lane_id
+   * @param The ID of the lane
+   * @param The size of samples alone the lane to compute the average curvature
+   * @return The average curvature
+   */
+  double AverageCurvature(const std::string& lane_id,
+                          const size_t sample_size) const;
+
  private:
   // The s of the obstacle on its own lane_segment.
   double start_s_ = 0;

modules/prediction/container/obstacles/obstacle.cc

@@ -988,8 +988,7 @@ void Obstacle::BuildLaneGraph() {
   }
   double speed = feature->speed();
   double t_max = FLAGS_prediction_trajectory_time_length;
-  double a_max = FLAGS_vehicle_max_linear_acc;
-  auto estimated_move_distance = speed * t_max + 0.5 * a_max * t_max * t_max;
+  auto estimated_move_distance = speed * t_max;
 
   double road_graph_search_distance = std::fmax(
       estimated_move_distance, FLAGS_min_prediction_trajectory_spatial_length);