#include <spline_1d_constraint.h>

apollo::planning::Spline1dConstraint 的协作图:

Public 成员函数
	Spline1dConstraint (const Spline1d &pss)

	Spline1dConstraint (const std::vector< double > &x_knots, const uint32_t order)

bool	AddInequalityConstraint (const Eigen::MatrixXd &constraint_matrix, const Eigen::MatrixXd &constraint_boundary)

bool	AddEqualityConstraint (const Eigen::MatrixXd &constraint_matrix, const Eigen::MatrixXd &constraint_boundary)

bool	AddBoundary (const std::vector< double > &x_coord, const std::vector< double > &lower_bound, const std::vector< double > &upper_bound)
	: inequality boundary constraints if no boundary, do specify either by std::infinity or let vector.size() = 0

bool	AddDerivativeBoundary (const std::vector< double > &x_coord, const std::vector< double > &lower_bound, const std::vector< double > &upper_bound)

bool	AddSecondDerivativeBoundary (const std::vector< double > &x_coord, const std::vector< double > &lower_bound, const std::vector< double > &upper_bound)

bool	AddThirdDerivativeBoundary (const std::vector< double > &x_coord, const std::vector< double > &lower_bound, const std::vector< double > &upper_bound)

bool	AddPointConstraint (const double x, const double fx)
	: equality constraint to guarantee joint smoothness boundary equality constriant constraint on fx, dfx, ddfx ... in vector form; up to third order

bool	AddPointDerivativeConstraint (const double x, const double dfx)

bool	AddPointSecondDerivativeConstraint (const double x, const double ddfx)

bool	AddPointThirdDerivativeConstraint (const double x, const double dddfx)

bool	AddPointConstraintInRange (const double x, const double fx, const double range)

bool	AddPointDerivativeConstraintInRange (const double x, const double dfx, const double range)

bool	AddPointSecondDerivativeConstraintInRange (const double x, const double ddfx, const double range)

bool	AddPointThirdDerivativeConstraintInRange (const double x, const double dddfx, const double range)

bool	AddSmoothConstraint ()

bool	AddDerivativeSmoothConstraint ()

bool	AddSecondDerivativeSmoothConstraint ()

bool	AddThirdDerivativeSmoothConstraint ()

bool	AddMonotoneInequalityConstraint (const std::vector< double > &x_coord)
	: Add monotone constraint inequality, guarantee the monotone city at evaluated point.

bool	AddMonotoneInequalityConstraintAtKnots ()

const AffineConstraint &	inequality_constraint () const
	: output interface inequality constraint

const AffineConstraint &	equality_constraint () const

详细描述

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

构造及析构函数说明

◆ Spline1dConstraint() [1/2]

apollo::planning::Spline1dConstraint::Spline1dConstraint ( const Spline1d & pss )

explicit

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

37 : Spline1dConstraint(pss.x_knots(), pss.spline_order()) {}

apollo::planning::Spline1dConstraint::Spline1dConstraint

Spline1dConstraint(const Spline1d &pss)

Definition spline_1d_constraint.cc:36

◆ Spline1dConstraint() [2/2]

apollo::planning::Spline1dConstraint::Spline1dConstraint	(	const std::vector< double > &	x_knots,
		const uint32_t	order
	)

在文件 spline_1d_constraint.cc 第 39 行定义.

    : x_knots_(x_knots), spline_order_(spline_order) {
  inequality_constraint_.SetIsEquality(false);
  equality_constraint_.SetIsEquality(true);
}

成员函数说明

◆ AddBoundary()

bool apollo::planning::Spline1dConstraint::AddBoundary	(	const std::vector< double > &	x_coord,
		const std::vector< double > &	lower_bound,
		const std::vector< double > &	upper_bound
	)

: inequality boundary constraints if no boundary, do specify either by std::infinity or let vector.size() = 0

在文件 spline_1d_constraint.cc 第 60 行定义.

                                                                           {
  std::vector<double> filtered_lower_bound;
  std::vector<double> filtered_upper_bound;
  std::vector<double> filtered_lower_bound_x;
  std::vector<double> filtered_upper_bound_x;
 
  if (x_knots_.size() < 2) {
    return false;
  }
 
  if (!FilterConstraints(x_coord, lower_bound, upper_bound,
                         &filtered_lower_bound_x, &filtered_lower_bound,
                         &filtered_upper_bound_x, &filtered_upper_bound)) {
    return false;
  }
  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),
      (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);
 
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
 
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef;
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }
 
  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef;
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) =
        -filtered_upper_bound[i];
  }
 
  return inequality_constraint_.AddConstraint(inequality_constraint,
                                              inequality_boundary);
}

◆ AddDerivativeBoundary()

bool apollo::planning::Spline1dConstraint::AddDerivativeBoundary	(	const std::vector< double > &	x_coord,
		const std::vector< double > &	lower_bound,
		const std::vector< double > &	upper_bound
	)

在文件 spline_1d_constraint.cc 第 114 行定义.

                                          {
  std::vector<double> filtered_lower_bound;
  std::vector<double> filtered_upper_bound;
  std::vector<double> filtered_lower_bound_x;
  std::vector<double> filtered_upper_bound_x;
 
  if (x_knots_.size() < 2) {
    return false;
  }
 
  if (!FilterConstraints(x_coord, lower_bound, upper_bound,
                         &filtered_lower_bound_x, &filtered_lower_bound,
                         &filtered_upper_bound_x, &filtered_upper_bound)) {
    return false;
  }
 
  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),
      (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);
 
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    for (uint32_t j = 1; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef * j;
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }
 
  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    for (uint32_t j = 1; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef * j;
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) =
        -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,
                                              inequality_boundary);
}

◆ AddDerivativeSmoothConstraint()

bool apollo::planning::Spline1dConstraint::AddDerivativeSmoothConstraint ( )

在文件 spline_1d_constraint.cc 第 418 行定义.

                                                       {
  if (x_knots_.size() < 3) {
    return false;
  }
 
  const uint32_t n_constraint =
      (static_cast<uint32_t>(x_knots_.size()) - 2) * 2;
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(n_constraint, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd equality_boundary = Eigen::MatrixXd::Zero(n_constraint, 1);
 
  for (uint32_t i = 0; i < n_constraint; i += 2) {
    double left_coef = 1.0;
    double right_coef = -1.0;
    double left_dcoef = 1.0;
    double right_dcoef = -1.0;
    const double x_left = x_knots_[i / 2 + 1] - x_knots_[i / 2];
    const double x_right = 0.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      equality_constraint(i, num_params * (i / 2) + j) = left_coef;
      equality_constraint(i, num_params * ((i / 2) + 1) + j) = right_coef;
      if (j >= 1) {
        equality_constraint(i + 1, num_params * (i / 2) + j) = left_dcoef * j;
        equality_constraint(i + 1, num_params * ((i / 2) + 1) + j) =
            right_dcoef * j;
        left_dcoef = left_coef;
        right_dcoef = right_coef;
      }
      left_coef *= x_left;
      right_coef *= x_right;
    }
  }
  return equality_constraint_.AddConstraint(equality_constraint,
                                            equality_boundary);
}

◆ AddEqualityConstraint()

bool apollo::planning::Spline1dConstraint::AddEqualityConstraint	(	const Eigen::MatrixXd &	constraint_matrix,
		const Eigen::MatrixXd &	constraint_boundary
	)

在文件 spline_1d_constraint.cc 第 53 行定义.

                                              {
  return equality_constraint_.AddConstraint(constraint_matrix,
                                            constraint_boundary);
}

◆ AddInequalityConstraint()

bool apollo::planning::Spline1dConstraint::AddInequalityConstraint	(	const Eigen::MatrixXd &	constraint_matrix,
		const Eigen::MatrixXd &	constraint_boundary
	)

在文件 spline_1d_constraint.cc 第 46 行定义.

                                              {
  return inequality_constraint_.AddConstraint(constraint_matrix,
                                              constraint_boundary);
}

◆ AddMonotoneInequalityConstraint()

bool apollo::planning::Spline1dConstraint::AddMonotoneInequalityConstraint ( const std::vector< double > & x_coord )

: Add monotone constraint inequality, guarantee the monotone city at evaluated point.

customized monotone inequality constraint at x_coord

在文件 spline_1d_constraint.cc 第 567 行定义.

                                      {
  if (x_coord.size() < 2) {
    // Skip because NO inequality constraint is needed.
    return false;
  }
 
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      x_coord.size() - 1, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary =
      Eigen::MatrixXd::Zero(x_coord.size() - 1, 1);
 
  uint32_t prev_spline_index = FindIndex(x_coord[0]);
  double prev_rel_x = x_coord[0] - x_knots_[prev_spline_index];
  std::vector<double> prev_coef;
  GeneratePowerX(prev_rel_x, num_params, &prev_coef);
  for (uint32_t i = 1; i < x_coord.size(); ++i) {
    uint32_t cur_spline_index = FindIndex(x_coord[i]);
    double cur_rel_x = x_coord[i] - x_knots_[cur_spline_index];
    std::vector<double> cur_coef;
 
    GeneratePowerX(cur_rel_x, num_params, &cur_coef);
    // if constraint on the same spline
    if (cur_spline_index == prev_spline_index) {
      for (uint32_t j = 0; j < cur_coef.size(); ++j) {
        inequality_constraint(i - 1, cur_spline_index * num_params + j) =
            cur_coef[j] - prev_coef[j];
      }
    } else {
      // if not on the same spline
      for (uint32_t j = 0; j < cur_coef.size(); ++j) {
        inequality_constraint(i - 1, prev_spline_index * num_params + j) =
            -prev_coef[j];
        inequality_constraint(i - 1, cur_spline_index * num_params + j) =
            cur_coef[j];
      }
    }
    prev_coef = cur_coef;
    prev_spline_index = cur_spline_index;
  }
 
  return inequality_constraint_.AddConstraint(inequality_constraint,
                                              inequality_boundary);
}

◆ AddMonotoneInequalityConstraintAtKnots()

bool apollo::planning::Spline1dConstraint::AddMonotoneInequalityConstraintAtKnots ( )

在文件 spline_1d_constraint.cc 第 613 行定义.

                                                                {
  return AddMonotoneInequalityConstraint(x_knots_);
}

◆ AddPointConstraint()

bool apollo::planning::Spline1dConstraint::AddPointConstraint	(	const double	x,
		const double	fx
	)

: equality constraint to guarantee joint smoothness boundary equality constriant constraint on fx, dfx, ddfx ... in vector form; up to third order

在文件 spline_1d_constraint.cc 第 277 行定义.

                                                                           {
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  const uint32_t num_params = spline_order_ + 1;
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 0; i < num_params; ++i) {
    equality_constraint(0, index_offset + i) = power_x[i];
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = fx;
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}

◆ AddPointConstraintInRange()

bool apollo::planning::Spline1dConstraint::AddPointConstraintInRange	(	const double	x,
		const double	fx,
		const double	range
	)

在文件 spline_1d_constraint.cc 第 309 行定义.

                                                                       {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddBoundary, this, _1, _2, _3), x, fx,
      range);
}

◆ AddPointDerivativeConstraint()

bool apollo::planning::Spline1dConstraint::AddPointDerivativeConstraint	(	const double	x,
		const double	dfx
	)

在文件 spline_1d_constraint.cc 第 317 行定义.

                                                                        {
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  const uint32_t num_params = spline_order_ + 1;
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 1; i < num_params; ++i) {
    equality_constraint(0, index_offset + i) = power_x[i - 1] * i;
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = dfx;
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}

◆ AddPointDerivativeConstraintInRange()

bool apollo::planning::Spline1dConstraint::AddPointDerivativeConstraintInRange	(	const double	x,
		const double	dfx,
		const double	range
	)

在文件 spline_1d_constraint.cc 第 334 行定义.

                                                          {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddDerivativeBoundary, this, _1, _2, _3),
      x, dfx, range);
}

◆ AddPointSecondDerivativeConstraint()

bool apollo::planning::Spline1dConstraint::AddPointSecondDerivativeConstraint	(	const double	x,
		const double	ddfx
	)

在文件 spline_1d_constraint.cc 第 341 行定义.

                                                                               {
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  const uint32_t num_params = spline_order_ + 1;
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 2; i < num_params; ++i) {
    equality_constraint(0, index_offset + i) = power_x[i - 2] * i * (i - 1);
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = ddfx;
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}

◆ AddPointSecondDerivativeConstraintInRange()

bool apollo::planning::Spline1dConstraint::AddPointSecondDerivativeConstraintInRange	(	const double	x,
		const double	ddfx,
		const double	range
	)

在文件 spline_1d_constraint.cc 第 358 行定义.

                                                           {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddSecondDerivativeBoundary, this, _1, _2,
                _3),
      x, ddfx, range);
}

◆ AddPointThirdDerivativeConstraint()

bool apollo::planning::Spline1dConstraint::AddPointThirdDerivativeConstraint	(	const double	x,
		const double	dddfx
	)

在文件 spline_1d_constraint.cc 第 366 行定义.

                                                                               {
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  const uint32_t num_params = spline_order_ + 1;
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 3; i < num_params; ++i) {
    equality_constraint(0, index_offset + i) =
        power_x[i - 3] * i * (i - 1) * (i - 2);
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = dddfx;
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}

◆ AddPointThirdDerivativeConstraintInRange()

bool apollo::planning::Spline1dConstraint::AddPointThirdDerivativeConstraintInRange	(	const double	x,
		const double	dddfx,
		const double	range
	)

在文件 spline_1d_constraint.cc 第 384 行定义.

                                                            {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddSecondDerivativeBoundary, this, _1, _2,
                _3),
      x, dddfx, range);
}

◆ AddSecondDerivativeBoundary()

bool apollo::planning::Spline1dConstraint::AddSecondDerivativeBoundary	(	const std::vector< double > &	x_coord,
		const std::vector< double > &	lower_bound,
		const std::vector< double > &	upper_bound
	)

在文件 spline_1d_constraint.cc 第 167 行定义.

                                          {
  std::vector<double> filtered_lower_bound;
  std::vector<double> filtered_upper_bound;
  std::vector<double> filtered_lower_bound_x;
  std::vector<double> filtered_upper_bound_x;
 
  if (x_knots_.size() < 2) {
    return false;
  }
 
  if (!FilterConstraints(x_coord, lower_bound, upper_bound,
                         &filtered_lower_bound_x, &filtered_lower_bound,
                         &filtered_upper_bound_x, &filtered_upper_bound)) {
    return false;
  }
 
  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),
      (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);
 
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    for (uint32_t j = 2; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef * j * (j - 1);
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }
 
  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    for (uint32_t j = 2; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef * j * (j - 1);
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) =
        -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,
                                              inequality_boundary);
}

◆ AddSecondDerivativeSmoothConstraint()

bool apollo::planning::Spline1dConstraint::AddSecondDerivativeSmoothConstraint ( )

在文件 spline_1d_constraint.cc 第 455 行定义.

                                                             {
  if (x_knots_.size() < 3) {
    return false;
  }
 
  const uint32_t n_constraint =
      (static_cast<uint32_t>(x_knots_.size()) - 2) * 3;
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(n_constraint, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd equality_boundary = Eigen::MatrixXd::Zero(n_constraint, 1);
 
  for (uint32_t i = 0; i < n_constraint; i += 3) {
    double left_coef = 1.0;
    double right_coef = -1.0;
    double left_dcoef = 1.0;
    double right_dcoef = -1.0;
    double left_ddcoef = 1.0;
    double right_ddcoef = -1.0;
 
    const double x_left = x_knots_[i / 3 + 1] - x_knots_[i / 3];
    const double x_right = 0.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      equality_constraint(i, num_params * (i / 3) + j) = left_coef;
      equality_constraint(i, num_params * (i / 3 + 1) + j) = right_coef;
 
      if (j >= 2) {
        equality_constraint(i + 2, num_params * i / 3 + j) =
            left_ddcoef * j * (j - 1);
        equality_constraint(i + 2, num_params * (i / 3 + 1) + j) =
            right_ddcoef * j * (j - 1);
        left_ddcoef = left_dcoef;
        right_ddcoef = right_dcoef;
      }
 
      if (j >= 1) {
        equality_constraint(i + 1, num_params * (i / 3) + j) = left_dcoef * j;
        equality_constraint(i + 1, num_params * (i / 3 + 1) + j) =
            right_dcoef * j;
        left_dcoef = left_coef;
        right_dcoef = right_coef;
      }
      left_coef *= x_left;
      right_coef *= x_right;
    }
  }
  return equality_constraint_.AddConstraint(equality_constraint,
                                            equality_boundary);
}

◆ AddSmoothConstraint()

bool apollo::planning::Spline1dConstraint::AddSmoothConstraint ( )

在文件 spline_1d_constraint.cc 第 392 行定义.

                                             {
  if (x_knots_.size() < 3) {
    return false;
  }
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd equality_constraint = Eigen::MatrixXd::Zero(
      x_knots_.size() - 2, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd equality_boundary =
      Eigen::MatrixXd::Zero(x_knots_.size() - 2, 1);
 
  for (uint32_t i = 0; i < x_knots_.size() - 2; ++i) {
    double left_coef = 1.0;
    double right_coef = -1.0;
    const double x_left = x_knots_[i + 1] - x_knots_[i];
    const double x_right = 0.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      equality_constraint(i, num_params * i + j) = left_coef;
      equality_constraint(i, num_params * (i + 1) + j) = right_coef;
      left_coef *= x_left;
      right_coef *= x_right;
    }
  }
  return equality_constraint_.AddConstraint(equality_constraint,
                                            equality_boundary);
}

◆ AddThirdDerivativeBoundary()

bool apollo::planning::Spline1dConstraint::AddThirdDerivativeBoundary	(	const std::vector< double > &	x_coord,
		const std::vector< double > &	lower_bound,
		const std::vector< double > &	upper_bound
	)

在文件 spline_1d_constraint.cc 第 220 行定义.

                                          {
  std::vector<double> filtered_lower_bound;
  std::vector<double> filtered_upper_bound;
  std::vector<double> filtered_lower_bound_x;
  std::vector<double> filtered_upper_bound_x;
 
  if (!FilterConstraints(x_coord, lower_bound, upper_bound,
                         &filtered_lower_bound_x, &filtered_lower_bound,
                         &filtered_upper_bound_x, &filtered_upper_bound)) {
    AERROR << "Fail to filter constraints.";
    return false;
  }
 
  if (x_knots_.size() < 2) {
    AERROR << "x_konts size cannot be < 2.";
    return false;
  }
 
  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),
      (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);
 
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    for (uint32_t j = 3; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) =
          coef * j * (j - 1) * (j - 2);
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }
 
  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    for (uint32_t j = 3; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) =
          coef * j * (j - 1) * (j - 2);
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) =
        -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,
                                              inequality_boundary);
}

◆ AddThirdDerivativeSmoothConstraint()

bool apollo::planning::Spline1dConstraint::AddThirdDerivativeSmoothConstraint ( )

在文件 spline_1d_constraint.cc 第 505 行定义.

                                                            {
  if (x_knots_.size() < 3) {
    return false;
  }
 
  const uint32_t n_constraint =
      (static_cast<uint32_t>(x_knots_.size()) - 2) * 4;
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(n_constraint, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd equality_boundary = Eigen::MatrixXd::Zero(n_constraint, 1);
 
  for (uint32_t i = 0; i < n_constraint; i += 4) {
    double left_coef = 1.0;
    double right_coef = -1.0;
    double left_dcoef = 1.0;
    double right_dcoef = -1.0;
    double left_ddcoef = 1.0;
    double right_ddcoef = -1.0;
    double left_dddcoef = 1.0;
    double right_dddcoef = -1.0;
 
    const double x_left = x_knots_[i / 4 + 1] - x_knots_[i / 4];
    const double x_right = 0.0;
    for (uint32_t j = 0; j < num_params; ++j) {
      equality_constraint(i, num_params * i / 4 + j) = left_coef;
      equality_constraint(i, num_params * (i / 4 + 1) + j) = right_coef;
 
      if (j >= 3) {
        equality_constraint(i + 3, num_params * i / 4 + j) =
            left_dddcoef * j * (j - 1) * (j - 2);
        equality_constraint(i + 3, num_params * (i / 4 + 1) + j) =
            right_dddcoef * j * (j - 1) * (j - 2);
        left_dddcoef = left_ddcoef;
        right_dddcoef = right_ddcoef;
      }
 
      if (j >= 2) {
        equality_constraint(i + 2, num_params * i / 4 + j) =
            left_ddcoef * j * (j - 1);
        equality_constraint(i + 2, num_params * (i / 4 + 1) + j) =
            right_ddcoef * j * (j - 1);
        left_ddcoef = left_dcoef;
        right_ddcoef = right_dcoef;
      }
 
      if (j >= 1) {
        equality_constraint(i + 1, num_params * i / 4 + j) = left_dcoef * j;
        equality_constraint(i + 1, num_params * (i / 4 + 1) + j) =
            right_dcoef * j;
        left_dcoef = left_coef;
        right_dcoef = right_coef;
      }
 
      left_coef *= x_left;
      right_coef *= x_right;
    }
  }
  return equality_constraint_.AddConstraint(equality_constraint,
                                            equality_boundary);
}

◆ equality_constraint()

const AffineConstraint & apollo::planning::Spline1dConstraint::equality_constraint ( ) const

在文件 spline_1d_constraint.cc 第 621 行定义.

                                                                      {
  return equality_constraint_;
}

◆ inequality_constraint()

const AffineConstraint & apollo::planning::Spline1dConstraint::inequality_constraint ( ) const

: output interface inequality constraint

在文件 spline_1d_constraint.cc 第 617 行定义.

                                                                        {
  return inequality_constraint_;
}

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

modules/planning/planning_base/math/smoothing_spline/spline_1d_constraint.h
modules/planning/planning_base/math/smoothing_spline/spline_1d_constraint.cc

Public 成员函数