Added free move trajectory

modules/prediction/container/obstacles/obstacle.cc

@@ -118,6 +118,11 @@ const KalmanFilter<double, 4, 2, 0>& Obstacle::kf_lane_tracker(
   return kf_lane_trackers_[lane_id];
 }
 
+const KalmanFilter<double, 6, 2, 0>& Obstacle::kf_motion_tracker() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  return kf_motion_tracker_;
+}
+
 bool Obstacle::IsOnLane() {
   std::lock_guard<std::mutex> lock(mutex_);
   if (feature_history_.size() > 0) {

modules/prediction/container/obstacles/obstacle.h

@@ -64,6 +64,8 @@ class Obstacle {
   const apollo::common::math::KalmanFilter<double, 4, 2, 0>& kf_lane_tracker(
       const std::string& lane_id);
 
+  const apollo::common::math::KalmanFilter<double, 6, 2, 0>& kf_motion_tracker();
+
   bool IsOnLane();
 
  private:

modules/prediction/predictor/BUILD

@@ -41,6 +41,7 @@ cc_library(
     deps = [
         "//modules/prediction/proto:prediction_proto",
         "//modules/prediction/container/obstacles:obstacle",
+        "//modules/common/proto:path_point_proto",
     ]
 )
 

modules/prediction/predictor/pedestrian/regional_predictor.cc

@@ -19,7 +19,7 @@
 namespace apollo {
 namespace prediction {
 
-void RegionalPredictor::Predict(Obstacle* obstacle) const {}
+void RegionalPredictor::Predict(Obstacle* obstacle) {}
 
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/predictor/pedestrian/regional_predictor.h

@@ -43,7 +43,7 @@ class RegionalPredictor : public Predictor {
    * @brief Make prediction
    * @param Obstacle pointer
    */
-  void Predict(Obstacle* obstacle) const override;
+  void Predict(Obstacle* obstacle) override;
 };
 
 }  // namespace prediction

modules/prediction/predictor/predictor.cc

@@ -23,5 +23,31 @@ const PredictionObstacle& Predictor::prediction_obstacle() {
   return prediction_obstacle_;
 }
 
+int Predictor::GetTrajectorySize() {
+  return prediction_obstacle_.trajectory_size();
+}
+
+void Predictor::GenerateTrajectory(
+      const std::vector<::apollo::common::TrajectoryPoint>& points,
+      Trajectory *trajectory) {
+  if (points.size() <= 0) {
+    return;
+  }
+
+  for (const auto& point : points) {
+    trajectory->add_trajectory_point()->CopyFrom(point);
+  }
+}
+
+void Predictor::SetEqualProbability(double probability, int start_index) {
+  int num = GetTrajectorySize();
+  if (start_index > 0 && num > 0 && num > start_index) {
+    probability = probability / (double(num - start_index));
+    for (int i = start_index; i < num; ++i) {
+      prediction_obstacle_.mutable_trajectory(i)->set_probability(probability);
+    }
+  }
+}
+
 }  // namespace prediction
 }  // namespace apollo

modules/prediction/predictor/predictor.h

@@ -25,6 +25,7 @@
 #include "modules/prediction/proto/prediction_obstacle.pb.h"
 
 #include "modules/prediction/container/obstacles/obstacle.h"
+#include "modules/common/proto/path_point.pb.h"
 
 /**
  * @namespace apollo::prediction
@@ -54,7 +55,25 @@ class Predictor {
    * @brief Make prediction
    * @param Obstacle pointer
    */
-  virtual void Predict(Obstacle* obstacle) const = 0;
+  virtual void Predict(Obstacle* obstacle) = 0;
+
+  /**
+   * @brief Get trajectory size
+   * @return Size of trajectories
+   */
+  int GetTrajectorySize();
+
+ protected:
+  /**
+   * @brief Generate trajectory from trajectory points
+   * @param A vector of trajectory points
+   *        A pointer to generated trajectory
+   */
+  void GenerateTrajectory(
+      const std::vector<::apollo::common::TrajectoryPoint>& points,
+      Trajectory *trajectory);
+
+  void SetEqualProbability(double probability, int start_index);
 
  protected:
   PredictionObstacle prediction_obstacle_;

modules/prediction/predictor/vehicle/BUILD

@@ -8,7 +8,7 @@ cc_library(
     srcs = ["lane_sequence_predictor.cc"],
     deps = [
         "//modules/prediction/predictor:predictor",
-        "@glog//:glog",
+        "//modules/common:log",
     ]
 )
 
@@ -18,7 +18,10 @@ cc_library(
     srcs = ["free_move_predictor.cc"],
     deps = [
         "//modules/prediction/predictor:predictor",
-        "@glog//:glog",
+        "//modules/prediction/proto:feature_proto",
+        "//modules/prediction/common:prediction_gflags",
+        "//modules/common:log",
+        "@eigen//:eigen",
     ]
 )
 

modules/prediction/predictor/vehicle/free_move_predictor.cc

@@ -16,10 +16,49 @@
 
 #include "modules/prediction/predictor/vehicle/free_move_predictor.h"
 
+#include <vector>
+#include "Eigen/Dense"
+
+#include "modules/prediction/common/prediction_gflags.h"
+#include "modules/prediction/proto/feature.pb.h"
+
 namespace apollo {
 namespace prediction {
 
-void FreeMovePredictor::Predict(Obstacle* obstacle) const {}
+using ::apollo::common::TrajectoryPoint;
+
+void FreeMovePredictor::Predict(Obstacle* obstacle) {
+  CHECK_NOTNULL(obstacle);
+  CHECK_GT(obstacle->history_size(), 0);
+
+  const Feature& feature = obstacle->latest_feature();
+  if (!feature.has_position() || !feature.has_velocity() ||
+      !feature.position().has_x() || !feature.position().has_y()) {
+    AERROR << "Obstacle [" << obstacle->id()
+           << " is missing position or velocity";
+    return;
+  }
+
+  Eigen::Vector2d position(feature.position().x(), feature.position().y());
+  Eigen::Vector2d velocity(feature.velocity().x(), feature.velocity().y());
+  if (FLAGS_enable_kf_tracking) {
+    position(0) = feature.t_position().x();
+    position(1) = feature.t_position().y();
+    velocity(0) = feature.t_velocity().x();
+    velocity(1) = feature.t_velocity().y();
+  }
+
+  std::vector<TrajectoryPoint> points(0);
+  double total_time = FLAGS_prediction_duration;
+  // TODO(jinghao):
+  // draw_free_move_trajectory(position, velocity, obstacle->kf_motion_tracker(),
+  //                           total_time, points);
+  Trajectory trajectory;
+  GenerateTrajectory(points, &trajectory);
+  trajectory.set_probability(1.0);
+  prediction_obstacle_.set_predicted_period(total_time);
+  prediction_obstacle_.add_trajectory()->CopyFrom(trajectory);
+}
 
 }  // prediction
 }  // apollo

modules/prediction/predictor/vehicle/free_move_predictor.h

@@ -43,7 +43,7 @@ class FreeMovePredictor : public Predictor {
    * @brief Make prediction
    * @param Obstacle pointer
    */
-  void Predict(Obstacle* obstacle) const override;
+  void Predict(Obstacle* obstacle) override;
 };
 
 }  // namespace prediction

modules/prediction/predictor/vehicle/lane_sequence_predictor.cc

@@ -19,7 +19,7 @@
 namespace apollo {
 namespace prediction {
 
-void LaneSequencePredictor::Predict(Obstacle* obstacle) const {}
+void LaneSequencePredictor::Predict(Obstacle* obstacle) {}
 
 }  // prediction
 }  // apollo

modules/prediction/predictor/vehicle/lane_sequence_predictor.h

@@ -43,7 +43,7 @@ class LaneSequencePredictor : public Predictor {
    * @brief Make prediction
    * @param Obstacle pointer
    */
-  void Predict(Obstacle* obstacle) const override;
+  void Predict(Obstacle* obstacle) override;
 };
 
 }  // namespace prediction