Prediction: tracking adaptation (#6433)

* Prediction: add id_mapping from perception_id to prediction_id

* Prediction: add prediction_id for SetId

* Prediction: add current frame id mapping logic for pred_id adaptation

* Prediction: disable adapt_tracking for now

* Prediction: some variable name changes

* Prediction: use raw_velocity for possible position tracking

* Prediction: move enable_tracking_adaptation to gflag

* Prediction: tracking_adaptation fix

* Prediction: move tracking_adaptation params to gflags

modules/prediction/common/prediction_gflags.cc

@@ -29,6 +29,8 @@ DEFINE_double(min_prediction_trajectory_spatial_length, 20.0,
               "Minimal spatial length of predicted trajectory");
 DEFINE_bool(enable_trajectory_validation_check, false,
             "If check the validity of prediction trajectory.");
+DEFINE_bool(enable_tracking_adaptation, false,
+            "If enable prediction tracking adaptation");
 
 DEFINE_double(vehicle_max_linear_acc, 4.0,
               "Upper bound of vehicle linear acceleration");
@@ -37,6 +39,12 @@ DEFINE_double(vehicle_min_linear_acc, -4.0,
 DEFINE_double(vehicle_max_speed, 35.0,
               "Max speed of vehicle");
 
+// Tracking Adaptation
+DEFINE_double(max_tracking_time, 0.5,
+              "Max tracking time for disappear obstacles");
+DEFINE_double(max_tracking_dist, 3.0,
+              "Max tracking distance for disappear obstacles");
+
 // Map
 DEFINE_double(lane_search_radius, 3.0, "Search radius for a candidate lane");
 DEFINE_double(lane_search_radius_in_junction, 15.0,

modules/prediction/common/prediction_gflags.h

@@ -25,11 +25,16 @@ DECLARE_double(prediction_trajectory_time_length);
 DECLARE_double(prediction_trajectory_time_resolution);
 DECLARE_double(min_prediction_trajectory_spatial_length);
 DECLARE_bool(enable_trajectory_validation_check);
+DECLARE_bool(enable_tracking_adaptation);
 
 DECLARE_double(vehicle_max_linear_acc);
 DECLARE_double(vehicle_min_linear_acc);
 DECLARE_double(vehicle_max_speed);
 
+// Tracking Adaptation
+DECLARE_double(max_tracking_time);
+DECLARE_double(max_tracking_dist);
+
 // Map
 DECLARE_double(lane_search_radius);
 DECLARE_double(lane_search_radius_in_junction);

modules/prediction/container/obstacles/obstacle.cc

@@ -152,7 +152,7 @@ bool Obstacle::IsNearJunction() {
 }
 
 void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
-                      const double timestamp) {
+                      const double timestamp, int pred_id) {
   if (feature_history_.size() > 0 &&
       timestamp <= feature_history_.front().timestamp()) {
     AERROR << "Obstacle [" << id_ << "] received an older frame ["
@@ -164,13 +164,14 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
 
   // Set ID, Type, and Status of the feature.
   Feature feature;
-  if (SetId(perception_obstacle, &feature) == ErrorCode::PREDICTION_ERROR) {
+  if (SetId(perception_obstacle, &feature, pred_id) ==
+      ErrorCode::PREDICTION_ERROR) {
     return;
   }
   if (SetType(perception_obstacle, &feature) == ErrorCode::PREDICTION_ERROR) {
     return;
   }
-  SetStatus(perception_obstacle, timestamp, &feature);
+  SetStatus(perception_obstacle, timestamp, &feature, pred_id);
 
   // Set obstacle observation for KF tracking
   if (!FLAGS_use_navigation_mode) {
@@ -320,8 +321,10 @@ void Obstacle::SetJunctionFeatureWithoutEnterLane(
 }
 
 void Obstacle::SetStatus(const PerceptionObstacle& perception_obstacle,
-                         const double timestamp, Feature* feature) {
-  if (SetId(perception_obstacle, feature) == ErrorCode::PREDICTION_ERROR) {
+                         const double timestamp, Feature* feature,
+                         int pred_id) {
+  if (SetId(perception_obstacle, feature, pred_id) ==
+      ErrorCode::PREDICTION_ERROR) {
     AERROR << "Obstacle has no ID";
     return;
   }
@@ -411,13 +414,13 @@ void Obstacle::UpdateStatus(Feature* feature) {
 }
 
 ErrorCode Obstacle::SetId(const PerceptionObstacle& perception_obstacle,
-                          Feature* feature) {
+                          Feature* feature, int pred_id) {
   if (!perception_obstacle.has_id()) {
     AERROR << "Obstacle has no ID.";
     return ErrorCode::PREDICTION_ERROR;
   }
 
-  int id = perception_obstacle.id();
+  int id = pred_id > 0 ? pred_id : perception_obstacle.id();
   if (id_ < 0) {
     id_ = id;
     ADEBUG << "Obstacle has id [" << id_ << "].";

modules/prediction/container/obstacles/obstacle.h

@@ -60,7 +60,7 @@ class Obstacle {
    * @param timestamp The timestamp when the perception obstacle was detected.
    */
   void Insert(const perception::PerceptionObstacle& perception_obstacle,
-              const double timestamp);
+              const double timestamp, int prediction_id);
 
   /**
    * @brief Insert a feature proto message.
@@ -226,13 +226,13 @@ class Obstacle {
 
  private:
   void SetStatus(const perception::PerceptionObstacle& perception_obstacle,
-                 double timestamp, Feature* feature);
+                 double timestamp, Feature* feature, int prediction_id);
 
   void UpdateStatus(Feature* feature);
 
   common::ErrorCode SetId(
       const perception::PerceptionObstacle& perception_obstacle,
-      Feature* feature);
+      Feature* feature, int prediction_id = -1);
 
   common::ErrorCode SetType(
       const perception::PerceptionObstacle& perception_obstacle,

modules/prediction/container/obstacles/obstacles_container.cc

@@ -17,6 +17,7 @@
 #include "modules/prediction/container/obstacles/obstacles_container.h"
 
 #include <utility>
+#include <unordered_set>
 
 #include "modules/prediction/common/feature_output.h"
 #include "modules/prediction/common/junction_analyzer.h"
@@ -32,13 +33,15 @@ using apollo::perception::PerceptionObstacles;
 
 
 ObstaclesContainer::ObstaclesContainer()
-    : obstacles_(FLAGS_max_num_obstacles) {}
+    : obstacles_(FLAGS_max_num_obstacles),
+      id_mapping_(FLAGS_max_num_obstacles) {}
 
 // This is called by Perception module at every frame to insert all
 // detected obstacles.
 void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   // Clean up the history and get the PerceptionObstacles
   curr_frame_predictable_obstacle_ids_.clear();
+  curr_frame_id_mapping_.clear();
   PerceptionObstacles perception_obstacles;
   perception_obstacles.CopyFrom(
       dynamic_cast<const PerceptionObstacles&>(message));
@@ -71,6 +74,11 @@ void ObstaclesContainer::Insert(const ::google::protobuf::Message& message) {
   timestamp_ = timestamp;
   ADEBUG << "Current timestamp is [" << timestamp_ << "]";
 
+  // Prediction tracking adaptation
+  if (FLAGS_enable_tracking_adaptation) {
+    BuildCurrentFrameIdMapping(perception_obstacles);
+  }
+
   // Set up the ObstacleClusters:
   // 1. Initialize ObstacleClusters
   ObstacleClusters::Init();
@@ -113,6 +121,7 @@ ObstaclesContainer::GetCurrentFramePredictableObstacleIds() const {
 
 void ObstaclesContainer::Clear() {
   obstacles_.Clear();
+  id_mapping_.Clear();
   timestamp_ = -1.0;
 }
 
@@ -120,27 +129,37 @@ 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 << "]";
+  int prediction_id = perception_obstacle.id();
+  if (curr_frame_id_mapping_.find(prediction_id) !=
+      curr_frame_id_mapping_.end()) {
+    prediction_id = curr_frame_id_mapping_[prediction_id];
+    if (prediction_id != perception_obstacle.id()) {
+      ADEBUG << "Obstacle have got AdaptTracking, with perception_id: "
+             << perception_obstacle.id()
+             << ", and prediction_id: " << prediction_id;
+    }
+  }
+  if (prediction_id < -1) {
+    AERROR << "Invalid ID [" << prediction_id << "]";
     return;
   }
   if (!IsPredictable(perception_obstacle)) {
-    ADEBUG << "Perception obstacle [" << id << "] is not predictable.";
+    ADEBUG << "Perception obstacle [" << prediction_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);
+  curr_frame_predictable_obstacle_ids_.push_back(prediction_id);
+  Obstacle* obstacle_ptr = obstacles_.Get(prediction_id);
   if (obstacle_ptr != nullptr) {
-    obstacle_ptr->Insert(perception_obstacle, timestamp);
-    ADEBUG << "Refresh obstacle [" << id << "]";
+    obstacle_ptr->Insert(perception_obstacle, timestamp, prediction_id);
+    ADEBUG << "Refresh obstacle [" << prediction_id << "]";
   } else {
     Obstacle obstacle;
-    obstacle.Insert(perception_obstacle, timestamp);
-    obstacles_.Put(id, std::move(obstacle));
-    ADEBUG << "Insert obstacle [" << id << "]";
+    obstacle.Insert(perception_obstacle, timestamp, prediction_id);
+    obstacles_.Put(prediction_id, std::move(obstacle));
+    ADEBUG << "Insert obstacle [" << prediction_id << "]";
   }
 }
 
@@ -160,6 +179,69 @@ void ObstaclesContainer::InsertFeatureProto(const Feature& feature) {
   }
 }
 
+
+void ObstaclesContainer::BuildCurrentFrameIdMapping(
+    const PerceptionObstacles& perception_obstacles) {
+  // Go through every obstacle in the current frame, after some
+  // sanity checks, build current_frame_id_mapping for every obstacle
+
+  std::unordered_set<int> seen_perception_ids;
+  // Loop all precept_id and find those in obstacles_LRU
+  for (const PerceptionObstacle& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int perception_id = perception_obstacle.id();
+    if (GetObstacle(perception_id) != nullptr) {
+      seen_perception_ids.insert(perception_id);
+    }
+  }
+
+  for (const PerceptionObstacle& perception_obstacle :
+       perception_obstacles.perception_obstacle()) {
+    int perception_id = perception_obstacle.id();
+    curr_frame_id_mapping_[perception_id] = perception_id;
+    if (seen_perception_ids.find(perception_id) != seen_perception_ids.end()) {
+      // find this perception_id in LRUCache, treat it as a tracked obstacle
+      continue;
+    }
+    std::unordered_set<int> seen_prediction_ids;
+    int prediction_id = 0;
+    if (id_mapping_.GetCopy(perception_id, &prediction_id)) {
+      if (seen_perception_ids.find(prediction_id) ==
+          seen_perception_ids.end()) {
+        // find this perception_id in LRUMapping, map it to a tracked obstacle
+        curr_frame_id_mapping_[perception_id] = prediction_id;
+        seen_prediction_ids.insert(prediction_id);
+      }
+    } else {  // process adaption
+      common::util::Node<int, Obstacle>* curr = obstacles_.First();
+      while (curr != nullptr) {
+        int obs_id = curr->key;
+        curr = curr->next;
+        if (seen_perception_ids.find(obs_id) != seen_perception_ids.end() ||
+            seen_prediction_ids.find(obs_id) != seen_prediction_ids.end()) {
+          // this obs_id has already been processed
+          continue;
+        }
+        Obstacle* obstacle_ptr = obstacles_.GetSilently(obs_id);
+        if (obstacle_ptr == nullptr) {
+          AERROR << "Obstacle id [" << obs_id << "] with empty obstacle_ptr.";
+          break;
+        }
+        if (timestamp_ - obstacle_ptr->timestamp() > FLAGS_max_tracking_time) {
+          ADEBUG << "Obstacle already reach time threshold.";
+          break;
+        }
+        if (AdaptTracking(perception_obstacle, obstacle_ptr)) {
+          id_mapping_.Put(perception_id, obs_id);
+          curr_frame_id_mapping_[perception_id] = obs_id;
+          break;
+        }
+      }
+    }
+  }
+}
+
+
 void ObstaclesContainer::BuildLaneGraph() {
   // Go through every obstacle in the current frame, after some
   // sanity checks, build lane graph for non-junction cases.
@@ -201,6 +283,33 @@ void ObstaclesContainer::BuildJunctionFeature() {
 }
 
 
+bool ObstaclesContainer::AdaptTracking(
+    const PerceptionObstacle& perception_obstacle, Obstacle* obstacle_ptr) {
+  if (!perception_obstacle.has_type() ||
+      perception_obstacle.type() != obstacle_ptr->type()) {
+    // different obstacle type, can't be same obstacle
+    return false;
+  }
+  // test perception_obstacle position with possible obstacle position
+  if (perception_obstacle.has_position() &&
+      perception_obstacle.position().has_x() &&
+      perception_obstacle.position().has_y()) {
+    double obs_x = obstacle_ptr->latest_feature().position().x() + (timestamp_ -
+                   obstacle_ptr->latest_feature().timestamp()) *
+                   obstacle_ptr->latest_feature().raw_velocity().x();
+    double obs_y = obstacle_ptr->latest_feature().position().y() + (timestamp_ -
+                   obstacle_ptr->latest_feature().timestamp()) *
+                   obstacle_ptr->latest_feature().raw_velocity().y();
+    double dist = std::hypot(perception_obstacle.position().x() - obs_x,
+                             perception_obstacle.position().y() - obs_y);
+    if (dist < FLAGS_max_tracking_dist) {
+      return true;
+    }
+  }
+  return false;
+}
+
+
 bool ObstaclesContainer::IsPredictable(
     const PerceptionObstacle& perception_obstacle) {
   if (!perception_obstacle.has_type() ||

modules/prediction/container/obstacles/obstacles_container.h

@@ -22,6 +22,7 @@
 #pragma once
 
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "modules/common/util/lru_cache.h"
@@ -64,6 +65,12 @@ class ObstaclesContainer : public Container {
    */
   void InsertFeatureProto(const Feature& feature);
 
+  /*
+   * @brief Build current frame id mapping for all obstacles
+   */
+  void BuildCurrentFrameIdMapping(
+      const perception::PerceptionObstacles& perception_obstacles);
+
   /**
    * @brief Build lane graph for obstacles
    */
@@ -93,6 +100,15 @@ class ObstaclesContainer : public Container {
   void Clear();
 
  private:
+  /**
+   * @brief Check if a perception_obstacle is an old existed obstacle
+   * @param A PerceptionObstacle
+   * @param An obstacle_ptr
+   * @return True if the perception_obstacle is this obstacle; otherwise false;
+   */
+  bool AdaptTracking(const perception::PerceptionObstacle& perception_obstacle,
+                      Obstacle* obstacle_ptr);
+
   /**
    * @brief Check if an obstacle is predictable
    * @param An obstacle
@@ -103,7 +119,10 @@ class ObstaclesContainer : public Container {
  private:
   double timestamp_ = -1.0;
   common::util::LRUCache<int, Obstacle> obstacles_;
+  // an id_mapping from perception_id to prediction_id
+  common::util::LRUCache<int, int> id_mapping_;
   std::vector<int> curr_frame_predictable_obstacle_ids_;
+  std::unordered_map<int, int> curr_frame_id_mapping_;
 };
 
 }  // namespace prediction