From c70ddec55912f33766bc3ca22111828d250329c2 Mon Sep 17 00:00:00 2001 From: kechxu Date: Mon, 4 Nov 2019 11:49:16 -0800 Subject: [PATCH] Prediction: unify extrapolation speed --- .../extrapolation/extrapolation_predictor.cc | 58 +++++++++++-------- .../extrapolation/extrapolation_predictor.h | 5 ++ 2 files changed, 40 insertions(+), 23 deletions(-) diff --git a/modules/prediction/predictor/extrapolation/extrapolation_predictor.cc b/modules/prediction/predictor/extrapolation/extrapolation_predictor.cc index 71972151ac..d1eb5e43a0 100644 --- a/modules/prediction/predictor/extrapolation/extrapolation_predictor.cc +++ b/modules/prediction/predictor/extrapolation/extrapolation_predictor.cc @@ -60,10 +60,12 @@ void ExtrapolationPredictor::PostProcess(Trajectory* trajectory_ptr) { constexpr int kNumTailPoint = 5; ExtrapolationPredictor::LaneSearchResult lane_search_result = SearchExtrapolationLane(*trajectory_ptr, kNumTailPoint); + double extraplation_speed = + ComputeExtraplationSpeed(kNumTailPoint, *trajectory_ptr); if (lane_search_result.found) { - ExtrapolateByLane(lane_search_result, trajectory_ptr); + ExtrapolateByLane(lane_search_result, extraplation_speed, trajectory_ptr); } else { - ExtrapolateByFreeMove(kNumTailPoint, trajectory_ptr); + ExtrapolateByFreeMove(kNumTailPoint, extraplation_speed, trajectory_ptr); } } @@ -96,7 +98,8 @@ ExtrapolationPredictor::SearchExtrapolationLane(const Trajectory& trajectory, } void ExtrapolationPredictor::ExtrapolateByLane( - const LaneSearchResult& lane_search_result, Trajectory* trajectory_ptr) { + const LaneSearchResult& lane_search_result, const double extraplation_speed, + Trajectory* trajectory_ptr) { std::string start_lane_id = lane_search_result.lane_id; int point_index = lane_search_result.point_index; while (trajectory_ptr->trajectory_point_size() > point_index + 1) { @@ -105,7 +108,7 @@ void ExtrapolationPredictor::ExtrapolateByLane( auto lane_info_ptr = PredictionMap::LaneById(start_lane_id); int num_trajectory_point = trajectory_ptr->trajectory_point_size(); const TrajectoryPoint& last_point = - trajectory_ptr->trajectory_point(num_trajectory_point); + trajectory_ptr->trajectory_point(num_trajectory_point - 1); Eigen::Vector2d position(last_point.path_point().x(), last_point.path_point().y()); @@ -120,11 +123,10 @@ void ExtrapolationPredictor::ExtrapolateByLane( } double last_relative_time = last_point.relative_time(); - double speed = last_point.v(); double time_range = FLAGS_prediction_trajectory_time_length - last_relative_time; double time_resolution = FLAGS_prediction_trajectory_time_resolution; - double length = speed * time_range; + double length = extraplation_speed * time_range; LaneGraph lane_graph = ObstacleClusters::GetLaneGraph(lane_s, length, false, lane_info_ptr); @@ -153,11 +155,11 @@ void ExtrapolationPredictor::ExtrapolateByLane( path_point->set_z(0.0); path_point->set_theta(theta); path_point->set_lane_id(lane_id); - trajectory_point->set_v(speed); + trajectory_point->set_v(extraplation_speed); trajectory_point->set_a(0.0); trajectory_point->set_relative_time(relative_time); - lane_s += speed * time_resolution; + lane_s += extraplation_speed * time_resolution; while (lane_s > PredictionMap::LaneById(lane_id)->total_length() && lane_segment_index + 1 < lane_sequence.lane_segment_size()) { lane_segment_index += 1; @@ -169,41 +171,51 @@ void ExtrapolationPredictor::ExtrapolateByLane( } } -void ExtrapolationPredictor::ExtrapolateByFreeMove(const int num_tail_point, - Trajectory* trajectory_ptr) { +void ExtrapolationPredictor::ExtrapolateByFreeMove( + const int num_tail_point, const double extraplation_speed, + Trajectory* trajectory_ptr) { int num_trajectory_point = trajectory_ptr->trajectory_point_size(); - CHECK_GT(num_trajectory_point, num_tail_point); - double time_resolution = FLAGS_prediction_trajectory_time_resolution; - double time_length = FLAGS_prediction_trajectory_time_length; const TrajectoryPoint& last_point = trajectory_ptr->trajectory_point(num_trajectory_point - 1); - const TrajectoryPoint& mid_point = - trajectory_ptr->trajectory_point(num_trajectory_point - num_tail_point); double theta = last_point.path_point().theta(); - double diff_x = last_point.path_point().x() - mid_point.path_point().x(); - double diff_y = last_point.path_point().y() - mid_point.path_point().y(); - double speed = - std::hypot(diff_x, diff_y) / (num_tail_point * time_resolution); + double time_resolution = FLAGS_prediction_trajectory_time_resolution; + double time_length = FLAGS_prediction_trajectory_time_length; double relative_time = last_point.relative_time() + time_resolution; while (relative_time < time_length) { int prev_size = trajectory_ptr->trajectory_point_size(); const TrajectoryPoint& prev_point = - trajectory_ptr->trajectory_point(prev_size); + trajectory_ptr->trajectory_point(prev_size - 1); TrajectoryPoint* curr_point = trajectory_ptr->add_trajectory_point(); - double dx = time_resolution * speed * std::cos(theta); - double dy = time_resolution * speed * std::sin(theta); + double dx = time_resolution * extraplation_speed * std::cos(theta); + double dy = time_resolution * extraplation_speed * std::sin(theta); double curr_x = prev_point.path_point().x() + dx; double curr_y = prev_point.path_point().y() + dy; PathPoint* curr_path_point_ptr = curr_point->mutable_path_point(); curr_path_point_ptr->set_x(curr_x); curr_path_point_ptr->set_y(curr_y); curr_path_point_ptr->set_theta(theta); - curr_point->set_v(speed); + curr_point->set_v(extraplation_speed); curr_point->set_relative_time(relative_time); relative_time += time_resolution; } } +double ExtrapolationPredictor::ComputeExtraplationSpeed( + const int num_tail_point, const Trajectory& trajectory) { + int num_trajectory_point = trajectory.trajectory_point_size(); + CHECK_GT(num_trajectory_point, num_tail_point); + double time_resolution = FLAGS_prediction_trajectory_time_resolution; + const TrajectoryPoint& last_point = + trajectory.trajectory_point(num_trajectory_point - 1); + const TrajectoryPoint& mid_point = + trajectory.trajectory_point(num_trajectory_point - num_tail_point); + double diff_x = last_point.path_point().x() - mid_point.path_point().x(); + double diff_y = last_point.path_point().y() - mid_point.path_point().y(); + double speed = + std::hypot(diff_x, diff_y) / (num_tail_point * time_resolution); + return speed; +} + } // namespace prediction } // namespace apollo diff --git a/modules/prediction/predictor/extrapolation/extrapolation_predictor.h b/modules/prediction/predictor/extrapolation/extrapolation_predictor.h index fd1c1a032c..09e5c3675e 100644 --- a/modules/prediction/predictor/extrapolation/extrapolation_predictor.h +++ b/modules/prediction/predictor/extrapolation/extrapolation_predictor.h @@ -65,10 +65,15 @@ class ExtrapolationPredictor : public SequencePredictor { const int num_tail_point); void ExtrapolateByLane(const LaneSearchResult& lane_search_result, + const double extrapolation_speed, Trajectory* trajectory_ptr); void ExtrapolateByFreeMove(const int num_tail_point, + const double extrapolation_speed, Trajectory* trajectory_ptr); + + double ComputeExtraplationSpeed(const int num_tail_point, + const Trajectory& trajectory); }; } // namespace prediction -- GitLab