apollo::prediction::predictor_util 命名空间参考

函数
void	TranslatePoint (const double translate_x, const double translate_y, common::TrajectoryPoint *point)
	Translate a point.
void	GenerateFreeMoveTrajectoryPoints (Eigen::Matrix< double, 6, 1 > *state, const Eigen::Matrix< double, 6, 6 > &transition, double theta, const double start_time, const std::size_t num, const double period, std::vector< TrajectoryPoint > *points)
	Generate a set of free move trajectory points
double	AdjustSpeedByCurvature (const double speed, const double curvature)
	Adjust a speed value according to a curvature.
void	GenerateFreeMoveTrajectoryPoints (Eigen::Matrix< double, 6, 1 > *state, const Eigen::Matrix< double, 6, 6 > &transition, double theta, const double start_time, const std::size_t num, const double period, std::vector< common::TrajectoryPoint > *points)
	Generate a set of free move trajectory points

函数说明

AdjustSpeedByCurvature()

double apollo::prediction::predictor_util::AdjustSpeedByCurvature	(	const double	speed,
		const double	curvature
	)

Adjust a speed value according to a curvature.

If the input speed is okay on the input curvature, return the original speed, otherwise, adjust the speed.

参数

speed	The original speed value.
curvature	The curvature value.

返回

The adjusted speed according to the curvature.

在文件 prediction_util.cc 第 322 行定义.

                                                                          {
  if (std::abs(curvature) < FLAGS_turning_curvature_lower_bound) {
    return speed;
  }
  if (std::abs(curvature) > FLAGS_turning_curvature_upper_bound) {
    return FLAGS_speed_at_upper_curvature;
  }
  return apollo::common::math::lerp(
      FLAGS_speed_at_lower_curvature, FLAGS_turning_curvature_lower_bound,
      FLAGS_speed_at_upper_curvature, FLAGS_turning_curvature_upper_bound,
      curvature);
}

GenerateFreeMoveTrajectoryPoints() [1/2]

void apollo::prediction::predictor_util::GenerateFreeMoveTrajectoryPoints	(	Eigen::Matrix< double, 6, 1 > *	state,
		const Eigen::Matrix< double, 6, 6 > &	transition,
		double	theta,
		const double	start_time,
		const std::size_t	num,
		const double	period,
		std::vector< common::TrajectoryPoint > *	points
	)

Generate a set of free move trajectory points

参数

state	matrix
transition	matrix
heading
start	time
total	number of generated trajectory points required
trajectory	point interval period
generated	trajectory points

在文件 prediction_util.cc 第 240 行定义.

                                        {
  double x = (*state)(0, 0);
  double y = (*state)(1, 0);
  double v_x = (*state)(2, 0);
  double v_y = (*state)(3, 0);
  double acc_x = (*state)(4, 0);
  double acc_y = (*state)(5, 0);
 
  for (std::size_t i = 0; i < num; ++i) {
    double speed = std::hypot(v_x, v_y);
    double acc = 0.0;
    if (speed <= std::numeric_limits<double>::epsilon()) {
      speed = 0.0;
      v_x = 0.0;
      v_y = 0.0;
      acc_x = 0.0;
      acc_y = 0.0;
      acc = 0.0;
    } else {
      speed = std::fmin(speed, FLAGS_vehicle_max_speed);
    }
 
    // update theta and acc
    if (speed > std::numeric_limits<double>::epsilon()) {
      if (points->size() > 0) {
        TrajectoryPoint& prev_trajectory_point = points->back();
        PathPoint* prev_path_point = prev_trajectory_point.mutable_path_point();
        theta = std::atan2(y - prev_path_point->y(), x - prev_path_point->x());
        prev_path_point->set_theta(theta);
        acc = (speed - prev_trajectory_point.v()) / period;
        prev_trajectory_point.set_a(acc);
      }
    } else {
      if (points->size() > 0) {
        theta = points->back().path_point().theta();
      }
    }
 
    // update state
    if (!FLAGS_free_move_predict_with_accelerate) {
      acc_x = 0.0;
      acc_y = 0.0;
    }
 
    (*state)(2, 0) = v_x;
    (*state)(3, 0) = v_y;
    (*state)(4, 0) = acc_x;
    (*state)(5, 0) = acc_y;
 
    // obtain position
    x = (*state)(0, 0);
    y = (*state)(1, 0);
 
    // Generate trajectory point
    TrajectoryPoint trajectory_point;
    PathPoint path_point;
    path_point.set_x(x);
    path_point.set_y(y);
    path_point.set_z(0.0);
    path_point.set_theta(theta);
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed);
    trajectory_point.set_a(acc);
    trajectory_point.set_relative_time(start_time +
                                       static_cast<double>(i) * period);
    points->emplace_back(std::move(trajectory_point));
 
    // Update position, velocity and acceleration
    (*state) = transition * (*state);
    x = (*state)(0, 0);
    y = (*state)(1, 0);
    v_x = (*state)(2, 0);
    v_y = (*state)(3, 0);
    acc_x = (*state)(4, 0);
    acc_y = (*state)(5, 0);
  }
}

GenerateFreeMoveTrajectoryPoints() [2/2]

void apollo::prediction::predictor_util::GenerateFreeMoveTrajectoryPoints	(	Eigen::Matrix< double, 6, 1 > *	state,
		const Eigen::Matrix< double, 6, 6 > &	transition,
		double	theta,
		const double	start_time,
		const std::size_t	num,
		const double	period,
		std::vector< common::TrajectoryPoint > *	points
	)

Generate a set of free move trajectory points

参数

state	matrix
transition	matrix
heading
start	time
total	number of generated trajectory points required
trajectory	point interval period
generated	trajectory points

在文件 prediction_util.cc 第 240 行定义.

                                        {
  double x = (*state)(0, 0);
  double y = (*state)(1, 0);
  double v_x = (*state)(2, 0);
  double v_y = (*state)(3, 0);
  double acc_x = (*state)(4, 0);
  double acc_y = (*state)(5, 0);
 
  for (std::size_t i = 0; i < num; ++i) {
    double speed = std::hypot(v_x, v_y);
    double acc = 0.0;
    if (speed <= std::numeric_limits<double>::epsilon()) {
      speed = 0.0;
      v_x = 0.0;
      v_y = 0.0;
      acc_x = 0.0;
      acc_y = 0.0;
      acc = 0.0;
    } else {
      speed = std::fmin(speed, FLAGS_vehicle_max_speed);
    }
 
    // update theta and acc
    if (speed > std::numeric_limits<double>::epsilon()) {
      if (points->size() > 0) {
        TrajectoryPoint& prev_trajectory_point = points->back();
        PathPoint* prev_path_point = prev_trajectory_point.mutable_path_point();
        theta = std::atan2(y - prev_path_point->y(), x - prev_path_point->x());
        prev_path_point->set_theta(theta);
        acc = (speed - prev_trajectory_point.v()) / period;
        prev_trajectory_point.set_a(acc);
      }
    } else {
      if (points->size() > 0) {
        theta = points->back().path_point().theta();
      }
    }
 
    // update state
    if (!FLAGS_free_move_predict_with_accelerate) {
      acc_x = 0.0;
      acc_y = 0.0;
    }
 
    (*state)(2, 0) = v_x;
    (*state)(3, 0) = v_y;
    (*state)(4, 0) = acc_x;
    (*state)(5, 0) = acc_y;
 
    // obtain position
    x = (*state)(0, 0);
    y = (*state)(1, 0);
 
    // Generate trajectory point
    TrajectoryPoint trajectory_point;
    PathPoint path_point;
    path_point.set_x(x);
    path_point.set_y(y);
    path_point.set_z(0.0);
    path_point.set_theta(theta);
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed);
    trajectory_point.set_a(acc);
    trajectory_point.set_relative_time(start_time +
                                       static_cast<double>(i) * period);
    points->emplace_back(std::move(trajectory_point));
 
    // Update position, velocity and acceleration
    (*state) = transition * (*state);
    x = (*state)(0, 0);
    y = (*state)(1, 0);
    v_x = (*state)(2, 0);
    v_y = (*state)(3, 0);
    acc_x = (*state)(4, 0);
    acc_y = (*state)(5, 0);
  }
}

TranslatePoint()

void apollo::prediction::predictor_util::TranslatePoint	(	const double	translate_x,
		const double	translate_y,
		common::TrajectoryPoint *	point
	)

Translate a point.

参数

translate_x	The translation along x-axis.
translate_y	The translation along y-axis.
point	The point to be translated.

在文件 prediction_util.cc 第 228 行定义.

                                            {
  if (point == nullptr || !point->has_path_point()) {
    AERROR << "Point is nullptr or has NO path_point.";
    return;
  }
  const double original_x = point->path_point().x();
  const double original_y = point->path_point().y();
  point->mutable_path_point()->set_x(original_x + translate_x);
  point->mutable_path_point()->set_y(original_y + translate_y);
}