From 9d46f0d24bdcb02a23b4b22cd981883ffac1bfd6 Mon Sep 17 00:00:00 2001 From: kechxu Date: Wed, 26 Jul 2017 14:32:00 -0700 Subject: [PATCH] implement pedestrian prediction trajectory generation --- .../prediction/common/prediction_gflags.cc | 3 + modules/prediction/common/prediction_gflags.h | 3 + .../pedestrian/regional_predictor.cc | 65 +++++++++++++++++++ .../predictor/pedestrian/regional_predictor.h | 14 ++++ 4 files changed, 85 insertions(+) diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index e78eeb6703..ac123f7ce5 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/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, diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 08995b5778..21e297d74c 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/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); diff --git a/modules/prediction/predictor/pedestrian/regional_predictor.cc b/modules/prediction/predictor/pedestrian/regional_predictor.cc index c9d8da3eb3..e9b3d95967 100644 --- a/modules/prediction/predictor/pedestrian/regional_predictor.cc +++ b/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* 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(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& kf, + const double total_time, + std::vector* left_points, + std::vector* 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 middle_points; + std::vector 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, diff --git a/modules/prediction/predictor/pedestrian/regional_predictor.h b/modules/prediction/predictor/pedestrian/regional_predictor.h index fdaf7d71df..ea09c52526 100644 --- a/modules/prediction/predictor/pedestrian/regional_predictor.h +++ b/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* points); + + void GenerateMovingTrajectory( + const Eigen::Vector2d& position, + const Eigen::Vector2d& velocity, + const Eigen::Vector2d& acceleration, + const apollo::common::math::KalmanFilter& kf, + const double total_time, + std::vector* left_points, + std::vector* right_points); + void GetTrajectoryCandidatePoints( const Eigen::Vector2d& position, const Eigen::Vector2d& velocity, -- GitLab