Prediction: update logic in junction

modules/prediction/common/prediction_gflags.cc

@@ -49,6 +49,8 @@ DEFINE_double(replay_timestamp_gap, 10.0,
 
 // Map
 DEFINE_double(lane_search_radius, 3.0, "Search radius for a candidate lane");
+DEFINE_double(lane_search_radius_in_junction, 15.0,
+              "Search radius for a candidate lane");
 DEFINE_double(junction_search_radius, 1.0, "Search radius for a junction");
 
 // Obstacle features
@@ -78,6 +80,12 @@ DEFINE_int32(max_num_current_lane, 2, "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_int32(max_num_current_lane_in_junction, 1,
+             "Max number to search current lanes");
+DEFINE_int32(max_num_nearby_lane_in_junction, 0,
+             "Max number to search nearby lanes");
+DEFINE_double(max_lane_angle_diff_in_junction, M_PI / 6.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");
 DEFINE_double(pedestrian_max_speed, 10.0, "speed upper bound for pedestrian");

modules/prediction/common/prediction_gflags.h

@@ -40,6 +40,7 @@ DECLARE_double(replay_timestamp_gap);
 
 // Map
 DECLARE_double(lane_search_radius);
+DECLARE_double(lane_search_radius_in_junction);
 DECLARE_double(junction_search_radius);
 
 // Obstacle features
@@ -62,6 +63,9 @@ DECLARE_double(target_lane_gap);
 DECLARE_int32(max_num_current_lane);
 DECLARE_int32(max_num_nearby_lane);
 DECLARE_double(max_lane_angle_diff);
+DECLARE_int32(max_num_current_lane_in_junction);
+DECLARE_int32(max_num_nearby_lane_in_junction);
+DECLARE_double(max_lane_angle_diff_in_junction);
 DECLARE_bool(enable_pedestrian_acc);
 DECLARE_double(coeff_mul_sigma);
 DECLARE_double(pedestrian_max_speed);

modules/prediction/common/prediction_map.cc

@@ -27,6 +27,7 @@
 
 #include "modules/common/math/linear_interpolation.h"
 #include "modules/common/math/vec2d.h"
+#include "modules/common/math/polygon2d.h"
 #include "modules/map/hdmap/hdmap_util.h"
 #include "modules/map/proto/map_id.pb.h"
 #include "modules/prediction/common/prediction_gflags.h"
@@ -34,6 +35,8 @@
 namespace apollo {
 namespace prediction {
 
+using apollo::common::math::Polygon2d;
+using apollo::common::math::Vec2d;
 using apollo::hdmap::HDMapUtil;
 using apollo::hdmap::Id;
 using apollo::hdmap::JunctionInfo;
@@ -130,21 +133,19 @@ bool PredictionMap::OnVirtualLane(const Eigen::Vector2d& point,
 void PredictionMap::OnLane(
     const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
     const Eigen::Vector2d& point, const double heading, const double radius,
-    const bool on_lane, std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
+    const bool on_lane, const int max_num_lane,
+    const double max_lane_angle_diff,
+    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
   std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
 
   common::PointENU hdmap_point;
   hdmap_point.set_x(point[0]);
   hdmap_point.set_y(point[1]);
   if (HDMapUtil::BaseMap().GetLanesWithHeading(hdmap_point, radius, heading,
-                                               FLAGS_max_lane_angle_diff,
+                                               max_lane_angle_diff,
                                                &candidate_lanes) != 0) {
     return;
   }
-  int max_num_lane = FLAGS_max_num_current_lane;
-  if (!on_lane) {
-    max_num_lane = FLAGS_max_num_nearby_lane;
-  }
 
   const common::math::Vec2d vec_point(point[0], point[1]);
   std::vector<std::pair<std::shared_ptr<const LaneInfo>, double>> lane_pairs;
@@ -168,7 +169,7 @@ void PredictionMap::OnLane(
     double nearest_point_heading = PathHeading(candidate_lane, nearest_point);
     double diff =
         std::fabs(common::math::AngleDiff(heading, nearest_point_heading));
-    if (diff <= FLAGS_max_lane_angle_diff) {
+    if (diff <= max_lane_angle_diff) {
       lane_pairs.emplace_back(candidate_lane, diff);
     }
   }
@@ -211,6 +212,33 @@ std::vector<std::shared_ptr<const JunctionInfo>> PredictionMap::GetJunctions(
   return junctions;
 }
 
+bool PredictionMap::InJunction(
+    const Eigen::Vector2d& point, const double radius) {
+  auto junction_infos = GetJunctions(point, radius);
+  Vec2d vec(point[0], point[1]);
+  if (junction_infos.empty()) {
+    return false;
+  }
+  for (const auto junction_info : junction_infos) {
+    if (junction_info == nullptr ||
+        !junction_info->junction().has_polygon()) {
+      continue;
+    }
+    std::vector<Vec2d> vertices;
+    for (const auto& point : junction_info->junction().polygon().point()) {
+      vertices.emplace_back(point.x(), point.y());
+    }
+    if (vertices.size() < 3) {
+      continue;
+    }
+    Polygon2d junction_polygon{vertices};
+    if (junction_polygon.IsPointIn(vec)) {
+      return true;
+    }
+  }
+  return false;
+}
+
 double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info,
                                   const common::PointENU& point) {
   common::math::Vec2d vec_point = {point.x(), point.y()};
@@ -237,10 +265,12 @@ bool PredictionMap::SmoothPointFromLane(const std::string& id, const double s,
 void PredictionMap::NearbyLanesByCurrentLanes(
     const Eigen::Vector2d& point, const double heading, const double radius,
     const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
+    const int max_num_lane,
     std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
   if (lanes.size() == 0) {
     std::vector<std::shared_ptr<const LaneInfo>> prev_lanes(0);
-    OnLane(prev_lanes, point, heading, radius, false, nearby_lanes);
+    OnLane(prev_lanes, point, heading, radius, false, max_num_lane,
+           FLAGS_max_lane_angle_diff, nearby_lanes);
   } else {
     std::unordered_set<std::string> lane_ids;
     for (auto& lane_ptr : lanes) {

modules/prediction/common/prediction_map.h

@@ -129,7 +129,8 @@ class PredictionMap {
   static void OnLane(
       const std::vector<std::shared_ptr<const hdmap::LaneInfo>>& prev_lanes,
       const Eigen::Vector2d& point, const double heading, const double radius,
-      const bool on_lane,
+      const bool on_lane, const int max_num_lane,
+      const double max_lane_angle_diff,
       std::vector<std::shared_ptr<const hdmap::LaneInfo>>* lanes);
 
   /**
@@ -141,6 +142,14 @@ class PredictionMap {
    */
   static bool NearJunction(const Eigen::Vector2d& point, const double radius);
 
+   /**
+   * @brief Check if the obstacle is in a junction.
+   * @param point position
+   * @param radius the radius to search candidate junctions
+   * @return If the obstacle is in a junction.
+   */
+  static bool InJunction(const Eigen::Vector2d& point, const double radius);
+
   /**
    * @brief Get a list of junctions given a point and a search radius
    * @param Point
@@ -183,6 +192,7 @@ class PredictionMap {
   static void NearbyLanesByCurrentLanes(
       const Eigen::Vector2d& point, const double heading, const double radius,
       const std::vector<std::shared_ptr<const hdmap::LaneInfo>>& lanes,
+      const int max_num_lane,
       std::vector<std::shared_ptr<const hdmap::LaneInfo>>* nearby_lanes);
 
   /**

modules/prediction/common/prediction_map_test.cc

@@ -19,6 +19,7 @@
 #include "gtest/gtest.h"
 
 #include "modules/prediction/common/kml_map_based_test.h"
+#include "modules/prediction/common/prediction_gflags.h"
 
 namespace apollo {
 namespace prediction {
@@ -119,14 +120,18 @@ TEST_F(PredictionMapTest, on_lane) {
 
   // on lane without previous lanes
   std::vector<std::shared_ptr<const LaneInfo>> curr_lanes(0);
-  PredictionMap::OnLane(prev_lanes, point, heading, radius, true, &curr_lanes);
+  PredictionMap::OnLane(prev_lanes, point, heading, radius, true,
+                        FLAGS_max_num_current_lane,
+                        FLAGS_max_lane_angle_diff, &curr_lanes);
   EXPECT_EQ(1, curr_lanes.size());
   EXPECT_EQ("l20", curr_lanes[0]->id().id());
 
   // on lane with previous lanes
   prev_lanes.emplace_back(PredictionMap::LaneById("l10"));
   curr_lanes.clear();
-  PredictionMap::OnLane(prev_lanes, point, heading, radius, true, &curr_lanes);
+  PredictionMap::OnLane(prev_lanes, point, heading, radius, true,
+                        FLAGS_max_num_current_lane,
+                        FLAGS_max_lane_angle_diff, &curr_lanes);
   EXPECT_EQ(0, curr_lanes.size());
 
   // off lane without previous lanes
@@ -134,7 +139,9 @@ TEST_F(PredictionMapTest, on_lane) {
   point(0) = 124.85931;
   point(1) = 357.52733;
   curr_lanes.clear();
-  PredictionMap::OnLane(prev_lanes, point, heading, radius, true, &curr_lanes);
+  PredictionMap::OnLane(prev_lanes, point, heading, radius, true,
+                        FLAGS_max_num_current_lane,
+                        FLAGS_max_lane_angle_diff, &curr_lanes);
   EXPECT_EQ(0, curr_lanes.size());
 }
 
@@ -170,7 +177,7 @@ TEST_F(PredictionMapTest, get_nearby_lanes_by_current_lanes) {
   double radius = 6.0;
   double theta = -0.061427808505166936;
   PredictionMap::NearbyLanesByCurrentLanes(point, theta, radius, curr_lanes,
-                                           &nearby_lanes);
+      FLAGS_max_num_nearby_lane, &nearby_lanes);
   EXPECT_EQ(1, nearby_lanes.size());
   EXPECT_EQ("l21", nearby_lanes[0]->id().id());
 
@@ -178,7 +185,7 @@ TEST_F(PredictionMapTest, get_nearby_lanes_by_current_lanes) {
   nearby_lanes.clear();
   radius = 0.5;
   PredictionMap::NearbyLanesByCurrentLanes(point, theta, radius, curr_lanes,
-                                           &nearby_lanes);
+      FLAGS_max_num_nearby_lane, &nearby_lanes);
   EXPECT_EQ(0, nearby_lanes.size());
 
   // without current lanes
@@ -186,7 +193,7 @@ TEST_F(PredictionMapTest, get_nearby_lanes_by_current_lanes) {
   nearby_lanes.clear();
   radius = 5.0;
   PredictionMap::NearbyLanesByCurrentLanes(point, theta, radius, curr_lanes,
-                                           &nearby_lanes);
+      FLAGS_max_num_nearby_lane, &nearby_lanes);
   EXPECT_EQ(2, nearby_lanes.size());
   EXPECT_EQ("l20", nearby_lanes[0]->id().id());
   EXPECT_EQ("l21", nearby_lanes[1]->id().id());

modules/prediction/conf/prediction.conf

@@ -2,7 +2,7 @@
 --prediction_conf_file=modules/prediction/conf/prediction_conf.pb.txt
 
 --adjust_velocity_by_obstacle_heading
---adjust_velocity_by_position_shift
+--noadjust_velocity_by_position_shift
 --noenable_kf_tracking
 --noprediction_offline_mode
 

modules/prediction/container/obstacles/obstacle.cc

@@ -680,9 +680,18 @@ void Obstacle::UpdateKFPedestrianTracker(const Feature& feature) {
 void Obstacle::SetCurrentLanes(Feature* feature) {
   Eigen::Vector2d point(feature->position().x(), feature->position().y());
   double heading = feature->velocity_heading();
+  int max_num_lane = FLAGS_max_num_current_lane;
+  double max_angle_diff = FLAGS_max_lane_angle_diff;
+  double lane_search_radius = FLAGS_lane_search_radius;
+  if (PredictionMap::InJunction(point, FLAGS_junction_search_radius)) {
+    max_num_lane = FLAGS_max_num_current_lane_in_junction;
+    max_angle_diff = FLAGS_max_lane_angle_diff_in_junction;
+    lane_search_radius = FLAGS_lane_search_radius_in_junction;
+  }
   std::vector<std::shared_ptr<const LaneInfo>> current_lanes;
   PredictionMap::OnLane(current_lanes_, point, heading,
-                        FLAGS_lane_search_radius, true, &current_lanes);
+                        lane_search_radius, true, max_num_lane,
+                        max_angle_diff, &current_lanes);
   current_lanes_ = current_lanes;
   if (current_lanes_.empty()) {
     ADEBUG << "Obstacle [" << id_ << "] has no current lanes.";
@@ -743,10 +752,15 @@ void Obstacle::SetCurrentLanes(Feature* feature) {
 
 void Obstacle::SetNearbyLanes(Feature* feature) {
   Eigen::Vector2d point(feature->position().x(), feature->position().y());
+  int max_num_lane = FLAGS_max_num_nearby_lane;
+  if (PredictionMap::InJunction(point, FLAGS_junction_search_radius)) {
+    max_num_lane = FLAGS_max_num_nearby_lane_in_junction;
+  }
   double theta = feature->velocity_heading();
   std::vector<std::shared_ptr<const LaneInfo>> nearby_lanes;
   PredictionMap::NearbyLanesByCurrentLanes(
-      point, theta, FLAGS_lane_search_radius, current_lanes_, &nearby_lanes);
+      point, theta, FLAGS_lane_search_radius, current_lanes_,
+      max_num_lane, &nearby_lanes);
   if (nearby_lanes.empty()) {
     ADEBUG << "Obstacle [" << id_ << "] has no nearby lanes.";
     return;