Prediction: refactored the LaneGraph construction by making...

Prediction: refactored the LaneGraph construction by making ComputeLaneSequence first sort the lanes from left to right before DFS.

modules/prediction/common/road_graph.cc

@@ -20,6 +20,7 @@
 #include <utility>
 
 #include "modules/common/util/string_util.h"
+#include "modules/common/math/math_utils.h"
 #include "modules/prediction/common/prediction_gflags.h"
 #include "modules/prediction/common/prediction_map.h"
 
@@ -30,29 +31,57 @@ using apollo::common::ErrorCode;
 using apollo::common::Status;
 using apollo::hdmap::Id;
 using apollo::hdmap::LaneInfo;
+using apollo::common::math::NormalizeAngle;
+
+// Custom helper functions for sorting purpose.
+bool HeadingIsAtLeft(std::vector<double> heading1,
+                     std::vector<double> heading2,
+                     size_t idx) {
+  if (heading1.empty() || heading2.empty()) {
+    return true;
+  }
+  if (idx >= heading1.size() || idx >= heading2.size()) {
+    return true;
+  }
+  if (NormalizeAngle(heading1[idx] - heading2[idx]) > 0.0) {
+    return true;
+  } else if (NormalizeAngle(heading1[idx] - heading2[idx] < 0.0)) {
+    return false;
+  } else {
+    return HeadingIsAtLeft(heading1, heading2, idx + 1);
+  }
+}
+bool IsAtLeft(std::shared_ptr<const LaneInfo> lane1,
+              std::shared_ptr<const LaneInfo> lane2) {
+  auto heading1 = lane1->headings();
+  auto heading2 = lane2->headings();
+  return HeadingIsAtLeft(heading1, heading2, 0);
+}
 
 RoadGraph::RoadGraph(const double start_s, const double length,
                      std::shared_ptr<const LaneInfo> lane_info_ptr)
     : start_s_(start_s), length_(length), lane_info_ptr_(lane_info_ptr) {}
 
 Status RoadGraph::BuildLaneGraph(LaneGraph* const lane_graph_ptr) {
+  // Sanity checks.
   if (length_ < 0.0 || lane_info_ptr_ == nullptr) {
     const auto error_msg = common::util::StrCat(
         "Invalid road graph settings. Road graph length = ", length_);
     AERROR << error_msg;
     return Status(ErrorCode::PREDICTION_ERROR, error_msg);
   }
-
   if (lane_graph_ptr == nullptr) {
     const auto error_msg = "Invalid input lane graph.";
     AERROR << error_msg;
     return Status(ErrorCode::PREDICTION_ERROR, error_msg);
   }
 
+  // Run the recursive function to perform DFS.
   std::vector<LaneSegment> lane_segments;
   double accumulated_s = 0.0;
-  ComputeLaneSequence(accumulated_s, start_s_, lane_info_ptr_, &lane_segments,
-                      lane_graph_ptr, FLAGS_road_graph_max_search_horizon);
+  ComputeLaneSequence(accumulated_s, start_s_, lane_info_ptr_,
+                      FLAGS_road_graph_max_search_horizon,
+                      &lane_segments, lane_graph_ptr);
 
   return Status::OK();
 }
@@ -78,20 +107,21 @@ bool RoadGraph::IsOnLaneGraph(std::shared_ptr<const LaneInfo> lane_info_ptr,
 void RoadGraph::ComputeLaneSequence(
     const double accumulated_s, const double start_s,
     std::shared_ptr<const LaneInfo> lane_info_ptr,
+    const int graph_search_horizon,
     std::vector<LaneSegment>* const lane_segments,
-    LaneGraph* const lane_graph_ptr,
-    const int graph_search_horizon) const {
+    LaneGraph* const lane_graph_ptr) const {
+  // Sanity checks.
   if (lane_info_ptr == nullptr) {
     AERROR << "Invalid lane.";
     return;
   }
-
   if (graph_search_horizon < 0) {
     AERROR << "The lane search has already reached the limits";
     AERROR << "Possible map error found!";
     return;
   }
 
+  // Create a new lane_segment based on the current lane_info_ptr.
   LaneSegment lane_segment;
   lane_segment.set_lane_id(lane_info_ptr->id().id());
   lane_segment.set_start_s(start_s);
@@ -103,11 +133,13 @@ void RoadGraph::ComputeLaneSequence(
     lane_segment.set_end_s(lane_info_ptr->total_length());
   }
   lane_segment.set_total_length(lane_info_ptr->total_length());
-
   lane_segments->push_back(std::move(lane_segment));
 
+
   if (accumulated_s + lane_info_ptr->total_length() - start_s >= length_ ||
       lane_info_ptr->lane().successor_id_size() == 0) {
+    // End condition: if search reached the max. search distance,
+    //             or if there is no more successor lane_segment.
     LaneSequence* sequence = lane_graph_ptr->add_lane_sequence();
     *sequence->mutable_lane_segment() = {lane_segments->begin(),
                                          lane_segments->end()};
@@ -115,11 +147,20 @@ void RoadGraph::ComputeLaneSequence(
   } else {
     const double successor_accumulated_s =
         accumulated_s + lane_info_ptr->total_length() - start_s;
+
+    // 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()) {
-      auto successor_lane = PredictionMap::LaneById(successor_lane_id.id());
-      ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lane,
-                          lane_segments, lane_graph_ptr,
-                          graph_search_horizon - 1);
+      successor_lanes.push_back
+          (PredictionMap::LaneById(successor_lane_id.id()));
+    }
+    std::sort(successor_lanes.begin(), successor_lanes.end(), IsAtLeft);
+
+    // 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,
+                          lane_segments, lane_graph_ptr);
     }
   }
   lane_segments->pop_back();

modules/prediction/common/road_graph.h

@@ -56,15 +56,32 @@ class RoadGraph {
                      const LaneGraph& lane_graph);
 
  private:
+  /**
+    * @brief
+    * @param The accumulated s starting from the obstacle's position.
+    * @param The starting s of the lane_segment to compute lane_sequence,
+    *        this should be start_s_ for the first time, and zero for
+    *        subsequent recursions.
+    * @param The LaneInfo the current lane segment we are looking at.
+    * @param The max. number of recursive calls (so that it won't recurse
+    *        for too many times when given unreasonable speed info. etc.)
+    * @param The vector of lane_segments visited (DFS).
+    * @param The LaneGraph that we need to write in.
+  */
   void ComputeLaneSequence(const double accumulated_s, const double start_s,
                            std::shared_ptr<const hdmap::LaneInfo> lane_info_ptr,
+                           const int graph_search_horizon,
                            std::vector<LaneSegment>* const lane_segments,
-                           LaneGraph* const lane_graph_ptr,
-                           const int graph_search_horizon) const;
+                           LaneGraph* const lane_graph_ptr) const;
 
  private:
+  // The s of the obstacle on its own lane_segment.
   double start_s_ = 0;
+
+  // The total length to search for lane_graph.
   double length_ = -1.0;
+
+  // The lane_info of the lane_segment where the obstacle is on.
   std::shared_ptr<const hdmap::LaneInfo> lane_info_ptr_ = nullptr;
 };
 

modules/prediction/container/obstacles/obstacle.cc

@@ -161,6 +161,7 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
     return;
   }
 
+  // Set ID and Type of the feature.
   Feature feature;
   if (SetId(perception_obstacle, &feature) == ErrorCode::PREDICTION_ERROR) {
     return;
@@ -171,7 +172,6 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
 
   // Set obstacle observation for KF tracking
   SetStatus(perception_obstacle, timestamp, &feature);
-
   if (!FLAGS_use_navigation_mode) {
     // Update KF
     if (!kf_motion_tracker_.IsInitialized()) {
@@ -184,15 +184,14 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
       }
       UpdateKFPedestrianTracker(feature);
     }
-
     // Update obstacle status based on KF if enabled
     if (FLAGS_enable_kf_tracking) {
       UpdateStatus(&feature);
     }
   }
 
+  // Set obstacle lane features
   if (type_ != PerceptionObstacle::PEDESTRIAN) {
-    // Set obstacle lane features
     SetCurrentLanes(&feature);
     SetNearbyLanes(&feature);
   }
@@ -1011,28 +1010,32 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
 }
 
 void Obstacle::BuildLaneGraph() {
+  // Sanity checks.
   if (history_size() == 0) {
     AERROR << "No feature found.";
     return;
   }
+
   Feature* feature = mutable_latest_feature();
+  // No need to BuildLaneGraph for those non-moving obstacles.
   if (feature->is_still()) {
     ADEBUG << "Not build lane graph for still obstacle";
     return;
   }
   double speed = feature->speed();
-  double road_graph_distance = std::max(
+  double road_graph_search_distance = std::max(
       speed * FLAGS_prediction_duration +
       0.5 * FLAGS_max_acc * FLAGS_prediction_duration *
       FLAGS_prediction_duration, FLAGS_min_prediction_length);
 
+  // BuildLaneGraph for current lanes.
   int seq_id = 0;
   int curr_lane_count = 0;
   for (auto& lane : feature->lane().current_lane_feature()) {
     std::shared_ptr<const LaneInfo> lane_info =
         PredictionMap::LaneById(lane.lane_id());
     const LaneGraph& lane_graph = ObstacleClusters::GetLaneGraph(
-        lane.lane_s(), road_graph_distance, lane_info);
+        lane.lane_s(), road_graph_search_distance, lane_info);
     if (lane_graph.lane_sequence_size() > 0) {
       ++curr_lane_count;
     }
@@ -1053,12 +1056,13 @@ void Obstacle::BuildLaneGraph() {
     }
   }
 
+  // BuildLaneGraph for neighbor lanes.
   int nearby_lane_count = 0;
   for (auto& lane : feature->lane().nearby_lane_feature()) {
     std::shared_ptr<const LaneInfo> lane_info =
         PredictionMap::LaneById(lane.lane_id());
     const LaneGraph& lane_graph = ObstacleClusters::GetLaneGraph(
-        lane.lane_s(), road_graph_distance, lane_info);
+        lane.lane_s(), road_graph_search_distance, lane_info);
     if (lane_graph.lane_sequence_size() > 0) {
       ++nearby_lane_count;
     }
@@ -1083,7 +1087,6 @@ void Obstacle::BuildLaneGraph() {
     SetLanePoints(feature);
     SetLaneSequencePath(feature->mutable_lane()->mutable_lane_graph());
   }
-
   ADEBUG << "Obstacle [" << id_ << "] set lane graph features.";
 }
 

modules/prediction/container/obstacles/obstacle_clusters.cc

@@ -43,6 +43,10 @@ const LaneGraph& ObstacleClusters::GetLaneGraph(
     const double start_s, const double length,
     std::shared_ptr<const LaneInfo> lane_info_ptr) {
   std::string lane_id = lane_info_ptr->id().id();
+
+  // If this lane_segment has been used for constructing LaneGraph,
+  // fetch the previously saved LaneGraph, modify its start_s,
+  // then return this (save the time to construct the entire LaneGraph).
   if (lane_graphs_.find(lane_id) != lane_graphs_.end()) {
     LaneGraph* lane_graph = &lane_graphs_[lane_id];
     for (int i = 0; i < lane_graph->lane_sequence_size(); ++i) {
@@ -58,6 +62,9 @@ const LaneGraph& ObstacleClusters::GetLaneGraph(
     }
     return lane_graphs_[lane_id];
   }
+
+  // If this lane_segment has not been used for constructing LaneGraph,
+  // construct the LaneGraph and return.
   RoadGraph road_graph(start_s, length, lane_info_ptr);
   LaneGraph lane_graph;
   road_graph.BuildLaneGraph(&lane_graph);

modules/prediction/container/obstacles/obstacles_container.cc

@@ -40,6 +40,7 @@ void ObstaclesContainer::Insert(const Message& message) {
   perception_obstacles.CopyFrom(
       dynamic_cast<const PerceptionObstacles&>(message));
 
+  // Update the timestamp_ of the ObstaclesContainer
   double timestamp = 0.0;
   if (perception_obstacles.has_header() &&
       perception_obstacles.header().has_timestamp_sec()) {
@@ -53,17 +54,19 @@ void ObstaclesContainer::Insert(const Message& message) {
            << timestamp_ << "].";
     return;
   }
-
   if (FLAGS_prediction_offline_mode) {
     if (std::fabs(timestamp - timestamp_) > FLAGS_replay_timestamp_gap ||
         FeatureOutput::Size() > FLAGS_max_num_dump_feature) {
       FeatureOutput::Write();
     }
   }
-
   timestamp_ = timestamp;
   ADEBUG << "Current timestamp is [" << timestamp_ << "]";
+
+  // Set up the ObstacleClusters:
+  // 1. Initialize ObstacleClusters
   ObstacleClusters::Init();
+  // 2. Insert the Obstacles one by one
   for (const PerceptionObstacle& perception_obstacle :
        perception_obstacles.perception_obstacle()) {
     ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "] "
@@ -72,7 +75,9 @@ void ObstaclesContainer::Insert(const Message& message) {
     ADEBUG << "Perception obstacle [" << perception_obstacle.id() << "] "
            << "was inserted";
   }
+  // 3. Sort the Obstacles
   ObstacleClusters::SortObstacles();
+  // 4. Deduct the NearbyObstacles info. from the sorted Obstacles
   for (const PerceptionObstacle& perception_obstacle :
        perception_obstacles.perception_obstacle()) {
     if (IsPredictable(perception_obstacle)) {
@@ -102,6 +107,7 @@ void ObstaclesContainer::Clear() {
 
 void ObstaclesContainer::InsertPerceptionObstacle(
     const PerceptionObstacle& perception_obstacle, const double timestamp) {
+  // Sanity checks.
   const int id = perception_obstacle.id();
   if (id < -1) {
     AERROR << "Invalid ID [" << id << "]";
@@ -111,11 +117,13 @@ void ObstaclesContainer::InsertPerceptionObstacle(
     ADEBUG << "Perception obstacle [" << id << "] is not predictable.";
     return;
   }
+
+  // Insert the obstacle and also update the LRUCache.
   curr_frame_predictable_obstacle_ids_.push_back(id);
   Obstacle* obstacle_ptr = obstacles_.Get(id);
   if (obstacle_ptr != nullptr) {
     obstacle_ptr->Insert(perception_obstacle, timestamp);
-    ADEBUG << "Insert obstacle [" << id << "]";
+    ADEBUG << "Refresh obstacle [" << id << "]";
   } else {
     Obstacle obstacle;
     obstacle.Insert(perception_obstacle, timestamp);

modules/prediction/evaluator/vehicle/cruise_mlp_evaluator.cc

@@ -132,6 +132,8 @@ void CruiseMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) {
     return;
   }
 
+  ADEBUG << "There are " << lane_graph_ptr->lane_sequence_size()
+         << " lane sequences with probabilities:";
   // For every possible lane sequence, extract needed features.
   // Then compute the likelihood of the obstacle moving onto that laneseq.
   for (int i = 0; i < lane_graph_ptr->lane_sequence_size(); ++i) {
@@ -160,6 +162,7 @@ void CruiseMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) {
     }
     double probability = model_output(0, 0);
     double finish_time = model_output(0, 1);
+    ADEBUG << probability;
     lane_sequence_ptr->set_probability(probability);
     lane_sequence_ptr->set_time_to_lane_center(finish_time);
   }
@@ -189,6 +192,7 @@ void CruiseMLPEvaluator::ExtractFeatureValues
            << obstacle_feature_values.size() << ".";
     return;
   }
+  ADEBUG << "Obstacle feature size = " << obstacle_feature_values.size();
   feature_values->insert(feature_values->end(), obstacle_feature_values.begin(),
                          obstacle_feature_values.end());
 
@@ -202,6 +206,7 @@ void CruiseMLPEvaluator::ExtractFeatureValues
            << interaction_feature_values.size() << ".";
     return;
   }
+  ADEBUG << "Interaction feature size = " << interaction_feature_values.size();
   feature_values->insert(feature_values->end(),
                          interaction_feature_values.begin(),
                          interaction_feature_values.end());
@@ -215,6 +220,7 @@ void CruiseMLPEvaluator::ExtractFeatureValues
            << ".";
     return;
   }
+  ADEBUG << "Lane feature size = " << lane_feature_values.size();
   feature_values->insert(feature_values->end(), lane_feature_values.begin(),
                          lane_feature_values.end());
 
@@ -264,6 +270,8 @@ void CruiseMLPEvaluator::SetObstacleFeatureValues(
       obstacle_ptr->timestamp() - FLAGS_prediction_duration;
   int count = 0;
   // Starting from the most recent timestamp and going backward.
+  ADEBUG << "Obstacle has " << obstacle_ptr->history_size()
+         << " history timestamps.";
   for (std::size_t i = 0; i < obstacle_ptr->history_size(); ++i) {
     const Feature& feature = obstacle_ptr->feature(i);
     if (!feature.IsInitialized()) {
@@ -272,6 +280,9 @@ void CruiseMLPEvaluator::SetObstacleFeatureValues(
     if (feature.timestamp() < obs_feature_history_start_time) {
       break;
     }
+    if (!feature.has_lane()) {
+      ADEBUG << "Feature has no lane. Quit.";
+    }
     if (feature.has_lane() && feature.lane().has_lane_feature()) {
       thetas.push_back(feature.lane().lane_feature().angle_diff());
       lane_ls.push_back(feature.lane().lane_feature().lane_l());
@@ -282,6 +293,9 @@ void CruiseMLPEvaluator::SetObstacleFeatureValues(
       timestamps.push_back(feature.timestamp());
       speeds.push_back(feature.speed());
       ++count;
+    } else {
+      ADEBUG << "Feature has no lane_feature!!!";
+      ADEBUG << feature.lane().current_lane_feature_size();
     }
     if (feature.has_position() &&
         i < FLAGS_cruise_historical_frame_length) {
@@ -304,6 +318,7 @@ void CruiseMLPEvaluator::SetObstacleFeatureValues(
     }
   }
   if (count <= 0) {
+    ADEBUG << "There is no feature with lane info. Quit.";
     return;
   }
 

modules/prediction/prediction_component.cc

@@ -215,6 +215,8 @@ void PredictionComponent::OnPerception(
   ADEBUG << "Time for scenario_manager: "
          << diff.count() * 1000 << " msec.";
 
+  // If in junction, BuildJunctionFeature();
+  // If not, BuildLaneGraph().
   const Scenario& scenario = ScenarioManager::Instance()->scenario();
   if (scenario.type() == Scenario::JUNCTION && scenario.has_junction_id() &&
       FLAGS_enable_junction_feature) {
@@ -225,27 +227,23 @@ void PredictionComponent::OnPerception(
   diff = end_time4 - end_time3;
   ADEBUG << "Time to build junction features: "
          << diff.count() * 1000 << " msec.";
-
   // TODO(kechxu) refactor logic of build lane graph and build junction feature
-  // Set up obstacle cluster
   ptr_obstacles_container->BuildLaneGraph();
   auto end_time5 = std::chrono::system_clock::now();
   diff = end_time5 - end_time4;
   ADEBUG << "Time to build cruise features: "
          << diff.count() * 1000 << " msec.";
-
   ADEBUG << "Received a perception message ["
          << perception_msg.ShortDebugString() << "].";
 
+  // Insert ADC into the obstacle_container as well.
   auto ptr_ego_pose_container = ContainerManager::Instance()->GetContainer<
       PoseContainer>(AdapterConfig::LOCALIZATION);
   auto ptr_ego_trajectory_container =
       ContainerManager::Instance()->GetContainer<ADCTrajectoryContainer>(
           AdapterConfig::PLANNING_TRAJECTORY);
-
   CHECK(ptr_ego_pose_container != nullptr &&
       ptr_ego_trajectory_container != nullptr);
-
   const PerceptionObstacle* ptr_ego_vehicle =
       ptr_ego_pose_container->ToPerceptionObstacle();
   if (ptr_ego_vehicle != nullptr) {
@@ -266,12 +264,10 @@ void PredictionComponent::OnPerception(
   ADEBUG << "Time to evaluate: "
         << diff.count() * 1000 << " msec.";
 
-  // No prediction trajectories for offline mode
+  // Make predictions
   if (FLAGS_prediction_offline_mode) {
     return;
   }
-
-  // Make predictions
   PredictorManager::Instance()->Run(perception_msg);
   auto end_time8 = std::chrono::system_clock::now();
   diff = end_time8 - end_time7;
@@ -329,6 +325,8 @@ bool PredictionComponent::Proc(
   frame_start_time_ = Clock::NowInSeconds();
   auto end_time1 = std::chrono::system_clock::now();
 
+  // Read localization info. and call OnLocalization to update
+  // the PoseContainer.
   localization_reader_->Observe();
   auto ptr_localization_msg = localization_reader_->GetLatestObserved();
   if (ptr_localization_msg == nullptr) {
@@ -342,6 +340,8 @@ bool PredictionComponent::Proc(
   ADEBUG << "Time for updating PoseContainer: "
         << diff.count() * 1000 << " msec.";
 
+  // Read planning info. of last frame and call OnPlanning to update
+  // the ADCTrajectoryContainer
   planning_reader_->Observe();
   auto ptr_trajectory_msg = planning_reader_->GetLatestObserved();
   if (ptr_trajectory_msg != nullptr) {
@@ -353,6 +353,8 @@ bool PredictionComponent::Proc(
   ADEBUG << "Time for updating ADCTrajectoryContainer: "
         << diff.count() * 1000 << " msec.";
 
+  // Get all perception_obstacles of this frame and call OnPerception to
+  // process them all.
   auto perception_msg = *perception_obstacles;
   OnPerception(perception_msg);
   auto end_time4 = std::chrono::system_clock::now();