implement SetMotionStatus() in prediction/container/obstacles/obstacle.cc

modules/prediction/common/prediction_gflags.cc

@@ -34,3 +34,10 @@ DEFINE_double(go_approach_rate, 0.995,
     "The rate to approach to the reference line of going straight");
 DEFINE_double(cutin_approach_rate, 0.9,
     "The rate to approach to the reference line of lane change");
+DEFINE_int32(still_obstacle_history_length, 10,
+    "Min # historical frames for still obstacles");
+DEFINE_bool(enable_kf_tracking, true, "Use measurements with KF tracking");
+DEFINE_double(still_obstacle_speed_threshold, 1.0,
+    "Speed threshold for still obstacles");
+DEFINE_double(still_obstacle_position_std, 1.0,
+    "Position standard deviation for still obstacles");

modules/prediction/common/prediction_gflags.h

@@ -31,5 +31,10 @@ DECLARE_double(r_var);
 DECLARE_double(p_var);
 DECLARE_double(go_approach_rate);
 DECLARE_double(cutin_approach_rate);
+DECLARE_int32(still_obstacle_history_length);
+DECLARE_bool(enable_kf_tracking);
+DECLARE_double(still_obstacle_speed_threshold);
+DECLARE_double(still_obstacle_position_std); 
+
 
 #endif  // MODULES_PREDICTION_COMMON_PREDICTION_GFLAGS_H_

modules/prediction/container/obstacles/obstacle.cc

@@ -609,9 +609,78 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
 }
 
 void Obstacle::SetMotionStatus() {
-  // TODO(kechxu) implement
+  bool is_still = true;
+  int history_size = static_cast<int>(feature_history_.size());
+  if (history_size < 2) {
+    ADEBUG << "Obstacle [" << id_ << "] has no history and "
+           << "is considered still [default = true].";
+    if (history_size > 0) {
+      feature_history_.front().set_is_still(is_still);
+      ADEBUG << "Obstacle [" << id_ << "] has stillness status [" << is_still
+             << "].";
+    }
+    return;
+  }
+
+  double start_x = 0.0;
+  double start_y = 0.0;
+  double avg_drift_x = 0.0;
+  double avg_drift_y = 0.0;
+  int len = std::min(history_size, FLAGS_still_obstacle_history_length);
+  CHECK(len > 1);
+
+  auto feature_riter = feature_history_.rbegin();
+  if (FLAGS_enable_kf_tracking) {
+    start_x = feature_riter->t_position().x();
+    start_y = feature_riter->t_position().y();
+  } else {
+    start_x = feature_riter->position().x();
+    start_y = feature_riter->position().y();
+  }
+  ++feature_riter;
+  while (feature_riter != feature_history_.rend()) {
+    if (FLAGS_enable_kf_tracking) {
+      avg_drift_x += (feature_riter->t_position().x() - start_x) / (len - 1);
+      avg_drift_y += (feature_riter->t_position().y() - start_y) / (len - 1);
+    } else {
+      avg_drift_x += (feature_riter->position().x() - start_x) / (len - 1);
+      avg_drift_y += (feature_riter->position().y() - start_y) / (len - 1);
+    }
+    ++feature_riter;
+  }
+
+  double delta_ts = feature_history_.front().timestamp() -
+                    feature_history_.back().timestamp();
+  double std = FLAGS_still_obstacle_position_std;
+  double speed_sensibility =
+      std::sqrt(2 * history_size) * 4 * std / ((history_size + 1) * delta_ts);
+  double speed = (FLAGS_enable_kf_tracking ? feature_history_.front().t_speed()
+                                           : feature_history_.front().speed());
+  double speed_threshold = FLAGS_still_obstacle_speed_threshold;
+  if (apollo::common::math::DoubleCompare(speed, speed_threshold) < 0) {
+    is_still = true;
+    ADEBUG << "Obstacle [" << id_
+           << "] has a small speed and is considered still.";
+  } else if (apollo::common::math::DoubleCompare(speed_sensibility,
+                                                 speed_threshold) < 0) {
+    is_still = false;
+    ADEBUG << "Obstacle [" << id_ << "] has a too short history ["
+           << history_size << "] and is considered moving [sensibility = "
+           << speed_sensibility << "]";
+  } else {
+    double distance = std::hypot(avg_drift_x, avg_drift_y);
+    double distance_std = std::sqrt(2.0 / len) * std;
+    if (apollo::common::math::DoubleCompare(distance, 2.0 * distance_std) > 0) {
+      is_still = false;
+      ADEBUG << "Obstacle [" << id_ << "] is moving.";
+    } else {
+      is_still = true;
+      ADEBUG << "Obstacle [" << id_ << "] is still.";
+    }
+  }
 }
 
+
 void Obstacle::InsertFeatureToHistory(Feature* feature) {
   // TODO(kechxu) implement
 }