From f5c253ab198cdd2e307ce55c9be1c4a17e2f2bb6 Mon Sep 17 00:00:00 2001 From: Jinghao Miao Date: Tue, 25 Jul 2017 19:48:02 -0700 Subject: [PATCH] Added lane sequence filters --- .../predictor/vehicle/free_move_predictor.cc | 3 + .../vehicle/lane_sequence_predictor.cc | 101 ++++++++++++++++-- .../vehicle/lane_sequence_predictor.h | 37 +++++++ 3 files changed, 131 insertions(+), 10 deletions(-) diff --git a/modules/prediction/predictor/vehicle/free_move_predictor.cc b/modules/prediction/predictor/vehicle/free_move_predictor.cc index e1884c920c..cef0c4b31f 100644 --- a/modules/prediction/predictor/vehicle/free_move_predictor.cc +++ b/modules/prediction/predictor/vehicle/free_move_predictor.cc @@ -70,6 +70,9 @@ void FreeMovePredictor::Predict(Obstacle* obstacle) { prediction_obstacle_.set_predicted_period(FLAGS_prediction_duration); prediction_obstacle_.add_trajectory()->CopyFrom(trajectory); SetEqualProbability(1.0, start_index); + ADEBUG << "Obstacle [" << obstacle->id() + << "] has " << prediction_obstacle_.trajectory_size() + << " trajectories."; } void FreeMovePredictor::DrawFreeMoveTrajectoryPoints( diff --git a/modules/prediction/predictor/vehicle/lane_sequence_predictor.cc b/modules/prediction/predictor/vehicle/lane_sequence_predictor.cc index 8089224561..25bd0c2bf3 100644 --- a/modules/prediction/predictor/vehicle/lane_sequence_predictor.cc +++ b/modules/prediction/predictor/vehicle/lane_sequence_predictor.cc @@ -42,18 +42,27 @@ void LaneSequencePredictor::Predict(Obstacle* obstacle) { return; } - for (const auto& sequence : feature.lane().lane_graph().lane_sequence()) { + std::string lane_id = ""; + if (feature.lane().has_lane_feature()) { + lane_id = feature.lane().lane_feature().lane_id(); + } + int num_lane_sequence = feature.lane().lane_graph().lane_sequence_size(); + std::vector enable_lane_sequence(num_lane_sequence, true); + FilterLaneSequences(feature.lane().lane_graph(), + lane_id, + &enable_lane_sequence); + + for (int i = 0; i < num_lane_sequence; ++i) { + const LaneSequence& sequence = feature.lane().lane_graph().lane_sequence(i); if (sequence.lane_segment_size() == 0) { AERROR << "Empty lane segments."; continue; } - if (!sequence.has_probability() || - sequence.probability() < FLAGS_lane_sequence_threshold) { - AERROR << "Lane sequence [" << ToString(sequence) - << "] has probability [" << sequence.probability() - << "] less than threshold [" << FLAGS_lane_sequence_threshold - << "]"; + if (!enable_lane_sequence[i]) { + ADEBUG << "Lane sequence [" << ToString(sequence) + << "] with probability [" + <CopyFrom(trajectory); + ADEBUG << "Obstacle [" << obstacle->id() + << "] has " << prediction_obstacle_.trajectory_size() + << " trajectories."; } } +void LaneSequencePredictor::FilterLaneSequences( + const LaneGraph& lane_graph, + const std::string& lane_id, + std::vector *enable_lane_sequence) { + int num_lane_sequence = lane_graph.lane_sequence_size(); + std::vector lane_change_type(num_lane_sequence, -1); + std::pair change(-1, -1.0); + std::pair all(-1, -1.0); + + for (int i = 0; i < num_lane_sequence; ++i) { + const LaneSequence& sequence = lane_graph.lane_sequence(i); + + // Get lane change type + int lane_change = GetLaneChangeType(lane_id, sequence); + lane_change_type[i] = lane_change; + + double probability = sequence.probability(); + + if (::apollo::common::math::DoubleCompare(probability, all.second) > 0 || + (::apollo::common::math::DoubleCompare(probability, all.second) == 0 && + lane_change_type[i] == 0)) { + all.first = i; + all.second = probability; + } + if (lane_change_type[i] > 0 && + ::apollo::common::math::DoubleCompare(probability, change.second) > 0) { + change.first = i; + change.second = probability; + } + } + + for (int i = 0; i < num_lane_sequence; ++i) { + const LaneSequence& sequence = lane_graph.lane_sequence(i); + double probability = sequence.probability(); + if (::apollo::common::math::DoubleCompare( + probability, FLAGS_lane_sequence_threshold) < 0 && + i != all.first) { + (*enable_lane_sequence)[i] = false; + } else if (lane_change_type[i] > 0 && + lane_change_type[i] != change.first) { + (*enable_lane_sequence)[i] = false; + } + } +} + +int LaneSequencePredictor::GetLaneChangeType( + const std::string& lane_id, + const LaneSequence& lane_sequence) { + PredictionMap *map = PredictionMap::instance(); + CHECK_NOTNULL(map); + + std::string lane_change_id = lane_sequence.lane_segment(0).lane_id(); + if (lane_id == lane_change_id) { + return 0; + } else { + if (map->IsLeftNeighborLane(map->LaneById(lane_change_id), + map->LaneById(lane_id))) { + return 1; + } else if (map->IsRightNeighborLane( + map->LaneById(lane_change_id), map->LaneById(lane_id))) { + return 2; + } + } + return -1; +} + void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints( const KalmanFilter& kf, const LaneSequence& sequence, @@ -168,9 +246,12 @@ void LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints( } std::string LaneSequencePredictor::ToString(const LaneSequence& sequence) { - std::string str_lane_sequence; - for (const auto& lane_segment: sequence.lane_segment()) { - str_lane_sequence = str_lane_sequence + "->" + lane_segment.lane_id(); + std::string str_lane_sequence = ""; + if (sequence.lane_segment_size() > 0) { + str_lane_sequence += sequence.lane_segment(0).lane_id(); + } + for (int i = 1; i < sequence.lane_segment_size(); ++i) { + str_lane_sequence += ("->" + sequence.lane_segment(i).lane_id()); } return str_lane_sequence; } diff --git a/modules/prediction/predictor/vehicle/lane_sequence_predictor.h b/modules/prediction/predictor/vehicle/lane_sequence_predictor.h index 5e6e33ec53..e318b49f1f 100644 --- a/modules/prediction/predictor/vehicle/lane_sequence_predictor.h +++ b/modules/prediction/predictor/vehicle/lane_sequence_predictor.h @@ -25,6 +25,7 @@ #include "modules/prediction/predictor/predictor.h" #include +#include #include "modules/prediction/proto/lane_graph.pb.h" #include "modules/common/proto/path_point.pb.h" @@ -51,6 +52,37 @@ class LaneSequencePredictor : public Predictor { void Predict(Obstacle* obstacle) override; protected: + /** + * @brief Filter lane sequences + * @param Lane graph + * @param Current lane id + * @param Vector of boolean indicating if a lane sequence is disqualified + */ + void FilterLaneSequences(const LaneGraph& lane_graph, + const std::string& lane_id, + std::vector *enable_lane_sequence); + + /** + * @brief Get lane change type + * @param Current lane id + * @param Lane sequence + * @return Integer indicating lane change type: + * 0: no lane change + * 1: left lane change + * 2: right lane change + * -1: other + */ + int GetLaneChangeType(const std::string& lane_id, + const LaneSequence& lane_sequence); + + /** + * @brief Draw lane sequence trajectory points + * @param Kalman filter + * @param Lane sequence + * @param Total prediction time + * @param Prediction frequency + * @param A vector of generated trajectory points + */ void DrawLaneSequenceTrajectoryPoints( const ::apollo::common::math::KalmanFilter& kf, const LaneSequence& sequence, @@ -58,6 +90,11 @@ class LaneSequencePredictor : public Predictor { double freq, std::vector<::apollo::common::TrajectoryPoint> *points); + /** + * @brief Convert a lane sequence to string + * @param Lane sequence + * @return String describing the lane sequence + */ std::string ToString(const LaneSequence& sequence); }; -- GitLab