Prediction: implement prioritizing obstacles

modules/prediction/common/BUILD

@@ -158,6 +158,7 @@ cc_library(
     srcs = ["environment_features.cc"],
     hdrs = ["environment_features.h"],
     deps = [
+        "//modules/common/proto:geometry_proto",
         "//framework:cybertron",
     ],
 )

modules/prediction/common/environment_features.cc

@@ -20,12 +20,22 @@
 namespace apollo {
 namespace prediction {
 
-void EnvironmentFeatures::set_ego_velocity(const double ego_velocity) {
-  ego_velocity_ = ego_velocity;
+void EnvironmentFeatures::set_ego_position(const double x, const double y) {
+  ego_position_.set_x(x);
+  ego_position_.set_y(y);
+  ego_position_.set_z(0.0);
 }
 
-double EnvironmentFeatures::get_ego_velocity() const {
-  return ego_velocity_;
+const apollo::common::Point3D& EnvironmentFeatures::get_ego_position() const {
+  return ego_position_;
+}
+
+void EnvironmentFeatures::set_ego_speed(const double ego_speed) {
+  ego_speed_ = ego_speed;
+}
+
+double EnvironmentFeatures::get_ego_speed() const {
+  return ego_speed_;
 }
 
 void EnvironmentFeatures::set_ego_heading(const double ego_heading) {

modules/prediction/common/environment_features.h

@@ -22,6 +22,8 @@
 #include <utility>
 #include <vector>
 
+#include "modules/common/proto/geometry.pb.h"
+
 namespace apollo {
 namespace prediction {
 
@@ -31,9 +33,13 @@ class EnvironmentFeatures {
 
   virtual ~EnvironmentFeatures() = default;
 
-  void set_ego_velocity(const double ego_velocity);
+  void set_ego_position(const double x, const double y);
+
+  const apollo::common::Point3D& get_ego_position() const;
 
-  double get_ego_velocity() const;
+  void set_ego_speed(const double ego_speed);
+
+  double get_ego_speed() const;
 
   void set_ego_acceleration(const double ego_acceleration);
 
@@ -80,7 +86,9 @@ class EnvironmentFeatures {
   const std::vector<int>& get_obstacle_ids() const;
 
  private:
-  double ego_velocity_ = 0.0;
+  apollo::common::Point3D ego_position_;
+
+  double ego_speed_ = 0.0;
 
   double ego_acceleration_ = 0.0;
 

modules/prediction/container/obstacles/obstacles_container.cc

@@ -76,6 +76,11 @@ Obstacle* ObstaclesContainer::GetObstacle(const int id) {
   return obstacles_.GetSilently(id);
 }
 
+const std::vector<int>&
+ObstaclesContainer::GetCurrentFramePredictableObstacleIds() const {
+  return curr_frame_predictable_obstacle_ids_;
+}
+
 void ObstaclesContainer::Clear() {
   obstacles_.Clear();
   timestamp_ = -1.0;
@@ -118,18 +123,6 @@ void ObstaclesContainer::BuildLaneGraph() {
   }
 }
 
-void ObstaclesContainer::PrioritizeObstacles(const Scenario& scenario,
-    const EnvironmentFeatures& environment_features) {
-  // TODO(all) implement
-  // According to scenario and filtered lanes and junctions, etc
-  // set priorities for obstacles
-  if (scenario.type() == Scenario::CRUISE ||
-      scenario.type() == Scenario::CRUISE_URBAN ||
-      scenario.type() == Scenario::CRUISE_HIGHWAY) {
-    PrioritizeObstaclesInCruise();
-  }
-}
-
 bool ObstaclesContainer::IsPredictable(
     const PerceptionObstacle& perception_obstacle) {
   if (!perception_obstacle.has_type() ||
@@ -139,9 +132,5 @@ bool ObstaclesContainer::IsPredictable(
   return true;
 }
 
-void ObstaclesContainer::PrioritizeObstaclesInCruise() {
-  // TODO(kechxu) implement
-}
-
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/container/obstacles/obstacles_container.h

@@ -68,12 +68,6 @@ class ObstaclesContainer : public Container {
    */
   void BuildLaneGraph();
 
-  /**
-   * @brief Set obstacles' priority to predict
-   */
-  void PrioritizeObstacles(const Scenario& scenario,
-                           const EnvironmentFeatures& environment_features);
-
   /**
    * @brief Get obstacle pointer
    * @param Obstacle ID
@@ -81,6 +75,12 @@ class ObstaclesContainer : public Container {
    */
   Obstacle* GetObstacle(const int id);
 
+  /**
+   * @brief Get predictable obstacle IDs in the current frame
+   * @return Predictable obstacle IDs in the current frame
+   */
+  const std::vector<int>& GetCurrentFramePredictableObstacleIds() const;
+
   /**
    * @brief Clear obstacle container
    */
@@ -94,8 +94,6 @@ class ObstaclesContainer : public Container {
    */
   bool IsPredictable(const perception::PerceptionObstacle& perception_obstacle);
 
-  void PrioritizeObstaclesInCruise();
-
  private:
   double timestamp_ = -1.0;
   common::util::LRUCache<int, Obstacle> obstacles_;

modules/prediction/scenario/BUILD

@@ -9,10 +9,12 @@ cc_library(
     deps = [
         "//framework:cybertron",
         "//modules/common/adapters:adapter_gflags",
+        "//modules/common/math:geometry",
         "//modules/prediction/proto:prediction_conf_proto",
         "//modules/prediction/container:container_manager",
         "//modules/prediction/scenario/analyzer:scenario_analyzer",
         "//modules/prediction/scenario/feature_extractor:feature_extractor",
+        "//modules/prediction/scenario/scenario_features:scenario_features",
     ],
 )
 

modules/prediction/scenario/analyzer/scenario_analyzer.cc

@@ -35,8 +35,8 @@ const Scenario& ScenarioAnalyzer::scenario() const {
   return scenario_;
 }
 
-std::shared_ptr<const ScenarioFeatures>
-ScenarioAnalyzer::GetScenarioFeatures() const {
+std::shared_ptr<ScenarioFeatures>
+ScenarioAnalyzer::GetScenarioFeatures() {
   return nullptr;
 }
 

modules/prediction/scenario/analyzer/scenario_analyzer.h

@@ -36,7 +36,7 @@ class ScenarioAnalyzer {
 
   const Scenario& scenario() const;
 
-  std::shared_ptr<const ScenarioFeatures> GetScenarioFeatures() const;
+  std::shared_ptr<ScenarioFeatures> GetScenarioFeatures();
  private:
   Scenario scenario_;
 };

modules/prediction/scenario/feature_extractor/feature_extractor.cc

@@ -77,7 +77,7 @@ const EnvironmentFeatures& FeatureExtractor::GetEnvironmentFeatures() const {
 
 void FeatureExtractor::ExtractEgoVehicleFeatures() {
   // TODO(all): change this to ego_speed and ego_heading
-  environment_features_.set_ego_velocity(pose_container_->GetSpeed());
+  environment_features_.set_ego_speed(pose_container_->GetSpeed());
   environment_features_.set_ego_heading(pose_container_->GetTheta());
   // TODO(all): add acceleration if needed
 }

modules/prediction/scenario/scenario_features/scenario_features.h

@@ -28,7 +28,7 @@ class ScenarioFeatures {
  public:
   ScenarioFeatures() = default;
 
-  ~ScenarioFeatures() = default;
+  virtual ~ScenarioFeatures() = default;
 };
 
 }  // namespace prediction

modules/prediction/scenario/scenario_manager.cc

@@ -15,7 +15,12 @@
  *****************************************************************************/
 
 #include "modules/prediction/scenario/scenario_manager.h"
+
+#include <cmath>
+#include <algorithm>
+
 #include "modules/common/adapters/adapter_gflags.h"
+#include "modules/common/math/vec2d.h"
 #include "modules/prediction/container/container_manager.h"
 #include "modules/prediction/container/obstacles/obstacles_container.h"
 
@@ -23,6 +28,7 @@ namespace apollo {
 namespace prediction {
 
 using apollo::common::adapter::AdapterConfig;
+using apollo::common::math::Vec2d;
 
 ScenarioManager::ScenarioManager() {}
 
@@ -41,9 +47,90 @@ void ScenarioManager::PrioritizeObstacles() {
   ObstaclesContainer* obstacles_container = dynamic_cast<ObstaclesContainer*>(
       ContainerManager::Instance()->GetContainer(
           AdapterConfig::PERCEPTION_OBSTACLES));
-  obstacles_container->PrioritizeObstacles(
-      scenario_analyzer_.scenario(),
-      feature_extractor_.GetEnvironmentFeatures());
+  if (obstacles_container == nullptr) {
+    AERROR << "Null obstacles container found.";
+    return;
+  }
+  const std::vector<int>& obstacle_ids =
+      obstacles_container->GetCurrentFramePredictableObstacleIds();
+  for (const int obstacle_id : obstacle_ids) {
+    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(obstacle_id);
+    PrioritizeObstacle(
+        feature_extractor_.GetEnvironmentFeatures(),
+        scenario_analyzer_.GetScenarioFeatures(),
+        obstacle_ptr);
+  }
+}
+
+void ScenarioManager::PrioritizeObstacle(
+    const EnvironmentFeatures& environment_features,
+    std::shared_ptr<ScenarioFeatures> scenario_features,
+    Obstacle* obstacle_ptr) {
+  const auto& scenario_type = scenario_analyzer_.scenario().type();
+  if (scenario_type == Scenario::CRUISE ||
+      scenario_type == Scenario::CRUISE_URBAN ||
+      scenario_type == Scenario::CRUISE_HIGHWAY) {
+    PrioritizeObstacleInCruise(environment_features,
+        std::dynamic_pointer_cast<CruiseScenarioFeatures>(scenario_features),
+        obstacle_ptr);
+  } else if (scenario_type == Scenario::JUNCTION ||
+             scenario_type == Scenario::JUNCTION_TRAFFIC_LIGHT ||
+             scenario_type == Scenario::JUNCTION_STOP_SIGN) {
+    // TODO(kechxu) PrioritizeObstaclesInJunction
+  }
+}
+
+void ScenarioManager::PrioritizeObstacleInCruise(
+    const EnvironmentFeatures& environment_features,
+    std::shared_ptr<CruiseScenarioFeatures> cruise_scenario_features,
+    Obstacle* obstacle_ptr) {
+  if (obstacle_ptr->history_size() == 0) {
+    AERROR << "Obstacle [" << obstacle_ptr->id() << "] has no feature.";
+    return;
+  }
+  Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
+  bool has_lane_of_interest = false;
+  if (obstacle_ptr->IsOnLane()) {
+    for (const auto& curr_lane :
+         latest_feature_ptr->lane().current_lane_feature()) {
+      const std::string& curr_lane_id = curr_lane.lane_id();
+      if (cruise_scenario_features->IsLaneOfInterest(curr_lane_id)) {
+        has_lane_of_interest = true;
+        break;
+      }
+    }
+    if (!has_lane_of_interest) {
+      for (const auto& nearby_lane :
+           latest_feature_ptr->lane().nearby_lane_feature()) {
+        const std::string& nearby_lane_id = nearby_lane.lane_id();
+        if (cruise_scenario_features->IsLaneOfInterest(nearby_lane_id)) {
+          has_lane_of_interest = true;
+          break;
+        }
+      }
+    }
+    if (!has_lane_of_interest) {
+      latest_feature_ptr->set_priority(Feature::IGNORE);
+    }
+  } else {
+    // Obstacle has neither lane nor neighbor lanes
+    // Filter obstacle by its relative position to ego vehicle
+    double ego_x = environment_features.get_ego_position().x();
+    double ego_y = environment_features.get_ego_position().y();
+    double ego_heading = environment_features.get_ego_heading();
+    double ego_speed = environment_features.get_ego_speed();
+    double obstacle_x = latest_feature_ptr->position().x();
+    double obstacle_y = latest_feature_ptr->position().y();
+    Vec2d ego_to_obstacle_vec(obstacle_x - ego_x, obstacle_y - ego_y);
+    Vec2d ego_vec = Vec2d::CreateUnitVec2d(ego_heading);
+    double l = ego_vec.CrossProd(ego_to_obstacle_vec);
+    double s = ego_to_obstacle_vec.InnerProd(ego_vec);
+    if (std::abs(l) > 10.0 ||
+        s > std::max(20.0, ego_speed * 5.0) ||
+        s < 0.0) {
+      latest_feature_ptr->set_priority(Feature::IGNORE);
+    }
+  }
 }
 
 }  // namespace prediction

modules/prediction/scenario/scenario_manager.h

@@ -23,12 +23,15 @@
 
 #include <vector>
 #include <string>
+#include <memory>
 
 #include "cybertron/common/macros.h"
 #include "modules/prediction/proto/scenario.pb.h"
 #include "modules/prediction/proto/prediction_conf.pb.h"
 #include "modules/prediction/scenario/feature_extractor/feature_extractor.h"
 #include "modules/prediction/scenario/analyzer/scenario_analyzer.h"
+#include "modules/prediction/container/obstacles/obstacle.h"
+#include "modules/prediction/scenario/scenario_features/cruise_scenario_features.h"
 
 namespace apollo {
 namespace prediction {
@@ -48,6 +51,16 @@ class ScenarioManager {
  private:
   void PrioritizeObstacles();
 
+  void PrioritizeObstacle(
+      const EnvironmentFeatures& environment_features,
+      std::shared_ptr<ScenarioFeatures> scenario_features,
+      Obstacle* obstacle_ptr);
+
+  void PrioritizeObstacleInCruise(
+      const EnvironmentFeatures& environment_features,
+      std::shared_ptr<CruiseScenarioFeatures> scenario_features,
+      Obstacle* obstacle_ptr);
+
  private:
   FeatureExtractor feature_extractor_;
   ScenarioAnalyzer scenario_analyzer_;

modules/prediction/scenario/scenario_manager_test.cc

@@ -38,6 +38,7 @@ class ScenarioManagerTest : public ::testing::Test {
 };
 
 TEST_F(ScenarioManagerTest, run) {
+  // TODO(kechxu) add unit tests with concrete contents
   ContainerManager::Instance()->RegisterContainers();
   std::unique_ptr<Container> adc_traj_container =
       ContainerManager::Instance()->CreateContainer(