Prediction: refactor lane feature searching

modules/prediction/common/prediction_gflags.cc

@@ -64,7 +64,9 @@ DEFINE_double(still_obstacle_position_std, 1.0,
               "Position standard deviation for still obstacles");
 DEFINE_double(max_history_time, 7.0, "Obstacles' maximal historical time.");
 DEFINE_double(target_lane_gap, 2.0, "gap between two lane points.");
-DEFINE_double(max_lane_angle_diff, M_PI / 4.0,
+DEFINE_int32(max_num_current_lane, 1, "Max number to search current lanes");
+DEFINE_int32(max_num_nearby_lane, 2, "Max number to search nearby lanes");
+DEFINE_double(max_lane_angle_diff, M_PI / 2.0,
               "Max angle difference for a candiate lane");
 DEFINE_bool(enable_pedestrian_acc, false, "Enable calculating speed by acc");
 DEFINE_double(coeff_mul_sigma, 2.0, "coefficient multiply standard deviation");

modules/prediction/common/prediction_gflags.h

@@ -52,6 +52,8 @@ DECLARE_double(still_pedestrian_speed_threshold);
 DECLARE_double(still_obstacle_position_std);
 DECLARE_double(max_history_time);
 DECLARE_double(target_lane_gap);
+DECLARE_int32(max_num_current_lane);
+DECLARE_int32(max_num_nearby_lane);
 DECLARE_double(max_lane_angle_diff);
 DECLARE_bool(enable_pedestrian_acc);
 DECLARE_double(coeff_mul_sigma);

modules/prediction/common/prediction_map.cc

@@ -20,8 +20,10 @@
 #include <iomanip>
 #include <memory>
 #include <string>
+#include <utility>
 #include <unordered_set>
 #include <vector>
+#include <algorithm>
 
 #include "modules/map/proto/map_id.pb.h"
 
@@ -136,6 +138,7 @@ void PredictionMap::OnLane(
   }
 
   const common::math::Vec2d vec_point(point[0], point[1]);
+  std::vector<std::pair<std::shared_ptr<const LaneInfo>, double>> lane_pairs;
   for (const auto& candidate_lane : candidate_lanes) {
     if (candidate_lane == nullptr) {
       continue;
@@ -156,9 +159,20 @@ void PredictionMap::OnLane(
     double diff =
         std::fabs(common::math::AngleDiff(heading, nearest_point_heading));
     if (diff <= FLAGS_max_lane_angle_diff) {
-      lanes->push_back(candidate_lane);
+      lane_pairs.emplace_back(candidate_lane, diff);
     }
   }
+  if (lane_pairs.empty()) {
+    return;
+  }
+  std::sort(lane_pairs.begin(), lane_pairs.end(),
+      [](const std::pair<std::shared_ptr<const LaneInfo>, double>& p1,
+         const std::pair<std::shared_ptr<const LaneInfo>, double>& p2) {
+        return p1.second < p2.second;
+      });
+  for (const auto& lane_pair : lane_pairs) {
+    lanes->push_back(lane_pair.first);
+  }
 }
 
 bool PredictionMap::NearJunction(const Eigen::Vector2d& point,

modules/prediction/common/prediction_map_test.cc

@@ -187,8 +187,8 @@ TEST_F(PredictionMapTest, get_nearby_lanes_by_current_lanes) {
   map_->NearbyLanesByCurrentLanes(point, theta, radius, curr_lanes,
                                   &nearby_lanes);
   EXPECT_EQ(2, nearby_lanes.size());
-  EXPECT_EQ("l21", nearby_lanes[0]->id().id());
-  EXPECT_EQ("l20", nearby_lanes[1]->id().id());
+  EXPECT_EQ("l20", nearby_lanes[0]->id().id());
+  EXPECT_EQ("l21", nearby_lanes[1]->id().id());
 }
 
 TEST_F(PredictionMapTest, neighbor_lane_detection) {

modules/prediction/container/obstacles/obstacle.cc

@@ -890,11 +890,15 @@ void Obstacle::SetLaneGraphFeature(Feature* feature) {
       speed * FLAGS_prediction_duration +
       0.5 * acc * FLAGS_prediction_duration * FLAGS_prediction_duration +
       FLAGS_min_prediction_length;
+  int curr_lane_count = 0;
   for (auto& lane : feature->lane().current_lane_feature()) {
     std::shared_ptr<const LaneInfo> lane_info = map->LaneById(lane.lane_id());
     RoadGraph road_graph(lane.lane_s(), road_graph_distance, lane_info);
     LaneGraph lane_graph;
     road_graph.BuildLaneGraph(&lane_graph);
+    if (lane_graph.lane_sequence_size() > 0) {
+      ++curr_lane_count;
+    }
     int seq_id =
         feature->mutable_lane()->mutable_lane_graph()->lane_sequence_size();
     for (const auto& lane_seq : lane_graph.lane_sequence()) {
@@ -910,12 +914,20 @@ void Obstacle::SetLaneGraphFeature(Feature* feature) {
       ADEBUG << "Obstacle [" << id_ << "] set a lane sequence ["
              << lane_seq.ShortDebugString() << "].";
     }
+    if (curr_lane_count >= FLAGS_max_num_current_lane) {
+      break;
+    }
   }
+
+  int nearby_lane_count = 0;
   for (auto& lane : feature->lane().nearby_lane_feature()) {
     std::shared_ptr<const LaneInfo> lane_info = map->LaneById(lane.lane_id());
     RoadGraph road_graph(lane.lane_s(), road_graph_distance, lane_info);
     LaneGraph lane_graph;
     road_graph.BuildLaneGraph(&lane_graph);
+    if (lane_graph.lane_sequence_size() > 0) {
+      ++nearby_lane_count;
+    }
     int seq_id =
         feature->mutable_lane()->mutable_lane_graph()->lane_sequence_size();
     for (const auto& lane_seq : lane_graph.lane_sequence()) {
@@ -930,6 +942,9 @@ void Obstacle::SetLaneGraphFeature(Feature* feature) {
       ADEBUG << "Obstacle [" << id_ << "] set a lane sequence ["
              << lane_seq.ShortDebugString() << "].";
     }
+    if (nearby_lane_count >= FLAGS_max_num_nearby_lane) {
+      break;
+    }
   }
 
   if (feature->has_lane() && feature->lane().has_lane_graph()) {