apollo::planning::SpiralReferenceLineSmoother类 参考

#include <spiral_reference_line_smoother.h>

类 apollo::planning::SpiralReferenceLineSmoother 继承关系图:

apollo::planning::SpiralReferenceLineSmoother 的协作图:

Public 成员函数
	SpiralReferenceLineSmoother (const ReferenceLineSmootherConfig &config)
virtual	~SpiralReferenceLineSmoother ()=default
bool	Smooth (const ReferenceLine &raw_reference_line, ReferenceLine *const smoothed_reference_line) override
	Smooth a given reference line
int	SmoothStandAlone (std::vector< Eigen::Vector2d > point2d, std::vector< double > *ptr_theta, std::vector< double > *ptr_kappa, std::vector< double > *ptr_dkappa, std::vector< double > *ptr_s, std::vector< double > *ptr_x, std::vector< double > *ptr_y) const
void	SetAnchorPoints (const std::vector< AnchorPoint > &) override
	Smoothing constraints
std::vector< common::PathPoint >	Interpolate (const std::vector< double > &theta, const std::vector< double > &kappa, const std::vector< double > &dkappa, const std::vector< double > &s, const std::vector< double > &x, const std::vector< double > &y, const double resolution) const
Public 成员函数 继承自 apollo::planning::ReferenceLineSmoother
	ReferenceLineSmoother (const ReferenceLineSmootherConfig &config)
virtual	~ReferenceLineSmoother ()=default

额外继承的成员函数
Protected 属性 继承自 apollo::planning::ReferenceLineSmoother
ReferenceLineSmootherConfig	config_

详细描述

在文件 spiral_reference_line_smoother.h 第 36 行定义.

构造及析构函数说明

SpiralReferenceLineSmoother()

apollo::planning::SpiralReferenceLineSmoother::SpiralReferenceLineSmoother ( const ReferenceLineSmootherConfig &  config )

explicit

在文件 spiral_reference_line_smoother.cc 第 36 行定义.

    : ReferenceLineSmoother(config) {}

~SpiralReferenceLineSmoother()

virtual apollo::planning::SpiralReferenceLineSmoother::~SpiralReferenceLineSmoother ( )

virtualdefault

成员函数说明

Interpolate()

std::vector< common::PathPoint > apollo::planning::SpiralReferenceLineSmoother::Interpolate	(	const std::vector< double > &	theta,
		const std::vector< double > &	kappa,
		const std::vector< double > &	dkappa,
		const std::vector< double > &	s,
		const std::vector< double > &	x,
		const std::vector< double > &	y,
		const double	resolution
	)		const

在文件 spiral_reference_line_smoother.cc 第 300 行定义.

                                   {
  std::vector<common::PathPoint> smoothed_point2d;
  double start_s = 0.0;
  common::PathPoint first_point =
      to_path_point(x.front(), y.front(), start_s, theta.front(), kappa.front(),
                    dkappa.front());
  smoothed_point2d.push_back(first_point);
 
  for (size_t i = 0; i + 1 < theta.size(); ++i) {
    double start_x = x[i];
    double start_y = y[i];
 
    auto path_point_seg = Interpolate(
        start_x, start_y, start_s, theta[i], kappa[i], dkappa[i], theta[i + 1],
        kappa[i + 1], dkappa[i + 1], s[i], resolution);
 
    smoothed_point2d.insert(smoothed_point2d.end(), path_point_seg.begin(),
                            path_point_seg.end());
 
    start_s = smoothed_point2d.back().s();
  }
  return smoothed_point2d;
}

SetAnchorPoints()

void apollo::planning::SpiralReferenceLineSmoother::SetAnchorPoints ( const std::vector< AnchorPoint > &  achor_points )

overridevirtual

Smoothing constraints

实现了 apollo::planning::ReferenceLineSmoother.

在文件 spiral_reference_line_smoother.cc 第 372 行定义.

                                                 {
  anchor_points_ = std::move(anchor_points);
 
  CHECK_GT(anchor_points_.size(), 1U);
  zero_x_ = anchor_points_.front().path_point.x();
  zero_y_ = anchor_points_.front().path_point.y();
 
  std::for_each(anchor_points_.begin(), anchor_points_.end(),
                [this](AnchorPoint& p) {
                  auto curr_x = p.path_point.x();
                  auto curr_y = p.path_point.y();
                  p.path_point.set_x(curr_x - zero_x_);
                  p.path_point.set_y(curr_y - zero_y_);
                });
}

Smooth()

bool apollo::planning::SpiralReferenceLineSmoother::Smooth ( const ReferenceLine &  , ReferenceLine *const    )

overridevirtual

Smooth a given reference line

实现了 apollo::planning::ReferenceLineSmoother.

在文件 spiral_reference_line_smoother.cc 第 40 行定义.

                                                  {
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  std::vector<double> opt_theta;
  std::vector<double> opt_kappa;
  std::vector<double> opt_dkappa;
  std::vector<double> opt_s;
 
  if (anchor_points_.empty()) {
    const double piecewise_length = config_.spiral().piecewise_length();
    const double length = raw_reference_line.Length();
    ADEBUG << "Length = " << length;
    uint32_t num_of_pieces =
        std::max(1u, static_cast<uint32_t>(length / piecewise_length));
 
    const double delta_s = length / num_of_pieces;
    double s = 0.0;
 
    std::vector<Eigen::Vector2d> raw_point2d;
    for (std::uint32_t i = 0; i <= num_of_pieces;
         ++i, s = std::fmin(s + delta_s, length)) {
      ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
      raw_point2d.emplace_back(rlp.x(), rlp.y());
    }
 
    Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
           &opt_y);
  } else {
    size_t start_index = 0;
    for (const auto& anchor_point : anchor_points_) {
      if (anchor_point.enforced) {
        start_index++;
      } else {
        break;
      }
    }
 
    std::vector<Eigen::Vector2d> raw_point2d;
    if (start_index == 0) {
      for (const auto& anchor_point : anchor_points_) {
        raw_point2d.emplace_back(anchor_point.path_point.x(),
                                 anchor_point.path_point.y());
      }
    } else {
      std::vector<double> overhead_s;
      for (size_t i = 0; i + 1 < start_index; ++i) {
        const auto& p0 = anchor_points_[i];
        const auto& p1 = anchor_points_[i + 1];
        overhead_s.push_back(p1.path_point.s() - p0.path_point.s());
      }
 
      std::vector<double> overhead_theta;
      std::vector<double> overhead_kappa;
      std::vector<double> overhead_dkappa;
      std::vector<double> overhead_x;
      std::vector<double> overhead_y;
      for (size_t i = 0; i < anchor_points_.size(); ++i) {
        const auto& p = anchor_points_[i];
        if (i + 1 < start_index) {
          overhead_theta.push_back(p.path_point.theta());
          overhead_kappa.push_back(p.path_point.kappa());
          overhead_dkappa.push_back(p.path_point.dkappa());
          overhead_x.push_back(p.path_point.x());
          overhead_y.push_back(p.path_point.y());
        } else {
          raw_point2d.emplace_back(p.path_point.x(), p.path_point.y());
        }
      }
 
      const auto& start_anchor_point = anchor_points_[start_index - 1];
      fixed_start_point_ = true;
      fixed_start_x_ = start_anchor_point.path_point.x();
      fixed_start_y_ = start_anchor_point.path_point.y();
      fixed_start_theta_ =
          common::math::NormalizeAngle(start_anchor_point.path_point.theta());
      fixed_start_kappa_ = start_anchor_point.path_point.kappa();
      fixed_start_dkappa_ = start_anchor_point.path_point.dkappa();
 
      const auto& end_anchor_point = anchor_points_.back();
      fixed_end_x_ = end_anchor_point.path_point.x();
      fixed_end_y_ = end_anchor_point.path_point.y();
 
      Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
             &opt_y);
 
      opt_theta.insert(opt_theta.begin(), overhead_theta.begin(),
                       overhead_theta.end());
      opt_kappa.insert(opt_kappa.begin(), overhead_kappa.begin(),
                       overhead_kappa.end());
      opt_dkappa.insert(opt_dkappa.begin(), overhead_dkappa.begin(),
                        overhead_dkappa.end());
      opt_s.insert(opt_s.begin(), overhead_s.begin(), overhead_s.end());
      opt_x.insert(opt_x.begin(), overhead_x.begin(), overhead_x.end());
      opt_y.insert(opt_y.begin(), overhead_y.begin(), overhead_y.end());
 
      std::for_each(opt_x.begin(), opt_x.end(),
                    [this](double& x) { x += zero_x_; });
 
      std::for_each(opt_y.begin(), opt_y.end(),
                    [this](double& y) { y += zero_y_; });
    }
  }
 
  std::vector<common::PathPoint> smoothed_point2d =
      Interpolate(opt_theta, opt_kappa, opt_dkappa, opt_s, opt_x, opt_y,
                  config_.resolution());
 
  std::vector<ReferencePoint> ref_points;
  for (const auto& p : smoothed_point2d) {
    const double heading = p.theta();
    const double kappa = p.kappa();
    const double dkappa = p.dkappa();
 
    common::SLPoint ref_sl_point;
    if (!raw_reference_line.XYToSL(p, &ref_sl_point)) {
      return false;
    }
    if (ref_sl_point.s() < 0 ||
        ref_sl_point.s() > raw_reference_line.Length()) {
      continue;
    }
 
    ReferencePoint rlp = raw_reference_line.GetReferencePoint(ref_sl_point.s());
    ref_points.emplace_back(
        ReferencePoint(hdmap::MapPathPoint(common::math::Vec2d(p.x(), p.y()),
                                           heading, rlp.lane_waypoints()),
                       kappa, dkappa));
  }
 
  ReferencePoint::RemoveDuplicates(&ref_points);
  if (ref_points.size() < 2) {
    AERROR << "Fail to generate smoothed reference line.";
    return false;
  }
  *smoothed_reference_line = ReferenceLine(ref_points);
  return true;
}

SmoothStandAlone()

int apollo::planning::SpiralReferenceLineSmoother::SmoothStandAlone	(	std::vector< Eigen::Vector2d >	point2d,
		std::vector< double > *	ptr_theta,
		std::vector< double > *	ptr_kappa,
		std::vector< double > *	ptr_dkappa,
		std::vector< double > *	ptr_s,
		std::vector< double > *	ptr_x,
		std::vector< double > *	ptr_y
	)		const

在文件 spiral_reference_line_smoother.cc 第 180 行定义.

                                    {
  CHECK_GT(point2d.size(), 1U);
 
  SpiralProblemInterface* ptop = new SpiralProblemInterface(point2d);
 
  ptop->set_default_max_point_deviation(config_.spiral().max_deviation());
  ptop->set_element_weight_curve_length(config_.spiral().weight_curve_length());
  ptop->set_element_weight_kappa(config_.spiral().weight_kappa());
  ptop->set_element_weight_dkappa(config_.spiral().weight_dkappa());
 
  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
 
  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
 
  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  app->Options()->SetIntegerValue("max_iter", config_.spiral().max_iteration());
  app->Options()->SetNumericValue("tol", config_.spiral().opt_tol());
  app->Options()->SetNumericValue("acceptable_tol",
                                  config_.spiral().opt_acceptable_tol());
 
  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    ADEBUG << "*** Error during initialization!";
    return -1;
  }
 
  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!";
 
    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    ADEBUG << "*** The final value of the objective function is " << final_obj
           << '.';
  } else {
    ADEBUG << "Return status: " << int(status);
  }
 
  ptop->get_optimization_results(ptr_theta, ptr_kappa, ptr_dkappa, ptr_s, ptr_x,
                                 ptr_y);
 
  if (!(status == Ipopt::Solve_Succeeded) &&
      !(status == Ipopt::Solved_To_Acceptable_Level)) {
    return -1;
  }
  return app->Statistics()->IterationCount();
}

---

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

• modules/planning/planning_base/reference_line/spiral_reference_line_smoother.h
• modules/planning/planning_base/reference_line/spiral_reference_line_smoother.cc