From 4d156dd4193b12f1de86dbde7b85a189621571d3 Mon Sep 17 00:00:00 2001 From: kechxu Date: Tue, 26 Mar 2019 17:09:07 -0700 Subject: [PATCH] Prediction: implement simplified lane sequence --- .../prediction/common/prediction_gflags.cc | 2 + modules/prediction/common/prediction_gflags.h | 1 + modules/prediction/common/prediction_map.cc | 4 + modules/prediction/common/road_graph.cc | 73 +++++++++++++++---- modules/prediction/common/road_graph.h | 20 +++++ .../container/obstacles/obstacle.cc | 3 +- 6 files changed, 88 insertions(+), 15 deletions(-) diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 4fd58fd2d8..3e785f85fe 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -162,6 +162,8 @@ DEFINE_double(distance_threshold_to_junction_exit, 1.0, "Threshold of distance to junction exit"); DEFINE_double(angle_threshold_to_junction_exit, M_PI * 0.25, "Threshold of angle to junction exit"); +DEFINE_uint32(sample_size_for_average_lane_curvature, 10, + "The sample size to compute average lane curvature"); // Validation checker DEFINE_double(centripetal_acc_coeff, 0.5, diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 5377e5b037..44b4e24b16 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -102,6 +102,7 @@ DECLARE_double(heading_filter_param); DECLARE_uint64(max_num_lane_point); DECLARE_double(distance_threshold_to_junction_exit); DECLARE_double(angle_threshold_to_junction_exit); +DECLARE_uint32(sample_size_for_average_lane_curvature); // Validation checker DECLARE_double(centripetal_acc_coeff); diff --git a/modules/prediction/common/prediction_map.cc b/modules/prediction/common/prediction_map.cc index d2bf8d0741..74e6370e98 100644 --- a/modules/prediction/common/prediction_map.cc +++ b/modules/prediction/common/prediction_map.cc @@ -51,6 +51,10 @@ double PredictionMap::HeadingOnLane( double PredictionMap::CurvatureOnLane(const std::string& lane_id, const double s) { std::shared_ptr lane_info = LaneById(lane_id); + if (lane_info == nullptr) { + AERROR << "Null lane_info ptr found"; + return 0.0; + } return lane_info->Curvature(s); } diff --git a/modules/prediction/common/road_graph.cc b/modules/prediction/common/road_graph.cc index 7ee7f94d99..d6f7b55bbf 100644 --- a/modules/prediction/common/road_graph.cc +++ b/modules/prediction/common/road_graph.cc @@ -178,30 +178,77 @@ void RoadGraph::ComputeLaneSequence( // Sort the successor lane_segments from left to right. std::vector> successor_lanes; for (const auto& successor_lane_id : lane_info_ptr->lane().successor_id()) { - successor_lanes.push_back( + successor_lanes.emplace_back( PredictionMap::LaneById(successor_lane_id.id())); } std::sort(successor_lanes.begin(), successor_lanes.end(), IsAtLeft); - if (consider_divide) { - if (successor_lanes.size() > 1) { - // Run recursion function to perform DFS. - for (size_t i = 0; i < successor_lanes.size(); i++) { - ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lanes[i], - graph_search_horizon - 1, false, lane_segments, - lane_graph_ptr); + if (!successor_lanes.empty()) { + if (consider_divide) { + if (successor_lanes.size() > 1) { + // Run recursion function to perform DFS. + for (size_t i = 0; i < successor_lanes.size(); i++) { + ComputeLaneSequence( + successor_accumulated_s, 0.0, successor_lanes[i], + graph_search_horizon - 1, false, lane_segments, + lane_graph_ptr); + } + } else { + ComputeLaneSequence( + successor_accumulated_s, 0.0, successor_lanes[0], + graph_search_horizon - 1, true, lane_segments, + lane_graph_ptr); } } else { - ComputeLaneSequence(successor_accumulated_s, 0.0, successor_lanes[0], - graph_search_horizon - 1, true, lane_segments, - lane_graph_ptr); + auto selected_successor_lane = + LaneWithSmallestAverageCurvature(successor_lanes); + ComputeLaneSequence( + successor_accumulated_s, 0.0, selected_successor_lane, + graph_search_horizon - 1, false, lane_segments, + lane_graph_ptr); } - } else { - // TODO(kechxu) select a successor lane } } lane_segments->pop_back(); } +std::shared_ptr +RoadGraph::LaneWithSmallestAverageCurvature( + const std::vector>& lane_infos) +const { + CHECK(!lane_infos.empty()); + size_t sample_size = FLAGS_sample_size_for_average_lane_curvature; + std::shared_ptr selected_lane_info = lane_infos[0]; + double smallest_curvature = AverageCurvature( + selected_lane_info->id().id(), sample_size); + for (size_t i = 1; i < lane_infos.size(); ++i) { + std::shared_ptr lane_info = lane_infos[i]; + double curvature = AverageCurvature(lane_info->id().id(), sample_size); + if (curvature < smallest_curvature) { + smallest_curvature = curvature; + selected_lane_info = lane_info; + } + } + return selected_lane_info; +} + +double RoadGraph::AverageCurvature( + const std::string& lane_id, const size_t sample_size) const { + CHECK_GT(sample_size, 0); + std::shared_ptr lane_info_ptr = + PredictionMap::LaneById(lane_id); + if (lane_info_ptr == nullptr) { + return 0.0; + } + double lane_length = lane_info_ptr->total_length(); + double s_gap = lane_length / static_cast(sample_size); + double curvature_sum = 0.0; + for (size_t i = 0; i < sample_size; ++i) { + double s = s_gap * static_cast(i); + curvature_sum += PredictionMap::CurvatureOnLane(lane_id, s); + } + return curvature_sum / static_cast(sample_size); +} + } // namespace prediction } // namespace apollo diff --git a/modules/prediction/common/road_graph.h b/modules/prediction/common/road_graph.h index f4c9556f27..38b1b5bd74 100644 --- a/modules/prediction/common/road_graph.h +++ b/modules/prediction/common/road_graph.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include "modules/prediction/proto/lane_graph.pb.h" @@ -78,6 +79,25 @@ class RoadGraph { std::vector* const lane_segments, LaneGraph* const lane_graph_ptr) const; + /** + * @brief Get the pointer to the lane with the smallest average curvature + * @param The vector of lane infos + * @return The pointer to the lane with the smallest average curvature + */ + std::shared_ptr + LaneWithSmallestAverageCurvature( + const std::vector>& lane_infos) + const; + + /** + * @brief Get the average curvature along a lane with the ID lane_id + * @param The ID of the lane + * @param The size of samples alone the lane to compute the average curvature + * @return The average curvature + */ + double AverageCurvature(const std::string& lane_id, + const size_t sample_size) const; + private: // The s of the obstacle on its own lane_segment. double start_s_ = 0; diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index fd0217b153..abfd7f75da 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/modules/prediction/container/obstacles/obstacle.cc @@ -988,8 +988,7 @@ void Obstacle::BuildLaneGraph() { } double speed = feature->speed(); double t_max = FLAGS_prediction_trajectory_time_length; - double a_max = FLAGS_vehicle_max_linear_acc; - auto estimated_move_distance = speed * t_max + 0.5 * a_max * t_max * t_max; + auto estimated_move_distance = speed * t_max; double road_graph_search_distance = std::fmax( estimated_move_distance, FLAGS_min_prediction_trajectory_spatial_length); -- GitLab