apollo::planning::FemPosDeviationSmoother类 参考

#include <fem_pos_deviation_smoother.h>

apollo::planning::FemPosDeviationSmoother 的协作图:

Public 成员函数
	FemPosDeviationSmoother (const FemPosDeviationSmootherConfig &config)
bool	QpWithOsqp (const std::vector< std::pair< double, double > > &raw_point2d, const std::vector< double > &bounds, std::vector< double > *opt_x, std::vector< double > *opt_y)
bool	NlpWithIpopt (const std::vector< std::pair< double, double > > &raw_point2d, const std::vector< double > &bounds, std::vector< double > *opt_x, std::vector< double > *opt_y)
bool	SqpWithOsqp (const std::vector< std::pair< double, double > > &raw_point2d, const std::vector< double > &bounds, std::vector< double > *opt_x, std::vector< double > *opt_y)
bool	Solve (const std::vector< std::pair< double, double > > &raw_point2d, const std::vector< double > &bounds, std::vector< double > *opt_x, std::vector< double > *opt_y, std::vector< std::vector< common::math::Vec2d > > point_box={})
bool	SqpWithOsqp (const std::vector< std::pair< double, double > > &raw_point2d, const std::vector< double > &bounds, std::vector< double > *opt_x, std::vector< double > *opt_y, std::vector< std::vector< common::math::Vec2d > > point_box)

详细描述

在文件 fem_pos_deviation_smoother.h 第 48 行定义.

构造及析构函数说明

FemPosDeviationSmoother()

apollo::planning::FemPosDeviationSmoother::FemPosDeviationSmoother ( const FemPosDeviationSmootherConfig &  config )

explicit

在文件 fem_pos_deviation_smoother.cc 第 33 行定义.

    : config_(config) {}

成员函数说明

NlpWithIpopt()

bool apollo::planning::FemPosDeviationSmoother::NlpWithIpopt	(	const std::vector< std::pair< double, double > > &	raw_point2d,
		const std::vector< double > &	bounds,
		std::vector< double > *	opt_x,
		std::vector< double > *	opt_y
	)

在文件 fem_pos_deviation_smoother.cc 第 138 行定义.

                              {
  if (opt_x == nullptr || opt_y == nullptr) {
    AERROR << "opt_x or opt_y is nullptr";
    return false;
  }
 
  FemPosDeviationIpoptInterface* smoother =
      new FemPosDeviationIpoptInterface(raw_point2d, bounds);
 
  smoother->set_weight_fem_pos_deviation(config_.weight_fem_pos_deviation());
  smoother->set_weight_path_length(config_.weight_path_length());
  smoother->set_weight_ref_deviation(config_.weight_ref_deviation());
  smoother->set_weight_curvature_constraint_slack_var(
      config_.weight_curvature_constraint_slack_var());
  smoother->set_curvature_constraint(config_.curvature_constraint());
 
  Ipopt::SmartPtr<Ipopt::TNLP> problem = smoother;
 
  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
 
  app->Options()->SetIntegerValue("print_level",
                                  static_cast<int>(config_.print_level()));
  app->Options()->SetIntegerValue(
      "max_iter", static_cast<int>(config_.max_num_of_iterations()));
  app->Options()->SetIntegerValue(
      "acceptable_iter",
      static_cast<int>(config_.acceptable_num_of_iterations()));
  app->Options()->SetNumericValue("tol", config_.tol());
  app->Options()->SetNumericValue("acceptable_tol", config_.acceptable_tol());
 
  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    AERROR << "*** Error during initialization!";
    return false;
  }
 
  status = app->OptimizeTNLP(problem);
 
  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
  } else {
    AERROR << "Solver fails with return code: " << static_cast<int>(status);
    return false;
  }
  smoother->get_optimization_results(opt_x, opt_y);
  return true;
}

QpWithOsqp()

bool apollo::planning::FemPosDeviationSmoother::QpWithOsqp	(	const std::vector< std::pair< double, double > > &	raw_point2d,
		const std::vector< double > &	bounds,
		std::vector< double > *	opt_x,
		std::vector< double > *	opt_y
	)

在文件 fem_pos_deviation_smoother.cc 第 54 行定义.

                              {
  if (opt_x == nullptr || opt_y == nullptr) {
    AERROR << "opt_x or opt_y is nullptr";
    return false;
  }
 
  FemPosDeviationOsqpInterface solver;
 
  solver.set_weight_fem_pos_deviation(config_.weight_fem_pos_deviation());
  solver.set_weight_path_length(config_.weight_path_length());
  solver.set_weight_ref_deviation(config_.weight_ref_deviation());
 
  solver.set_max_iter(config_.max_iter());
  solver.set_time_limit(config_.time_limit());
  solver.set_verbose(config_.verbose());
  solver.set_scaled_termination(config_.scaled_termination());
  solver.set_warm_start(config_.warm_start());
 
  solver.set_ref_points(raw_point2d);
  solver.set_bounds_around_refs(bounds);
 
  if (!solver.Solve()) {
    return false;
  }
 
  *opt_x = solver.opt_x();
  *opt_y = solver.opt_y();
  return true;
}

Solve()

bool apollo::planning::FemPosDeviationSmoother::Solve	(	const std::vector< std::pair< double, double > > &	raw_point2d,
		const std::vector< double > &	bounds,
		std::vector< double > *	opt_x,
		std::vector< double > *	opt_y,
		std::vector< std::vector< common::math::Vec2d > >	point_box = {}
	)

在文件 fem_pos_deviation_smoother.cc 第 37 行定义.

                                                     {
  if (config_.apply_curvature_constraint()) {
    if (config_.use_sqp()) {
      return SqpWithOsqp(raw_point2d, bounds, opt_x, opt_y, point_box);
    } else {
      return NlpWithIpopt(raw_point2d, bounds, opt_x, opt_y);
    }
  } else {
    return QpWithOsqp(raw_point2d, bounds, opt_x, opt_y);
  }
  return true;
}

SqpWithOsqp() [1/2]

bool apollo::planning::FemPosDeviationSmoother::SqpWithOsqp	(	const std::vector< std::pair< double, double > > &	raw_point2d,
		const std::vector< double > &	bounds,
		std::vector< double > *	opt_x,
		std::vector< double > *	opt_y
	)

SqpWithOsqp() [2/2]

bool apollo::planning::FemPosDeviationSmoother::SqpWithOsqp	(	const std::vector< std::pair< double, double > > &	raw_point2d,
		const std::vector< double > &	bounds,
		std::vector< double > *	opt_x,
		std::vector< double > *	opt_y,
		std::vector< std::vector< common::math::Vec2d > >	point_box
	)

在文件 fem_pos_deviation_smoother.cc 第 87 行定义.

                                                     {
  if (opt_x == nullptr || opt_y == nullptr) {
    AERROR << "opt_x or opt_y is nullptr";
    return false;
  }
 
  FemPosDeviationSqpOsqpInterface solver;
 
  solver.set_weight_fem_pos_deviation(config_.weight_fem_pos_deviation());
  solver.set_weight_path_length(config_.weight_path_length());
  solver.set_weight_ref_deviation(config_.weight_ref_deviation());
  solver.set_weight_curvature_constraint_slack_var(
      config_.weight_curvature_constraint_slack_var());
  solver.set_point_box(point_box);
 
  solver.set_curvature_constraint(config_.curvature_constraint());
 
  solver.set_sqp_sub_max_iter(config_.sqp_sub_max_iter());
  solver.set_sqp_ftol(config_.sqp_ftol());
  solver.set_sqp_pen_max_iter(config_.sqp_pen_max_iter());
  solver.set_sqp_ctol(config_.sqp_ctol());
 
  solver.set_max_iter(config_.max_iter());
  solver.set_time_limit(config_.time_limit());
  solver.set_verbose(config_.verbose());
  solver.set_scaled_termination(config_.scaled_termination());
  solver.set_warm_start(config_.warm_start());
 
  solver.set_ref_points(raw_point2d);
  solver.set_bounds_around_refs(bounds);
 
  if (!solver.Solve()) {
    return false;
  }
 
  std::vector<std::pair<double, double>> opt_xy = solver.opt_xy();
 
  // TODO(Jinyun): unify output data container
  opt_x->resize(opt_xy.size());
  opt_y->resize(opt_xy.size());
  for (size_t i = 0; i < opt_xy.size(); ++i) {
    (*opt_x)[i] = opt_xy[i].first;
    (*opt_y)[i] = opt_xy[i].second;
  }
  return true;
}

---

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

• modules/planning/planning_base/math/discretized_points_smoothing/fem_pos_deviation_smoother.h
• modules/planning/planning_base/math/discretized_points_smoothing/fem_pos_deviation_smoother.cc