#include <sequence_predictor.h>

类 apollo::prediction::SequencePredictor 继承关系图:

apollo::prediction::SequencePredictor 的协作图:

Public 类型
enum class	LaneChangeType { LEFT , RIGHT , STRAIGHT , ONTO_LANE , INVALID }

Public 成员函数
	SequencePredictor ()=default
	Constructor

virtual	~SequencePredictor ()=default
	Destructor

bool	Predict (const ADCTrajectoryContainer adc_trajectory_container, Obstacle obstacle, ObstaclesContainer *obstacles_container) override
	Make prediction

	FRIEND_TEST (SequencePredictorTest, General)

Public 成员函数继承自 apollo::prediction::Predictor
	Predictor ()=default
	Constructor

virtual	~Predictor ()=default
	Destructor

int	NumOfTrajectories (const Obstacle &obstacle)
	Get trajectory size

void	TrimTrajectories (const ADCTrajectoryContainer &adc_trajectory_container, Obstacle *obstacle)
	Trim prediction trajectories by adc trajectory

const ObstacleConf::PredictorType &	predictor_type ()
	get the predictor type

Protected 成员函数
bool	GetLongitudinalPolynomial (const Obstacle &obstacle, const LaneSequence &lane_sequence, const std::pair< double, double > &lon_end_state, std::array< double, 5 > *coefficients)

bool	GetLateralPolynomial (const Obstacle &obstacle, const LaneSequence &lane_sequence, const double time_to_end_state, std::array< double, 4 > *coefficients)

void	FilterLaneSequences (const Feature &feature, const std::string &lane_id, const Obstacle ego_vehicle_ptr, const ADCTrajectoryContainer adc_trajectory_container, std::vector< bool > *enable_lane_sequence)
	Filter lane sequences

LaneChangeType	GetLaneChangeType (const std::string &lane_id, const LaneSequence &lane_sequence)
	Get lane change type

double	GetLaneChangeDistanceWithADC (const LaneSequence &lane_sequence, const Obstacle ego_vehicle_ptr, const ADCTrajectoryContainer adc_trajectory_container)
	Get lane change distance with ADC

void	DrawConstantAccelerationTrajectory (const Obstacle &obstacle, const LaneSequence &lane_sequence, const double total_time, const double period, const double acceleration, std::vector< apollo::common::TrajectoryPoint > *points)
	Draw constant acceleration trajectory points

double	GetLaneSequenceCurvatureByS (const LaneSequence &lane_sequence, const double s)
	Get lane sequence curvature by s

void	Clear ()
	Clear private members

std::string	ToString (const LaneSequence &sequence)
	Convert a lane sequence to string

Protected 成员函数继承自 apollo::prediction::Predictor
void	SetEqualProbability (const double probability, const int start_index, Obstacle *obstacle_ptr)
	Set equal probability to prediction trajectories

bool	TrimTrajectory (const ADCTrajectoryContainer &adc_trajectory_container, Obstacle obstacle, Trajectory trajectory)
	Trim a single prediction trajectory, keep the portion that is not in junction.

bool	SupposedToStop (const Feature &feature, const double stop_distance, double *acceleration)
	Determine if an obstacle is supposed to stop within a distance

额外继承的成员函数
静态 Protected 成员函数继承自 apollo::prediction::Predictor
static Trajectory	GenerateTrajectory (const std::vector< apollo::common::TrajectoryPoint > &points)
	Generate trajectory from trajectory points

Protected 属性继承自 apollo::prediction::Predictor
ObstacleConf::PredictorType	predictor_type_

详细描述

在文件 sequence_predictor.h 第 35 行定义.

成员枚举类型说明

◆ LaneChangeType

enum class apollo::prediction::SequencePredictor::LaneChangeType

strong

枚举值
LEFT
RIGHT
STRAIGHT
ONTO_LANE
INVALID

在文件 sequence_predictor.h 第 37 行定义.

                            {
    LEFT,
    RIGHT,
    STRAIGHT,
    ONTO_LANE,
    INVALID,
  };

构造及析构函数说明

◆ SequencePredictor()

apollo::prediction::SequencePredictor::SequencePredictor ( )

default

Constructor

◆ ~SequencePredictor()

virtual apollo::prediction::SequencePredictor::~SequencePredictor ( )

virtualdefault

Destructor

成员函数说明

◆ Clear()

void apollo::prediction::SequencePredictor::Clear ( )

protectedvirtual

Clear private members

重载 apollo::prediction::Predictor .

在文件 sequence_predictor.cc 第 45 行定义.

45{ Predictor::Clear(); }

apollo::prediction::Predictor::Clear

virtual void Clear()

Clear all trajectories

Definition predictor.cc:55

◆ DrawConstantAccelerationTrajectory()

void apollo::prediction::SequencePredictor::DrawConstantAccelerationTrajectory	(	const Obstacle &	obstacle,
		const LaneSequence &	lane_sequence,
		const double	total_time,
		const double	period,
		const double	acceleration,
		std::vector< apollo::common::TrajectoryPoint > *	points
	)

protected

Draw constant acceleration trajectory points

参数

Obstacle
Lane	sequence
Total	prediction time
Prediction	period
acceleration
A	vector of generated trajectory points

在文件 sequence_predictor.cc 第 260 行定义.

                                        {
  const Feature& feature = obstacle.latest_feature();
  if (!feature.has_position() || !feature.has_velocity() ||
      !feature.position().has_x() || !feature.position().has_y()) {
    AERROR << "Obstacle [" << obstacle.id()
           << " is missing position or velocity";
    return;
  }
 
  Eigen::Vector2d position(feature.position().x(), feature.position().y());
  double speed = feature.speed();
 
  int lane_segment_index = 0;
  std::string lane_id =
      lane_sequence.lane_segment(lane_segment_index).lane_id();
  std::shared_ptr<const LaneInfo> lane_info = PredictionMap::LaneById(lane_id);
  double lane_s = 0.0;
  double lane_l = 0.0;
  if (!PredictionMap::GetProjection(position, lane_info, &lane_s, &lane_l)) {
    AERROR << "Failed in getting lane s and lane l";
    return;
  }
  size_t total_num = static_cast<size_t>(total_time / period);
  for (size_t i = 0; i < total_num; ++i) {
    double relative_time = static_cast<double>(i) * period;
    Eigen::Vector2d point;
    double theta = M_PI;
    if (!PredictionMap::SmoothPointFromLane(lane_id, lane_s, lane_l, &point,
                                            &theta)) {
      AERROR << "Unable to get smooth point from lane [" << lane_id
             << "] with s [" << lane_s << "] and l [" << lane_l << "]";
      break;
    }
    TrajectoryPoint trajectory_point;
    PathPoint path_point;
    path_point.set_x(point.x());
    path_point.set_y(point.y());
    path_point.set_z(0.0);
    path_point.set_theta(theta);
    path_point.set_lane_id(lane_id);
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed);
    trajectory_point.set_a(0.0);
    trajectory_point.set_relative_time(relative_time);
    points->emplace_back(std::move(trajectory_point));
 
    if (speed < FLAGS_double_precision) {
      continue;
    }
 
    lane_s += speed * period + 0.5 * acceleration * period * period;
    speed += acceleration * period;
 
    while (lane_s > PredictionMap::LaneById(lane_id)->total_length() &&
           lane_segment_index + 1 < lane_sequence.lane_segment_size()) {
      lane_segment_index += 1;
      lane_s = lane_s - PredictionMap::LaneById(lane_id)->total_length();
      lane_id = lane_sequence.lane_segment(lane_segment_index).lane_id();
    }
 
    lane_l *= FLAGS_go_approach_rate;
  }
}

◆ FilterLaneSequences()

void apollo::prediction::SequencePredictor::FilterLaneSequences	(	const Feature &	feature,
		const std::string &	lane_id,
		const Obstacle *	ego_vehicle_ptr,
		const ADCTrajectoryContainer *	adc_trajectory_container,
		std::vector< bool > *	enable_lane_sequence
	)

protected

Filter lane sequences

参数

Lane	graph
Current	lane id
Ego	vehicle pointer
ADC	trajectory container
Vector	of boolean indicating if a lane sequence is disqualified

Filter out those obstacles that are close to the ADC so that we will ignore them and drive normally unless they really kick into our lane.

Pick the most probable lane-sequence and lane-change

在文件 sequence_predictor.cc 第 58 行定义.

                                           {
  if (!feature.has_lane() || !feature.lane().has_lane_graph()) {
    return;
  }
  const LaneGraph& lane_graph = feature.lane().lane_graph();
  int num_lane_sequence = lane_graph.lane_sequence_size();
  std::vector<LaneChangeType> lane_change_type(num_lane_sequence,
                                               LaneChangeType::INVALID);
  std::pair<int, double> change(-1, -1.0);
  std::pair<int, double> all(-1, -1.0);
 
  for (int i = 0; i < num_lane_sequence; ++i) {
    const LaneSequence& sequence = lane_graph.lane_sequence(i);
    if (sequence.lane_type() == apollo::hdmap::Lane::PARKING) {
      (*enable_lane_sequence)[i] = false;
      ADEBUG << "Ignore lane sequence [" << ToString(sequence) << "].";
      continue;
    }
    lane_change_type[i] = GetLaneChangeType(lane_id, sequence);
 
    if (lane_change_type[i] != LaneChangeType::LEFT &&
        lane_change_type[i] != LaneChangeType::RIGHT &&
        lane_change_type[i] != LaneChangeType::ONTO_LANE) {
      ADEBUG << "Ignore lane sequence [" << ToString(sequence) << "].";
      continue;
    }
 
    // The obstacle has interference with ADC within a small distance
    double distance = GetLaneChangeDistanceWithADC(sequence, ego_vehicle_ptr,
                                                   adc_trajectory_container);
    ADEBUG << "Distance to ADC " << std::fixed << std::setprecision(6)
           << distance;
    if (distance > 0.0 && distance < FLAGS_lane_change_dist) {
      bool obs_within_its_own_lane = true;
      for (int j = 0; j < feature.polygon_point_size(); j++) {
        Eigen::Vector2d position(feature.polygon_point(j).x(),
                                 feature.polygon_point(j).y());
        std::shared_ptr<const LaneInfo> lane_info =
            PredictionMap::LaneById(lane_id);
        if (lane_info == nullptr) {
          obs_within_its_own_lane = false;
          break;
        }
 
        double lane_s = 0.0;
        double lane_l = 0.0;
        PredictionMap::GetProjection(position, lane_info, &lane_s, &lane_l);
 
        double left = 0.0;
        double right = 0.0;
        lane_info->GetWidth(lane_s, &left, &right);
 
        if (lane_l > left || lane_l < -right) {
          obs_within_its_own_lane = false;
          break;
        }
      }
 
      if (obs_within_its_own_lane) {
        (*enable_lane_sequence)[i] = false;
        ADEBUG << "Filter trajectory [" << ToString(sequence)
               << "] due to small distance " << distance << ".";
      }
    }
  }
 
  for (int i = 0; i < num_lane_sequence; ++i) {
    const LaneSequence& sequence = lane_graph.lane_sequence(i);
 
    if (!(*enable_lane_sequence)[i]) {
      ADEBUG << "Disabled lane sequence [" << ToString(sequence) << "].";
      continue;
    }
 
    double probability = sequence.probability();
    if (LaneSequenceWithMaxProb(lane_change_type[i], probability, all.second)) {
      all.first = i;
      all.second = probability;
    }
    if (LaneChangeWithMaxProb(lane_change_type[i], probability,
                              change.second)) {
      change.first = i;
      change.second = probability;
    }
  }
 
  double lane_sequence_threshold = FLAGS_lane_sequence_threshold_cruise;
  if (feature.has_junction_feature()) {
    lane_sequence_threshold = FLAGS_lane_sequence_threshold_junction;
  }
 
  for (int i = 0; i < num_lane_sequence; ++i) {
    const LaneSequence& sequence = lane_graph.lane_sequence(i);
 
    if (!(*enable_lane_sequence)[i]) {
      ADEBUG << "Disabled lane sequence [" << ToString(sequence) << "].";
      continue;
    }
 
    double probability = sequence.probability();
    if (probability < lane_sequence_threshold && i != all.first) {
      (*enable_lane_sequence)[i] = false;
    } else if (change.first >= 0 && change.first < num_lane_sequence &&
               (lane_change_type[i] == LaneChangeType::LEFT ||
                lane_change_type[i] == LaneChangeType::RIGHT) &&
               lane_change_type[i] != lane_change_type[change.first]) {
      (*enable_lane_sequence)[i] = false;
    }
  }
}

◆ FRIEND_TEST()

apollo::prediction::SequencePredictor::FRIEND_TEST	(	SequencePredictorTest	,
		General
	)

◆ GetLaneChangeDistanceWithADC()

double apollo::prediction::SequencePredictor::GetLaneChangeDistanceWithADC	(	const LaneSequence &	lane_sequence,
		const Obstacle *	ego_vehicle_ptr,
		const ADCTrajectoryContainer *	adc_trajectory_container
	)

protected

Get lane change distance with ADC

参数

Target	lane sequence
Ego	vehicle pointer
ADC	trajectory container

返回: Lane change distance with ADC

在文件 sequence_predictor.cc 第 203 行定义.

                                                            {
  if (!adc_trajectory_container->HasOverlap(lane_sequence)) {
    ADEBUG << "The sequence [" << ToString(lane_sequence)
           << "] has no overlap with ADC.";
    return std::numeric_limits<double>::max();
  }
 
  Eigen::Vector2d adc_position;
  if (ego_vehicle_ptr != nullptr && ego_vehicle_ptr->history_size() > 0) {
    const auto& position = ego_vehicle_ptr->latest_feature().position();
    adc_position[0] = position.x();
    adc_position[1] = position.y();
 
    std::string obstacle_lane_id = lane_sequence.lane_segment(0).lane_id();
    double obstacle_lane_s = lane_sequence.lane_segment(0).start_s();
    double lane_s = 0.0;
    double lane_l = 0.0;
    if (PredictionMap::GetProjection(adc_position,
                                     PredictionMap::LaneById(obstacle_lane_id),
                                     &lane_s, &lane_l)) {
      ADEBUG << "Distance with ADC is " << std::fabs(lane_s - obstacle_lane_s);
      return obstacle_lane_s - lane_s;
    }
  }
 
  ADEBUG << "Invalid ADC pose.";
  return std::numeric_limits<double>::max();
}

◆ GetLaneChangeType()

SequencePredictor::LaneChangeType apollo::prediction::SequencePredictor::GetLaneChangeType	(	const std::string &	lane_id,
		const LaneSequence &	lane_sequence
	)

protected

Get lane change type

参数

Current	lane id
Lane	sequence

返回: Integer indicating lane change type:

在文件 sequence_predictor.cc 第 181 行定义.

                                                                 {
  if (lane_id.empty()) {
    return LaneChangeType::ONTO_LANE;
  }
 
  std::string lane_change_id = lane_sequence.lane_segment(0).lane_id();
  if (lane_id == lane_change_id) {
    return LaneChangeType::STRAIGHT;
  } else {
    auto ptr_change_lane = PredictionMap::LaneById(lane_change_id);
    auto ptr_current_lane = PredictionMap::LaneById(lane_id);
    if (PredictionMap::IsLeftNeighborLane(ptr_change_lane, ptr_current_lane)) {
      return LaneChangeType::LEFT;
    } else if (PredictionMap::IsRightNeighborLane(ptr_change_lane,
                                                  ptr_current_lane)) {
      return LaneChangeType::RIGHT;
    }
  }
  return LaneChangeType::INVALID;
}

◆ GetLaneSequenceCurvatureByS()

double apollo::prediction::SequencePredictor::GetLaneSequenceCurvatureByS	(	const LaneSequence &	lane_sequence,
		const double	s
	)

protected

Get lane sequence curvature by s

参数

lane	sequence
s

返回: the curvature

在文件 sequence_predictor.cc 第 327 行定义.

                                                       {
  CHECK_GT(lane_sequence.lane_segment_size(), 0);
  double lane_s = s + lane_sequence.lane_segment(0).start_s();
  for (const LaneSegment& lane_segment : lane_sequence.lane_segment()) {
    std::string lane_id = lane_segment.lane_id();
    std::shared_ptr<const LaneInfo> lane_info_ptr =
        PredictionMap::LaneById(lane_id);
    double lane_length = lane_info_ptr->total_length();
    if (lane_s > lane_length + FLAGS_double_precision) {
      lane_s -= lane_length;
    } else {
      return lane_info_ptr->Curvature(lane_s);
    }
  }
  ADEBUG << "Outside lane sequence range, use 0.0 to approximate.";
  return 0.0;
}

◆ GetLateralPolynomial()

bool apollo::prediction::SequencePredictor::GetLateralPolynomial	(	const Obstacle &	obstacle,
		const LaneSequence &	lane_sequence,
		const double	time_to_end_state,
		std::array< double, 4 > *	coefficients
	)

protected

在文件 sequence_predictor.cc 第 401 行定义.

                                                                       {
  // Sanity check.
  CHECK_NOTNULL(coefficients);
  CHECK_GT(obstacle.history_size(), 0U);
  CHECK_GT(lane_sequence.lane_segment_size(), 0);
  CHECK_GT(lane_sequence.lane_segment(0).lane_point_size(), 0);
 
  // Get obstacle info.
  const Feature& feature = obstacle.latest_feature();
  double theta = feature.velocity_heading();
  double v = feature.speed();
  Point3D position = feature.position();
  int lane_seg_start_idx = lane_sequence.adc_lane_segment_idx();
  int lane_point_start_idx =
      lane_sequence.lane_segment(lane_seg_start_idx).adc_lane_point_idx();
  if (lane_point_start_idx >=
      lane_sequence.lane_segment(lane_seg_start_idx).lane_point_size()) {
    lane_point_start_idx =
        lane_sequence.lane_segment(lane_seg_start_idx).lane_point_size() - 1;
  }
  const LanePoint& start_lane_point =
      lane_sequence.lane_segment(lane_seg_start_idx)
          .lane_point(lane_point_start_idx);
 
  // Get lane info.
  double pos_delta_x = position.x() - start_lane_point.position().x();
  double pos_delta_y = position.y() - start_lane_point.position().y();
  double lane_heading_x = std::cos(start_lane_point.heading());
  double lane_heading_y = std::sin(start_lane_point.heading());
 
  // Check if obstacle is to the left(+) or right(-) of the lane.
  double cross_prod =
      lane_heading_x * pos_delta_y - lane_heading_y * pos_delta_x;
  double shift = std::hypot(pos_delta_x, pos_delta_y);
 
  // Set the initial conditions for solving diff. eqn.
  double l0 = (cross_prod > 0) ? shift : -shift;
  double dl0 = v * std::sin(theta - start_lane_point.heading());
  double l1 = 0.0;
  double dl1 = 0.0;
 
  // Solve for the coefficients.
  coefficients->operator[](0) = l0;
  coefficients->operator[](1) = dl0;
  double p = time_to_end_state;
  double p2 = p * p;
  double p3 = p2 * p;
  double tmp_var1 = (l1 - dl0) * p;
  double tmp_var2 = dl1 - l0 - dl0 * p;
  coefficients->operator[](2) = (3.0 * tmp_var2 - tmp_var1) / p2;
  coefficients->operator[](3) = (tmp_var1 - 2.0 * tmp_var2) / p3;
 
  return true;
}

◆ GetLongitudinalPolynomial()

bool apollo::prediction::SequencePredictor::GetLongitudinalPolynomial	(	const Obstacle &	obstacle,
		const LaneSequence &	lane_sequence,
		const std::pair< double, double > &	lon_end_state,
		std::array< double, 5 > *	coefficients
	)

protected

在文件 sequence_predictor.cc 第 346 行定义.

                                       {
  // Sanity check.
  CHECK_NOTNULL(coefficients);
  CHECK_GT(obstacle.history_size(), 0U);
  CHECK_GT(lane_sequence.lane_segment_size(), 0);
  CHECK_GT(lane_sequence.lane_segment(0).lane_point_size(), 0);
 
  // Get obstacle info.
  const Feature& feature = obstacle.latest_feature();
  double theta = feature.velocity_heading();
  double v = feature.speed();
  double a = 0.0;
 
  // Get lane info.
  if (FLAGS_enable_lane_sequence_acc && lane_sequence.has_acceleration()) {
    a = lane_sequence.acceleration();
  }
  int lane_seg_start_idx = lane_sequence.adc_lane_segment_idx();
  int lane_point_start_idx =
      lane_sequence.lane_segment(lane_seg_start_idx).adc_lane_point_idx();
  if (lane_point_start_idx >=
      lane_sequence.lane_segment(lane_seg_start_idx).lane_point_size()) {
    lane_point_start_idx =
        lane_sequence.lane_segment(lane_seg_start_idx).lane_point_size() - 1;
  }
  double lane_heading = lane_sequence.lane_segment(lane_seg_start_idx)
                            .lane_point(lane_point_start_idx)
                            .heading();
 
  // Set the initial conditions for the diff. eqn.
  double s0 = 0.0;
  double ds0 = v * std::cos(theta - lane_heading);
  double dds0 = a * std::cos(theta - lane_heading);
  // double min_end_speed = std::min(FLAGS_still_obstacle_speed_threshold, ds0);
  // double ds1 = std::max(min_end_speed, ds0 + dds0 * time_to_end_state);
  double ds1 = lon_end_vt.first;
  double dds1 = 0.0;
  double p = lon_end_vt.second;  // time to lon end state
 
  // Solve for the coefficients.
  coefficients->operator[](0) = s0;
  coefficients->operator[](1) = ds0;
  coefficients->operator[](2) = 0.5 * dds0;
  double b0 = ds1 - dds0 * p - ds0;
  double b1 = dds1 - dds0;
  double p2 = p * p;
  double p3 = p2 * p;
  coefficients->operator[](3) = b0 / p2 - b1 / 3.0 / p;
  coefficients->operator[](4) = -0.5 / p3 * b0 + 0.25 / p2 * b1;
  return true;
}

◆ Predict()

bool apollo::prediction::SequencePredictor::Predict	(	const ADCTrajectoryContainer *	adc_trajectory_container,
		Obstacle *	obstacle,
		ObstaclesContainer *	obstacles_container
	)

overridevirtual

Make prediction

参数

ADC	trajectory container
Obstacle	pointer
Obstacles	container

返回: If predicted successfully

实现了 apollo::prediction::Predictor.

被 apollo::prediction::SingleLanePredictor 重载.

在文件 sequence_predictor.cc 第 35 行定义.

                                             {
  Clear();
 
  CHECK_NOTNULL(obstacle);
  CHECK_GT(obstacle->history_size(), 0U);
  return true;
}

◆ ToString()

std::string apollo::prediction::SequencePredictor::ToString ( const LaneSequence & sequence )

protected

Convert a lane sequence to string

参数

Lane sequence

返回: String describing the lane sequence

在文件 sequence_predictor.cc 第 47 行定义.

                                                                  {
  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;
}

该类的文档由以下文件生成:

modules/prediction/predictor/sequence/sequence_predictor.h
modules/prediction/predictor/sequence/sequence_predictor.cc

Public 类型

Public 成员函数

Protected 成员函数

额外继承的成员函数

详细描述

成员枚举类型说明

◆ LaneChangeType

构造及析构函数说明

◆ SequencePredictor()

◆ ~SequencePredictor()

成员函数说明

◆ Clear()

◆ DrawConstantAccelerationTrajectory()

◆ FilterLaneSequences()

◆ FRIEND_TEST()

◆ GetLaneChangeDistanceWithADC()

◆ GetLaneChangeType()

◆ GetLaneSequenceCurvatureByS()

◆ GetLateralPolynomial()

◆ GetLongitudinalPolynomial()

◆ Predict()

◆ ToString()