Prediction: junction code clean and small bugs fixed

modules/prediction/common/junction_analyzer.cc

@@ -61,14 +61,16 @@ void JunctionAnalyzer::SetAllJunctionExits() {
     if (overlap_info_ptr == nullptr) {
       continue;
     }
+    // TODO(all) consider to delete
     if (overlap_info_ptr->overlap().object_size() != 2) {
       continue;
     }
     for (const auto &object : overlap_info_ptr->overlap().object()) {
       if (object.has_lane_overlap_info()) {
-        std::string lane_id = object.id().id();
+        const std::string& lane_id = object.id().id();
         auto lane_info_ptr = PredictionMap::LaneById(lane_id);
-        if (object.lane_overlap_info().end_s() + 1e-3 <
+        // TODO(all) move 0.1 to gflags
+        if (object.lane_overlap_info().end_s() + 0.1 <
             lane_info_ptr->total_length()) {
           JunctionExit junction_exit;
           double s = object.lane_overlap_info().end_s();
@@ -98,6 +100,7 @@ std::vector<JunctionExit> JunctionAnalyzer::GetJunctionExits(
     lane_info_queue.pop();
     const std::string& curr_lane_id = curr_lane->id().id();
     if (IsExitLane(curr_lane_id)) {
+      // TODO(kechxu) deal with duplicates
       junction_exits.push_back(junction_exits_[curr_lane_id]);
       continue;
     }
@@ -119,7 +122,6 @@ const JunctionFeature& JunctionAnalyzer::GetJunctionFeature(
     return junction_features_[start_lane_id];
   }
   JunctionFeature junction_feature;
-  // TODO(all) not fill enter_lane yet
   junction_feature.set_junction_id(GetJunctionId());
   junction_feature.set_junction_range(ComputeJunctionRange());
   std::vector<JunctionExit> junction_exits = GetJunctionExits(start_lane_id);

modules/prediction/container/obstacles/obstacle.cc

@@ -242,6 +242,7 @@ void Obstacle::BuildJunctionFeature() {
     CHECK(prev_feature.junction_feature().enter_lane().has_lane_id());
     std::string enter_lane_id =
         prev_feature.junction_feature().enter_lane().lane_id();
+    // TODO(kechxu) deal with consecutive junctions
     SetJunctionFeatureWithEnterLane(
         enter_lane_id, latest_feature_ptr);
   } else {
@@ -267,6 +268,7 @@ void Obstacle::SetJunctionFeatureWithoutEnterLane(
   } else {
     if (feature_ptr->lane().nearby_lane_feature_size() > 0) {
       // TODO(kechxu) start with multiple lane ids
+      // TODO(kechxu) enlarge searching range for lanes in junction
       start_lane_id = feature_ptr->lane().nearby_lane_feature(0).lane_id();
     }
   }

modules/prediction/evaluator/vehicle/junction_mlp_evaluator.cc

@@ -43,8 +43,8 @@ bool IsClosed(const double x0, const double y0, const double theta0,
               const double x1, const double y1, const double theta1) {
   // TODO(all) move constants to gflags
   double angle_diff = std::abs(common::math::AngleDiff(theta0, theta1));
-  return std::abs(x0 - x1) < 1.0 && std::abs(y0 - y1) &&
-         angle_diff < M_PI * 0.25;
+  double distance = std::hypot(x0 - x1, y0 - y1);
+  return distance < 1.0 && angle_diff < M_PI * 0.25;
 }
 
 JunctionMLPEvaluator::JunctionMLPEvaluator() {
@@ -52,7 +52,6 @@ JunctionMLPEvaluator::JunctionMLPEvaluator() {
 }
 
 void JunctionMLPEvaluator::Clear() {
-  obstacle_feature_values_map_.clear();
 }
 
 void JunctionMLPEvaluator::Evaluate(Obstacle* obstacle_ptr) {
@@ -126,17 +125,13 @@ void JunctionMLPEvaluator::ExtractFeatureValues(
   CHECK_NOTNULL(obstacle_ptr);
   int id = obstacle_ptr->id();
 
-  auto it = obstacle_feature_values_map_.find(id);
-  if (it == obstacle_feature_values_map_.end()) {
-    std::vector<double> obstacle_feature_values;
-    SetObstacleFeatureValues(obstacle_ptr, &obstacle_feature_values);
-    obstacle_feature_values_map_[id] = obstacle_feature_values;
-  }
+  std::vector<double> obstacle_feature_values;
+  SetObstacleFeatureValues(obstacle_ptr, &obstacle_feature_values);
 
-  if (obstacle_feature_values_map_[id].size() != OBSTACLE_FEATURE_SIZE) {
+  if (obstacle_feature_values.size() != OBSTACLE_FEATURE_SIZE) {
     AERROR << "Obstacle [" << id << "] has fewer than "
            << "expected obstacle feature_values "
-           << obstacle_feature_values_map_[id].size() << ".";
+           << obstacle_feature_values.size() << ".";
     return;
   }
 
@@ -150,9 +145,10 @@ void JunctionMLPEvaluator::ExtractFeatureValues(
   }
 
   feature_values->insert(feature_values->end(),
-                         obstacle_feature_values_map_[id].begin(),
-                         obstacle_feature_values_map_[id].end());
-  feature_values->insert(feature_values->end(), junction_feature_values.begin(),
+                         obstacle_feature_values.begin(),
+                         obstacle_feature_values.end());
+  feature_values->insert(feature_values->end(),
+                         junction_feature_values.begin(),
                          junction_feature_values.end());
   if (FLAGS_prediction_offline_mode) {
     SaveOfflineFeatures(obstacle_ptr->mutable_latest_feature(),
@@ -196,11 +192,11 @@ void JunctionMLPEvaluator::SetJunctionFeatureValues(
   std::string junction_id = feature_ptr->junction_feature().junction_id();
   double junction_range = feature_ptr->junction_feature().junction_range();
   for (int i = 0; i < 12; ++i) {
-    feature_values->push_back(0);
-    feature_values->push_back(1);
-    feature_values->push_back(1);
-    feature_values->push_back(1);
-    feature_values->push_back(0);
+    feature_values->push_back(0.0);
+    feature_values->push_back(1.0);
+    feature_values->push_back(1.0);
+    feature_values->push_back(1.0);
+    feature_values->push_back(0.0);
   }
   int num_junction_exit = feature_ptr->junction_feature().junction_exit_size();
   for (int i = 0; i < num_junction_exit; ++i) {
@@ -213,9 +209,10 @@ void JunctionMLPEvaluator::SetJunctionFeatureValues(
     double diff_heading = apollo::common::math::AngleDiff(heading,
                               junction_exit.exit_heading());
     double angle = std::atan2(diff_y, diff_x);
+    // TODO(Hongyi) test d_idx
     double d_idx = (angle / (2.0 * M_PI)) * 12.0;
     int idx = static_cast<int>(floor(d_idx >= 0 ? d_idx : d_idx + 12));
-    feature_values->operator[](idx * 5) = 1;
+    feature_values->operator[](idx * 5) = 1.0;
     feature_values->operator[](idx * 5 + 1) = diff_x / junction_range;
     feature_values->operator[](idx * 5 + 2) = diff_y / junction_range;
     feature_values->operator[](idx * 5 + 3) =

modules/prediction/evaluator/vehicle/junction_mlp_evaluator.h

@@ -100,7 +100,6 @@ class JunctionMLPEvaluator : public Evaluator {
   double ComputeProbability(const std::vector<double>& feature_values);
 
  private:
-  std::unordered_map<int, std::vector<double>> obstacle_feature_values_map_;
   static const size_t OBSTACLE_FEATURE_SIZE = 3;
   static const size_t JUNCTION_FEATURE_SIZE = 60;
 };