implement pedestrian prediction trajectory generation

modules/prediction/common/prediction_gflags.cc

@@ -59,6 +59,9 @@ DEFINE_double(max_lane_angle_diff, M_PI / 2.0,
     "Max angle difference for a candiate lane");
 DEFINE_bool(enable_pedestrian_acc, false, "Enable calculating speed by acc");
 DEFINE_double(coeff_mul_sigma, 2.0, "coefficient multiply standard deviation");
+DEFINE_double(pedestrian_max_speed, 10.0, "speed upper bound for pedestrian");
+DEFINE_double(pedestrian_min_acc, -4.0, "minimum pedestrian acceleration");
+DEFINE_double(pedestrian_max_acc, 2.0, "maximum pedestrian acceleration");
 
 // Obstacle trajectory
 DEFINE_double(lane_sequence_threshold, 0.5,

modules/prediction/common/prediction_gflags.h

@@ -50,6 +50,9 @@ DECLARE_double(target_lane_gap);
 DECLARE_double(max_lane_angle_diff);
 DECLARE_bool(enable_pedestrian_acc);
 DECLARE_double(coeff_mul_sigma);
+DECLARE_double(pedestrian_max_speed);
+DECLARE_double(pedestrian_min_acc);
+DECLARE_double(pedestrian_max_acc);
 
 // Obstacle trajectory
 DECLARE_double(lane_sequence_threshold);

modules/prediction/predictor/pedestrian/regional_predictor.cc

@@ -70,6 +70,71 @@ void RegionalPredictor::Predict(Obstacle* obstacle) {
   // TODO(kechxu) implement
 }
 
+void RegionalPredictor::GenerateStillTrajectory(
+    const Eigen::Vector2d& position,
+    const double heading, const double speed, const double total_time,
+    std::vector<TrajectoryPoint>* points) {
+  double delta_ts = FLAGS_prediction_freq;
+  double x = position[0];
+  double y = position[1];
+  double direction_x = std::cos(heading);
+  double direction_y = std::sin(heading);
+  for (int i = 0; i < static_cast<int>(total_time / delta_ts); ++i) {
+    TrajectoryPoint point;
+    point.mutable_path_point()->set_x(x);
+    point.mutable_path_point()->set_y(y);
+    point.mutable_path_point()->set_theta(heading);
+    point.set_v(speed);
+    point.set_relative_time(i * delta_ts);
+    points->push_back(std::move(point));
+    x += direction_x * speed * delta_ts;
+    y += direction_y * speed * delta_ts;
+  }
+}
+
+void RegionalPredictor::GenerateMovingTrajectory(
+    const Eigen::Vector2d& position,
+    const Eigen::Vector2d& velocity,
+    const Eigen::Vector2d& acceleration,
+    const apollo::common::math::KalmanFilter<double, 2, 2, 4>& kf,
+    const double total_time,
+    std::vector<TrajectoryPoint>* left_points,
+    std::vector<TrajectoryPoint>* right_points) {
+
+  double delta_ts = FLAGS_prediction_freq;
+  Eigen::Vector2d vel = velocity;
+  CompressVector2d(FLAGS_pedestrian_max_speed, &vel);
+  Eigen::Vector2d acc = acceleration;
+  CompressVector2d(FLAGS_pedestrian_max_acc, &acc);
+  double speed = std::hypot(vel[0], vel[1]);
+
+  // candidate point sequences
+  std::vector<TrajectoryPoint> middle_points;
+  std::vector<TrajectoryPoint> boundary_points;
+
+  TrajectoryPoint starting_point;
+  starting_point.mutable_path_point()->set_x(0.0);
+  starting_point.mutable_path_point()->set_y(0.0);
+  starting_point.set_v(speed);
+
+  Eigen::Vector2d translated_vec(0.0, 0.0);
+  GetTrajectoryCandidatePoints(
+      translated_vec, vel, acc, kf, total_time,
+      &middle_points, &boundary_points);
+
+  if (middle_points.empty() || boundary_points.size() < 2) {
+    ADEBUG << "No valid points found.";
+    return;
+  }
+
+  UpdateTrajectoryPoints(starting_point, vel, delta_ts,
+      middle_points, boundary_points, left_points, right_points);
+  for (size_t i = 0; i < left_points->size(); ++i) {
+    TranslatePoint(position[0], position[1], &(left_points->operator[](i)));
+    TranslatePoint(position[0], position[1], &(right_points->operator[](i)));
+  }
+}
+
 void RegionalPredictor::GetTrajectoryCandidatePoints(
     const Eigen::Vector2d& position,
     const Eigen::Vector2d& velocity,

modules/prediction/predictor/pedestrian/regional_predictor.h

@@ -50,6 +50,20 @@ class RegionalPredictor : public Predictor {
    */
   void Predict(Obstacle* obstacle) override;
 
+  void GenerateStillTrajectory(
+      const Eigen::Vector2d& position,
+      const double heading, const double speed, const double total_time,
+      std::vector<apollo::common::TrajectoryPoint>* points);
+
+  void GenerateMovingTrajectory(
+      const Eigen::Vector2d& position,
+      const Eigen::Vector2d& velocity,
+      const Eigen::Vector2d& acceleration,
+      const apollo::common::math::KalmanFilter<double, 2, 2, 4>& kf,
+      const double total_time,
+      std::vector<apollo::common::TrajectoryPoint>* left_points,
+      std::vector<apollo::common::TrajectoryPoint>* right_points);
+
   void GetTrajectoryCandidatePoints(
       const Eigen::Vector2d& position,
       const Eigen::Vector2d& velocity,