Prediction: fix several bugs

modules/prediction/common/junction_analyzer.cc

@@ -127,6 +127,7 @@ const JunctionFeature& JunctionAnalyzer::GetJunctionFeature(
   for (const auto& junction_exit : junction_exits) {
     junction_feature.add_junction_exit()->CopyFrom(junction_exit);
   }
+  junction_feature.mutable_enter_lane()->set_lane_id(start_lane_id);
   junction_features_[start_lane_id] = junction_feature;
   return junction_features_[start_lane_id];
 }

modules/prediction/container/adc_trajectory/adc_trajectory_container.cc

@@ -51,9 +51,7 @@ void ADCTrajectoryContainer::Insert(
          << adc_trajectory_.ShortDebugString() << "].";
 
   // Find junction
-  if (IsProtected()) {
-    SetJunctionPolygon();
-  }
+  SetJunctionPolygon();
   ADEBUG << "Generate a polygon [" << adc_junction_polygon_.DebugString()
          << "].";
 
@@ -120,9 +118,9 @@ void ADCTrajectoryContainer::SetJunctionPolygon() {
       vertices.emplace_back(point.x(), point.y());
     }
     if (vertices.size() >= 3) {
-      adc_junction_polygon_ = Polygon2d{vertices};
       adc_junction_info_ptr_ = junction_info;
       s_dist_to_junction_ = s_at_junction - s_start;
+      adc_junction_polygon_ = Polygon2d{vertices};
     }
   }
 }

modules/prediction/container/obstacles/obstacle.cc

@@ -662,6 +662,13 @@ void Obstacle::SetIsNearJunction(
   feature->set_is_near_junction(is_near_junction);
 }
 
+bool Obstacle::HasJunctionFeature() {
+  if (history_size() == 0) {
+    return false;
+  }
+  return latest_feature().has_junction_feature();
+}
+
 void Obstacle::InitKFMotionTracker(const Feature& feature) {
   // Set transition matrix F
   // constant acceleration dynamic model

modules/prediction/container/obstacles/obstacle.h

@@ -167,6 +167,12 @@ class Obstacle {
    */
   bool IsInJunction(const std::string& junction_id);
 
+  /**
+   * @brief Check if the obstacle has junction feature.
+   * @return If the obstacle has junction feature.
+   */
+  bool HasJunctionFeature();
+
   /**
    * @brief Build junction feature.
    */

modules/prediction/evaluator/evaluator_manager.cc

@@ -129,17 +129,13 @@ void EvaluatorManager::Run(
       continue;
     }
 
-    const Scenario& scenario = ScenarioManager::Instance()->scenario();
-
     switch (perception_obstacle.type()) {
       case PerceptionObstacle::VEHICLE: {
-        if (obstacle->IsOnLane()) {
-          if (scenario.type() == Scenario::JUNCTION &&
-              obstacle->IsInJunction(scenario.junction_id())) {
-            evaluator = GetEvaluator(vehicle_in_junction_evaluator_);
-          } else {
-            evaluator = GetEvaluator(vehicle_on_lane_evaluator_);
-          }
+        if (obstacle->HasJunctionFeature()) {
+          evaluator = GetEvaluator(vehicle_in_junction_evaluator_);
+          CHECK_NOTNULL(evaluator);
+        } else if (obstacle->IsOnLane()) {
+          evaluator = GetEvaluator(vehicle_on_lane_evaluator_);
           CHECK_NOTNULL(evaluator);
         }
         break;

modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc

@@ -74,6 +74,14 @@ void JunctionMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) {
     return;
   }
 
+  if (obstacle_ptr->history_size() == 0 ||
+      !obstacle_ptr->latest_feature().has_junction_feature() ||
+      obstacle_ptr->latest_feature().junction_feature()
+                                    .junction_exit_size() <= 1) {
+    // TODO(all) Predict based on only one exit
+    return;
+  }
+
   std::vector<double> feature_values;
 
   ExtractFeatureValues(obstacle_ptr, &feature_values);
@@ -95,8 +103,8 @@ bool JunctionMLPEvaluator::IsClosedToJunctionExit(
   const Feature& latest_feature = obstacle_ptr->latest_feature();
   double position_x = latest_feature.position().x();
   double position_y = latest_feature.position().y();
-  double raw_velocity_heading = std::atan2(latest_feature.raw_velocity().x(),
-                                           latest_feature.raw_velocity().y());
+  double raw_velocity_heading = std::atan2(latest_feature.raw_velocity().y(),
+                                           latest_feature.raw_velocity().x());
 
   for (const JunctionExit& junction_exit :
        latest_feature.junction_feature().junction_exit()) {

modules/prediction/predictor/predictor.cc

@@ -70,6 +70,10 @@ bool Predictor::TrimTrajectory(
     const Obstacle* obstacle,
     const ADCTrajectoryContainer* adc_trajectory_container,
     Trajectory* trajectory) {
+  if (!adc_trajectory_container->IsProtected()) {
+    ADEBUG << "Not in protection mode.";
+    return false;
+  }
   if (obstacle == nullptr || obstacle->history_size() == 0) {
     AERROR << "Invalid obstacle.";
     return false;

modules/prediction/scenario/feature_extractor/feature_extractor.cc

@@ -187,16 +187,18 @@ void FeatureExtractor::ExtractFrontJunctionFeatures(
     return;
   }
   JunctionInfoPtr junction = ego_trajectory_container->ADCJunction();
-  bool need_consider = false;
-  if (junction != nullptr) {
-    for (const auto &overlap_id : junction->junction().overlap_id()) {
-      if (PredictionMap::OverlapById(overlap_id.id()) != nullptr) {
-        for (const auto &object :
-          PredictionMap::OverlapById(overlap_id.id())->overlap().object()) {
-          if (object.has_signal_overlap_info() ||
-              object.has_stop_sign_overlap_info()) {
-            need_consider = true;
-          }
+  // TODO(all) change need_consider to false once map is fixed
+  bool need_consider = true;
+  if (junction == nullptr) {
+    return;
+  }
+  for (const auto &overlap_id : junction->junction().overlap_id()) {
+    if (PredictionMap::OverlapById(overlap_id.id()) != nullptr) {
+      for (const auto &object :
+        PredictionMap::OverlapById(overlap_id.id())->overlap().object()) {
+        if (object.has_signal_overlap_info() ||
+            object.has_stop_sign_overlap_info()) {
+          need_consider = true;
         }
       }
     }

modules/tools/prediction/mlp_train/common/trajectory.py

@@ -332,8 +332,9 @@ class TrajectoryToSample(object):
                 continue
             curr_pos = np.array([fea.position.x, fea.position.y])
             # Only keep speed > 1
-            if fea.speed <= 1:
-                continue
+            # TODO(all) consider recovery
+            # if fea.speed <= 1:
+            #     continue
             heading = math.atan2(fea.raw_velocity.y, fea.raw_velocity.x)
             # Construct dictionary of all exit with dict[exit_lane_id] = np.array(exit_position)
             exit_dict = dict()
@@ -350,7 +351,7 @@ class TrajectoryToSample(object):
                         exit_pos = exit_pos_dict[key]
                         delta_pos = exit_pos - curr_pos
                         angle = math.atan2(delta_pos[1], delta_pos[0]) - heading
-                        d_idx = int((angle / (2.0 * np.pi) + 1) * 12 % 12)
+                        d_idx = int((angle / (2.0 * np.pi)) * 12 % 12)
                         label = [0 for idx in range(12)]
                         label[d_idx] = 1
                         fea.junction_feature.junction_mlp_label.extend(label)

modules/tools/prediction/mlp_train/generate_junction_labels.py

@@ -43,7 +43,6 @@ def label_file(input_file, output_file):
     # and label the rest.
     for fea_key, fea_traj in fea_trajs.items():
         fea_traj = fea_trajs[fea_key]
-        fea_traj = TrajectoryToSample.clean(fea_traj)
         fea_traj = TrajectoryToSample.label_junction(fea_traj)
         for i, fea in enumerate(fea_traj):
             if not fea.HasField('junction_feature') or \