Prediction: split off building lane graph from inserting perception message

modules/prediction/container/obstacles/obstacle.cc

@@ -177,7 +177,6 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
   // Set obstacle lane features
   SetCurrentLanes(&feature);
   SetNearbyLanes(&feature);
-  SetLaneGraphFeature(&feature);
 
   if (FLAGS_adjust_vehicle_heading_by_lane &&
      type_ == PerceptionObstacle::VEHICLE) {
@@ -837,7 +836,12 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
   }
 }
 
-void Obstacle::SetLaneGraphFeature(Feature* feature) {
+void Obstacle::BuildLaneGraph() {
+  if (history_size() == 0) {
+    AERROR << "No feature found.";
+    return;
+  }
+  Feature* feature = mutable_latest_feature();
   double speed = feature->speed();
   double road_graph_distance = std::max(
       speed * FLAGS_prediction_duration +

modules/prediction/container/obstacles/obstacle.h

@@ -150,6 +150,11 @@ class Obstacle {
    */
   bool IsNearJunction();
 
+  /**
+   * @brief Build obstacle's lane graph
+   */
+  void BuildLaneGraph();
+
   /**
    * @brief Set RNN state
    * @param RNN state matrix
@@ -221,8 +226,6 @@ class Obstacle {
 
   void SetNearbyLanes(Feature* feature);
 
-  void SetLaneGraphFeature(Feature* feature);
-
   void SetLanePoints(Feature* feature);
 
   void SetLaneSequencePath(LaneGraph* const lane_graph);

modules/prediction/container/obstacles/obstacle_test.cc

@@ -52,6 +52,7 @@ class ObstacleTest : public KMLMapBasedTest {
       perception::PerceptionObstacles perception_obstacles;
       common::util::GetProtoFromFile(filename, &perception_obstacles);
       container_.Insert(perception_obstacles);
+      container_.BuildLaneGraph();
     }
   }
 

modules/prediction/container/obstacles/obstacles_container.cc

@@ -33,6 +33,7 @@ ObstaclesContainer::ObstaclesContainer()
     : obstacles_(FLAGS_max_num_obstacles) {}
 
 void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
+  curr_frame_predictable_obstacle_ids_.clear();
   PerceptionObstacles perception_obstacles;
   perception_obstacles.CopyFrom(
       dynamic_cast<const PerceptionObstacles&>(message));
@@ -91,6 +92,7 @@ void ObstaclesContainer::InsertPerceptionObstacle(
     ADEBUG << "Perception obstacle [" << id << "] is not predictable.";
     return;
   }
+  curr_frame_predictable_obstacle_ids_.push_back(id);
   Obstacle* obstacle_ptr = obstacles_.GetSilently(id);
   if (obstacle_ptr != nullptr) {
     obstacle_ptr->Insert(perception_obstacle, timestamp);
@@ -101,6 +103,18 @@ void ObstaclesContainer::InsertPerceptionObstacle(
   }
 }
 
+void ObstaclesContainer::BuildLaneGraph() {
+  // TODO(kechxu) implement building lane graph, lane sequences
+  for (const int id : curr_frame_predictable_obstacle_ids_) {
+    Obstacle* obstacle_ptr = obstacles_.GetSilently(id);
+    if (obstacle_ptr == nullptr) {
+      AERROR << "Null obstacle found.";
+      continue;
+    }
+    obstacle_ptr->BuildLaneGraph();
+  }
+}
+
 bool ObstaclesContainer::IsPredictable(
     const PerceptionObstacle& perception_obstacle) {
   if (!perception_obstacle.has_type() ||

modules/prediction/container/obstacles/obstacles_container.h

@@ -22,6 +22,8 @@
 #ifndef MODULES_PREDICTION_CONTAINER_OBSTACLES_OBSTACLES_CONTAINER_H_
 #define MODULES_PREDICTION_CONTAINER_OBSTACLES_OBSTACLES_CONTAINER_H_
 
+#include <vector>
+
 #include "cybertron/common/macros.h"
 
 #include "modules/common/util/lru_cache.h"
@@ -59,6 +61,11 @@ class ObstaclesContainer : public Container {
       const perception::PerceptionObstacle& perception_obstacle,
       const double timestamp);
 
+  /**
+   * @brief Build lane graph for obstacles
+   */
+  void BuildLaneGraph();
+
   /**
    * @brief Get obstacle pointer
    * @param Obstacle ID
@@ -82,6 +89,7 @@ class ObstaclesContainer : public Container {
  private:
   double timestamp_ = -1.0;
   common::util::LRUCache<int, Obstacle> obstacles_;
+  std::vector<int> curr_frame_predictable_obstacle_ids_;
 };
 
 }  // namespace prediction

modules/prediction/prediction_component.cc

@@ -233,6 +233,8 @@ bool PredictionComponent::Proc(
   CHECK_NOTNULL(obstacles_container);
   obstacles_container->Insert(*perception_obstacles);
 
+  obstacles_container->BuildLaneGraph();
+
   ADEBUG << "Received a perception message ["
          << perception_obstacles->ShortDebugString() << "].";
 

modules/prediction/predictor/lane_sequence/lane_sequence_predictor_test.cc

@@ -54,6 +54,7 @@ TEST_F(LaneSequencePredictorTest, OnLaneCase) {
   MLPEvaluator mlp_evaluator;
   ObstaclesContainer container;
   container.Insert(perception_obstacles_);
+  container.BuildLaneGraph();
   Obstacle* obstacle_ptr = container.GetObstacle(1);
   EXPECT_TRUE(obstacle_ptr != nullptr);
   mlp_evaluator.Evaluate(obstacle_ptr);

modules/prediction/predictor/move_sequence/move_sequence_predictor_test.cc

@@ -54,6 +54,7 @@ TEST_F(MoveSequencePredictorTest, OnLaneCase) {
   MLPEvaluator mlp_evaluator;
   ObstaclesContainer container;
   container.Insert(perception_obstacles_);
+  container.BuildLaneGraph();
   Obstacle* obstacle_ptr = container.GetObstacle(1);
   EXPECT_TRUE(obstacle_ptr != nullptr);
   mlp_evaluator.Evaluate(obstacle_ptr);
@@ -71,6 +72,7 @@ TEST_F(MoveSequencePredictorTest, Polynomial) {
   MLPEvaluator mlp_evaluator;
   ObstaclesContainer container;
   container.Insert(perception_obstacles_);
+  container.BuildLaneGraph();
   Obstacle* obstacle_ptr = container.GetObstacle(1);
   EXPECT_TRUE(obstacle_ptr != nullptr);
   mlp_evaluator.Evaluate(obstacle_ptr);

modules/prediction/predictor/sequence/sequence_predictor_test.cc

@@ -54,6 +54,7 @@ TEST_F(SequencePredictorTest, General) {
   MLPEvaluator mlp_evaluator;
   ObstaclesContainer container;
   container.Insert(perception_obstacles_);
+  container.BuildLaneGraph();
   Obstacle* obstacle_ptr = container.GetObstacle(1);
   EXPECT_TRUE(obstacle_ptr != nullptr);
   mlp_evaluator.Evaluate(obstacle_ptr);

modules/prediction/predictor/single_lane/single_lane_predictor_test.cc

@@ -54,6 +54,7 @@ TEST_F(SingleLanePredictorTest, OnLaneCase) {
   CostEvaluator cost_evaluator;
   ObstaclesContainer container;
   container.Insert(perception_obstacles_);
+  container.BuildLaneGraph();
   Obstacle* obstacle_ptr = container.GetObstacle(1);
   EXPECT_TRUE(obstacle_ptr != nullptr);
   cost_evaluator.Evaluate(obstacle_ptr);