Added prediction map unit test and fixed prediction map bugs

modules/prediction/common/prediction_map.cc

@@ -20,6 +20,7 @@
 #include <unordered_set>
 #include <vector>
 #include <iomanip>
+#include <iostream>
 
 #include "modules/common/math/linear_interpolation.h"
 #include "modules/common/math/vec2d.h"
@@ -75,9 +76,9 @@ double PredictionMap::HeadingOnLane(std::shared_ptr<const LaneInfo> lane_info,
   }
   const auto low_itr =
       std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
-  CHECK(low_itr != accumulated_s.end());
+  // CHECK(low_itr != accumulated_s.end());
   size_t index = low_itr - accumulated_s.begin();
-  if (index == size - 1) {
+  if (index >= size - 1) {
     return headings.back();
   }
   return apollo::common::math::slerp(headings[index], accumulated_s[index],
@@ -134,40 +135,44 @@ void PredictionMap::OnLane(
     const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
     const Eigen::Vector2d& point, const double heading,
     const double radius,
-    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
+    std::vector<std::shared_ptr<const LaneInfo>>* lanes,
+    bool on_lane) {
   std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
+
   // TODO(kechxu) clean the messy code of this function
   apollo::common::PointENU hdmap_point;
   hdmap_point.set_x(point[0]);
   hdmap_point.set_y(point[1]);
+  if (hdmap_->get_lanes_with_heading(
+        hdmap_point, radius, heading, M_PI, &candidate_lanes) != 0) {
+    return;
+  }
+
   apollo::common::math::Vec2d vec_point;
   vec_point.set_x(point[0]);
   vec_point.set_y(point[1]);
-  if (hdmap_->get_lanes_with_heading(hdmap_point, radius, heading, M_PI,
-                                     &candidate_lanes) != 0) {
-    return;
-  }
-  for (auto candidate_lane : candidate_lanes) {
+  for (const auto &candidate_lane : candidate_lanes) {
     if (candidate_lane == nullptr) {
       continue;
-    } else if (!candidate_lane->is_on_lane(vec_point)) {
+    }
+    if (on_lane && !candidate_lane->is_on_lane(vec_point)) {
       continue;
-    } else if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
-               !IsSuccessorLane(candidate_lane, prev_lanes) &&
-               !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
-               !IsRightNeighborLane(candidate_lane, prev_lanes)) {
+    }
+    if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
+        !IsSuccessorLane(candidate_lane, prev_lanes) &&
+        !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
+        !IsRightNeighborLane(candidate_lane, prev_lanes)) {
       continue;
-    } else {
-      double distance = 0.0;
-      apollo::common::PointENU nearest_point =
-          candidate_lane->get_nearest_point(vec_point, &distance);
-      double nearest_point_heading =
-          PathHeading(candidate_lane, nearest_point);
-      double diff = std::fabs(
-          apollo::common::math::AngleDiff(heading, nearest_point_heading));
-      if (diff <= FLAGS_max_lane_angle_diff) {
-        lanes->push_back(candidate_lane);
-      }
+    }
+    double distance = 0.0;
+    apollo::common::PointENU nearest_point =
+        candidate_lane->get_nearest_point(vec_point, &distance);
+    double nearest_point_heading =
+        PathHeading(candidate_lane, nearest_point);
+    double diff = std::fabs(
+        apollo::common::math::AngleDiff(heading, nearest_point_heading));
+    if (diff <= FLAGS_max_lane_angle_diff) {
+      lanes->push_back(candidate_lane);
     }
   }
 }
@@ -201,28 +206,49 @@ int PredictionMap::SmoothPointFromLane(const apollo::hdmap::Id& id,
 
 void PredictionMap::NearbyLanesByCurrentLanes(
     const Eigen::Vector2d& point,
+    double heading,
+    double radius,
     const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
     std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
-  std::unordered_set<std::string> lane_ids;
-  for (auto& lane_ptr : lanes) {
-    if (lane_ptr == nullptr) {
-      continue;
-    }
-    for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
-      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
+  if (lanes.size() == 0) {
+    std::vector<std::shared_ptr<const LaneInfo>> prev_lanes(0);
+    OnLane(prev_lanes, point, heading, radius, nearby_lanes, false);
+  } else {
+    std::unordered_set<std::string> lane_ids;
+    for (auto& lane_ptr : lanes) {
+      if (lane_ptr == nullptr) {
         continue;
       }
-      lane_ids.insert(lane_id.id());
-      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
-      nearby_lanes->push_back(nearby_lane);
-    }
-    for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
-      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
-        continue;
+      for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
+        if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
+          continue;
+        }
+        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
+        double s = -1.0;
+        double l = 0.0;
+        GetProjection(point, nearby_lane, &s, &l);
+        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
+            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
+          continue;
+        }
+        lane_ids.insert(lane_id.id());  
+        nearby_lanes->push_back(nearby_lane);
+      }
+      for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
+        if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
+          continue;
+        }
+        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
+        double s = -1.0;
+        double l = 0.0;
+        GetProjection(point, nearby_lane, &s, &l);
+        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
+            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
+          continue;
+        }
+        lane_ids.insert(lane_id.id());
+        nearby_lanes->push_back(nearby_lane);
       }
-      lane_ids.insert(lane_id.id());
-      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
-      nearby_lanes->push_back(nearby_lane);
     }
   }
 }
@@ -356,7 +382,7 @@ bool PredictionMap::IsIdenticalLane(
     std::shared_ptr<const LaneInfo> other_lane,
     std::shared_ptr<const LaneInfo> curr_lane) {
   if (curr_lane == nullptr || other_lane == nullptr) {
-    return false;
+    return true;
   }
   return id_string(other_lane) == id_string(curr_lane);
 }

modules/prediction/common/prediction_map.h

@@ -74,7 +74,8 @@ class PredictionMap {
           prev_lanes,
       const Eigen::Vector2d& point, const double heading,
       const double radius,
-      std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>>* lanes);
+      std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>>* lanes,
+      bool on_lane = true);
 
   double PathHeading(
       std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info_ptr,
@@ -87,6 +88,8 @@ class PredictionMap {
 
   void NearbyLanesByCurrentLanes(
       const Eigen::Vector2d& point,
+      double heading,
+      double radius,
       const std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>>& lanes,
       std::vector<std::shared_ptr<const apollo::hdmap::LaneInfo>>*
           nearby_lanes);

modules/prediction/common/prediction_map_test.cc

@@ -23,16 +23,242 @@
 namespace apollo {
 namespace prediction {
 
+using ::apollo::hdmap::Id;
+using ::apollo::hdmap::LaneInfo;
+using ::apollo::hdmap::MapPathPoint;
+
 class PredictionMapTest : public ::testing::Test {
  public:
-  virtual void SetUp() {
+  void SetUp() override {
     FLAGS_map_file = "modules/prediction/testdata/kml_map.bin";
     map_ = PredictionMap::instance();
+    CHECK_NOTNULL(map_);
   }
 
  protected:
   PredictionMap *map_;
 };
 
+TEST_F(PredictionMapTest, set_id) {
+  Id id = map_->id("l20");
+  EXPECT_EQ("l20", id.id());
+}
+
+TEST_F(PredictionMapTest, get_lane_info) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+  EXPECT_TRUE(lane_info != nullptr);
+  EXPECT_EQ("l20", lane_info->id().id());
+
+  lane_info = map_->LaneById("l500");
+  EXPECT_TRUE(lane_info == nullptr);
+}
+
+TEST_F(PredictionMapTest, get_position_on_lane) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+
+  // on lane
+  Eigen::Vector2d position_on_lane = map_->PositionOnLane(lane_info, 10.0);
+  EXPECT_DOUBLE_EQ(124.85930930657942, position_on_lane(0));
+  EXPECT_DOUBLE_EQ(348.52732962417451, position_on_lane(1));
+
+  // beyond end of lane
+  Eigen::Vector2d position_off_lane = map_->PositionOnLane(lane_info, 1000.0);
+  EXPECT_DOUBLE_EQ(392.71861332684404, position_off_lane(0));
+  EXPECT_DOUBLE_EQ(286.16205764480401, position_off_lane(1));
+}
+
+TEST_F(PredictionMapTest, heading_on_lane) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+
+  // on lane
+  EXPECT_DOUBLE_EQ(-0.061427808505166936, map_->HeadingOnLane(lane_info, 10.0));
+
+  // beyond end of lane
+  EXPECT_DOUBLE_EQ(-0.2656845063517943, map_->HeadingOnLane(lane_info, 1000.0));
+}
+
+TEST_F(PredictionMapTest, get_lane_width) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+
+  // on lane
+  EXPECT_DOUBLE_EQ(2.9895597224833121, map_->LaneTotalWidth(lane_info, 10.0));
+
+  // beyond end of lane
+  EXPECT_DOUBLE_EQ(3.1943980708125523, map_->LaneTotalWidth(lane_info, 1000.0));
+  EXPECT_DOUBLE_EQ(3.1943980708125523, map_->LaneTotalWidth(lane_info, 1000.0));
+}
+
+TEST_F(PredictionMapTest, get_projection) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+
+  // on lane
+  Eigen::Vector2d position_on_lane(124.85931, 347.52733);
+  double s = 0.0;
+  double l = 0.0;
+  map_->GetProjection(position_on_lane, lane_info, &s, &l);
+  EXPECT_DOUBLE_EQ(10.061275933723756, s);
+  EXPECT_DOUBLE_EQ(-0.9981204878650296, l);
+
+  // off lane
+  Eigen::Vector2d position_off_lane(124.85931, 357.52733);
+  map_->GetProjection(position_off_lane, lane_info, &s, &l);
+  EXPECT_DOUBLE_EQ(9.4485232873738045, s);
+  EXPECT_DOUBLE_EQ(8.9830885668733345, l);
+}
+
+TEST_F(PredictionMapTest, get_map_pathpoint) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+  double s = 10.0;
+
+  // on lane
+  MapPathPoint point;
+  EXPECT_TRUE(map_->ProjectionFromLane(lane_info, s, &point));
+  EXPECT_DOUBLE_EQ(124.85930930657942, point.x());
+  EXPECT_DOUBLE_EQ(348.52732962417451, point.y());
+  EXPECT_DOUBLE_EQ(-0.061427808505166936, point.heading());
+
+  // off lane
+  s = 1000.0;
+  EXPECT_TRUE(map_->ProjectionFromLane(lane_info, s, &point));
+  EXPECT_DOUBLE_EQ(392.71861332684404, point.x());
+  EXPECT_DOUBLE_EQ(286.16205764480401, point.y());
+  EXPECT_DOUBLE_EQ(-0.2656845063517943, point.heading());
+
+  // non-existent lane
+  lane_info.reset();
+  s = 10.0;
+  EXPECT_FALSE(map_->ProjectionFromLane(lane_info, s, &point));
+}
+
+TEST_F(PredictionMapTest, on_lane) {
+  std::vector<std::shared_ptr<const LaneInfo>> prev_lanes(0);
+  Eigen::Vector2d point(124.85931, 347.52733);
+  double heading = 0.0;
+  double radius = 3.0;
+
+  // on lane without previous lanes
+  std::vector<std::shared_ptr<const LaneInfo>> curr_lanes(0);
+  map_->OnLane(prev_lanes, point, heading, radius, &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(map_->LaneById("l10"));
+  curr_lanes.clear();
+  map_->OnLane(prev_lanes, point, heading, radius, &curr_lanes);
+  EXPECT_EQ(0, curr_lanes.size());
+
+  // off lane without previous lanes
+  prev_lanes.clear();
+  point(0) = 124.85931;
+  point(1) = 357.52733;
+  curr_lanes.clear();
+  map_->OnLane(prev_lanes, point, heading, radius, &curr_lanes);
+  EXPECT_EQ(0, curr_lanes.size());
+}
+
+TEST_F(PredictionMapTest, get_path_heading) {
+  std::shared_ptr<const LaneInfo> lane_info = map_->LaneById("l20");
+  ::apollo::common::PointENU point;
+  point.set_x(124.85931);
+  point.set_y(347.52733);
+  EXPECT_DOUBLE_EQ(-0.061693188601892768, map_->PathHeading(lane_info, point));
+}
+
+TEST_F(PredictionMapTest, get_smooth_point_from_lane) {
+  Id id;
+  id.set_id("l20");
+  double s = 10.0;
+  double l = 0.0;
+  double heading = M_PI;
+
+  Eigen::Vector2d point;
+  EXPECT_EQ(0, map_->SmoothPointFromLane(id, s, l, &point, &heading));
+  EXPECT_DOUBLE_EQ(124.85930930657942, point.x());
+  EXPECT_DOUBLE_EQ(348.52732962417451, point.y());
+  EXPECT_DOUBLE_EQ(-0.061427808505166936, heading);
+}
+
+TEST_F(PredictionMapTest, get_nearby_lanes_by_current_lanes) {
+  std::vector<std::shared_ptr<const LaneInfo>> curr_lanes(0);
+  curr_lanes.emplace_back(map_->LaneById("l20"));
+  std::vector<std::shared_ptr<const LaneInfo>> nearby_lanes(0);
+
+  // large radius
+  Eigen::Vector2d point(124.85931, 348.52733);
+  double radius = 6.0;
+  double theta = -0.061427808505166936;
+  map_->NearbyLanesByCurrentLanes(
+      point, theta, radius, curr_lanes, &nearby_lanes);
+  EXPECT_EQ(1, nearby_lanes.size());
+  EXPECT_EQ("l21", nearby_lanes[0]->id().id());
+
+  // small radius
+  nearby_lanes.clear();
+  radius = 0.5;
+  map_->NearbyLanesByCurrentLanes(
+      point, theta, radius, curr_lanes, &nearby_lanes);
+  EXPECT_EQ(0, nearby_lanes.size());
+
+  // without current lanes
+  curr_lanes.clear();
+  nearby_lanes.clear();
+  radius = 5.0;
+  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());
+}
+
+TEST_F(PredictionMapTest, neighbor_lane_detection) {
+  std::vector<std::shared_ptr<const LaneInfo>> curr_lanes(0);
+
+  // empty current lanes
+  EXPECT_TRUE(map_->IsLeftNeighborLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_TRUE(map_->IsRightNeighborLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_TRUE(map_->IsSuccessorLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_TRUE(map_->IsPredecessorLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_TRUE(map_->IsIdenticalLane(map_->LaneById("l20"), curr_lanes));
+
+  // given current lanes
+  std::shared_ptr<const LaneInfo> curr_lane = map_->LaneById("l21");
+  curr_lanes.emplace_back(curr_lane);
+  EXPECT_TRUE(map_->IsLeftNeighborLane(map_->LaneById("l22"), curr_lanes));
+  EXPECT_FALSE(map_->IsRightNeighborLane(map_->LaneById("l22"), curr_lanes));
+  EXPECT_FALSE(map_->IsSuccessorLane(map_->LaneById("l22"), curr_lanes));
+  EXPECT_FALSE(map_->IsPredecessorLane(map_->LaneById("l22"), curr_lanes));
+  EXPECT_FALSE(map_->IsIdenticalLane(map_->LaneById("l22"), curr_lanes));
+
+  EXPECT_FALSE(map_->IsLeftNeighborLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_TRUE(map_->IsRightNeighborLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_FALSE(map_->IsSuccessorLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_FALSE(map_->IsPredecessorLane(map_->LaneById("l20"), curr_lanes));
+  EXPECT_FALSE(map_->IsIdenticalLane(map_->LaneById("l20"), curr_lanes));
+
+  EXPECT_FALSE(map_->IsLeftNeighborLane(map_->LaneById("l18"), curr_lanes));
+  EXPECT_FALSE(map_->IsRightNeighborLane(map_->LaneById("l18"), curr_lanes));
+  EXPECT_FALSE(map_->IsSuccessorLane(map_->LaneById("l18"), curr_lanes));
+  EXPECT_TRUE(map_->IsPredecessorLane(map_->LaneById("l18"), curr_lanes));
+  EXPECT_FALSE(map_->IsIdenticalLane(map_->LaneById("l18"), curr_lanes));
+
+  EXPECT_FALSE(map_->IsLeftNeighborLane(map_->LaneById("l99"), curr_lanes));
+  EXPECT_FALSE(map_->IsRightNeighborLane(map_->LaneById("l99"), curr_lanes));
+  EXPECT_TRUE(map_->IsSuccessorLane(map_->LaneById("l99"), curr_lanes));
+  EXPECT_FALSE(map_->IsPredecessorLane(map_->LaneById("l99"), curr_lanes));
+  EXPECT_FALSE(map_->IsIdenticalLane(map_->LaneById("l99"), curr_lanes));
+}
+
+TEST_F(PredictionMapTest, lane_turn_type) {
+  // Valid lane
+  EXPECT_EQ(1, map_->LaneTurnType("l20"));
+
+  // Invalid lane
+  EXPECT_FALSE(map_->LaneById("l500"));
+  EXPECT_EQ(1, map_->LaneTurnType("l500"));
+
+  EXPECT_EQ(3, map_->LaneTurnType("l5"));
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/container/obstacles/obstacle.cc

@@ -820,13 +820,16 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
   CHECK_NOTNULL(map);
 
   Eigen::Vector2d point(feature->position().x(), feature->position().y());
+  double theta = point.theta();
   if (FLAGS_enable_kf_tracking) {
     point[0] = feature->t_position().x();
     point[1] = feature->t_position().y();
+    theta = feature->t_velocity_heading();
   }
 
   std::vector<std::shared_ptr<const LaneInfo>> nearby_lanes;
-  map->NearbyLanesByCurrentLanes(point, current_lanes_, &nearby_lanes);
+  map->NearbyLanesByCurrentLanes(
+      point, theta, FLAGS_search_radius * 2.0, current_lanes_, &nearby_lanes);
   if (nearby_lanes.empty()) {
     ADEBUG << "Obstacle [" << id_ << "] has no nearby lanes.";
     return;

modules/prediction/container/obstacles/obstacles_container.h

@@ -67,6 +67,11 @@ class ObstaclesContainer : public Container {
   Obstacle* GetObstacle(int id);
 
  private:
+  /**
+   * @brief Check if an obstacle is predictable
+   * @param An obstacle
+   * @return True if the obstacle is predictable; otherwise false;
+   */
   bool IsPredictable(
       const apollo::perception::PerceptionObstacle& perception_obstacle);