平滑cost:
template<typename T>
inline void addSmoothingResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r) const
{
Eigen::Matrix<T, 2, 1> d_next = pt_next - pt;
Eigen::Matrix<T, 2, 1> d_prev = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d_diff = next_to_last_length_ratio_ * d_next - d_prev;
r += (T)weight * d_diff.dot(d_diff); // objective function value
}
曲率cost
圆心计算： https://paulbourke.net/geometry/circlesphere/
template<typename T>
inline void addCurvatureResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r) const
{
Eigen::Matrix<T, 2, 1> center = arcCenter(
pt_prev, pt, pt_next,
next_to_last_length_ratio_ < 0);
if (CERES_ISINF(center[0])) {
return;
}
T turning_rad = (pt - center).norm(); //计算半径
T ki_minus_kmax = (T)1.0 / turning_rad - params_.max_curvature; //曲率与最大曲率之间的差
if (ki_minus_kmax <= (T)EPSILON) {
return;
}
r += (T)weight * ki_minus_kmax * ki_minus_kmax; // objective function value
}
template<typename T>
inline Eigen::Matrix<T, 2, 1> arcCenter(
Eigen::Matrix<T, 2, 1> pt_prev,
Eigen::Matrix<T, 2, 1> pt,
Eigen::Matrix<T, 2, 1> pt_next,
bool is_cusp)
{
Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
if (is_cusp) {
d2 = -d2;
pt_next = pt + d2;
}
T det = d1[0] * d2[1] - d1[1] * d2[0];
if (ceres::abs(det) < (T)1e-4) { // straight line
return Eigen::Matrix<T, 2, 1>(
(T)std::numeric_limits<double>::infinity(), (T)std::numeric_limits<double>::infinity());
}
// circle center is at the intersection of mirror axes of the segments:
// http://paulbourke.net/geometry/circlesphere/
// line intersection:
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
Eigen::Matrix<T, 2, 1> mid1 = (pt_prev + pt) / (T)2;
Eigen::Matrix<T, 2, 1> mid2 = (pt + pt_next) / (T)2;
Eigen::Matrix<T, 2, 1> n1(-d1[1], d1[0]);
Eigen::Matrix<T, 2, 1> n2(-d2[1], d2[0]);
T det1 = (mid1[0] + n1[0]) * mid1[1] - (mid1[1] + n1[1]) * mid1[0];
T det2 = (mid2[0] + n2[0]) * mid2[1] - (mid2[1] + n2[1]) * mid2[0];
Eigen::Matrix<T, 2, 1> center((det1 * n2[0] - det2 * n1[0]) / det,
(det1 * n2[1] - det2 * n1[1]) / det);
return center;
}
参考点偏移cost,与原始参考点的偏移
template<typename T>
inline void addDistanceResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & xi,
const Eigen::Matrix<T, 2, 1> & xi_original,
T & r) const
{
r += (T)weight * (xi - xi_original).squaredNorm(); // objective function value
}
障碍物距离代价，通过压入costmap最小化cost来远离障碍物，自车用多圆模型表示，每个圆心的cost代价可不同：
template<typename T>
inline void addCostResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r) const
{
if (params_.cost_check_points.empty()) {
Eigen::Matrix<T, 2, 1> interp_pos =
(pt - costmap_origin_.template cast<T>()) / (T)costmap_resolution_;
T value;
costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value); //双三次插值
r += (T)weight * value * value; // objective function value
} else {
//这里计算了路径的切线方向 dir，以便将成本检查点放在沿着路径方向的正确一侧。切线方向取决于路径是否正在倒车
Eigen::Matrix<T, 2, 1> dir = tangentDir(
pt_prev, pt, pt_next,
next_to_last_length_ratio_ < 0);
dir.normalize();
//这个条件检查路径的朝向是否与 dir 方向一致，如果不一致，则取相反的方向。
if (((pt_next - pt).dot(dir) < (T)0) != reversing_) {
dir = -dir;
}
//这里创建了一个仿射变换矩阵 transform，该矩阵用于将成本检查点从局部坐标系转换为全局坐标系。这个变换基于 dir 和当前路径点 pt 的位置。
Eigen::Matrix<T, 3, 3> transform;
transform << dir[0], -dir[1], pt[0],
dir[1], dir[0], pt[1],
(T)0, (T)0, (T)1;
for (size_t i = 0; i < params_.cost_check_points.size(); i += 3) {
Eigen::Matrix<T, 3, 1> ccpt((T)params_.cost_check_points[i],
(T)params_.cost_check_points[i + 1], (T)1);
auto ccpt_world = (transform * ccpt).template block<2, 1>(0, 0);
Eigen::Matrix<T, 2,
1> interp_pos = (ccpt_world - costmap_origin_.template cast<T>()) /
(T)costmap_resolution_;
T value;
costmap_interpolator_->Evaluate(interp_pos[1] - (T)0.5, interp_pos[0] - (T)0.5, &value);
r += (T)weight * (T)params_.cost_check_points[i + 2] * value * value;
}
}
}