Apollo 常见math库学习

1 Vec2d

向量表示point
vec2d.h

#pragma once
// 定义二维向量类
#include <cmath>
#include <string>/*** @namespace apollo::common::math* @brief apollo::common::math*/
namespace apollo {
namespace common {
namespace math {constexpr double kMathEpsilon = 1e-10;/*** @class Vec2d** @brief Implements a class of 2-dimensional vectors.*/
class Vec2d {public://! Constructor which takes x- and y-coordinates.constexpr Vec2d(const double x, const double y) noexcept : x_(x), y_(y) {}//! 返回零向量的构造函数constexpr Vec2d() noexcept : Vec2d(0, 0) {}//! 根据给定角度创建一个单位向量，角度是相对于正 x 半轴的static Vec2d CreateUnitVec2d(const double angle);//! Getter for x componentdouble x() const { return x_; }//! Getter for y componentdouble y() const { return y_; }//! Setter for x componentvoid set_x(const double x) { x_ = x; }//! Setter for y componentvoid set_y(const double y) { y_ = y; }//! 获取向量的长度double Length() const;//! 获取向量的平方长度double LengthSquare() const;//! 获取向量与正 x 半轴之间的夹角double Angle() const;//! 返回与该向量共线的单位向量void Normalize();//! 返回到给定向量的距离double DistanceTo(const Vec2d &other) const;//! 返回到给定向量的平方距离double DistanceSquareTo(const Vec2d &other) const;//! 返回这两个 Vec2d 向量的叉乘（非标准定义）double CrossProd(const Vec2d &other) const;//! 返回这两个 Vec2d 向量的点积double InnerProd(const Vec2d &other) const;//! 将向量按照给定角度旋转Vec2d rotate(const double angle) const;//! 将向量本身按照给定角度旋转void SelfRotate(const double angle);//! 重载 + 操作符，实现两个 Vec2d 向量的加法Vec2d operator+(const Vec2d &other) const;//! 重载 - 操作符，实现两个 Vec2d 向量的减法Vec2d operator-(const Vec2d &other) const;//! 重载 * 操作符，将 Vec2d 向量乘以标量Vec2d operator*(const double ratio) const;//! 重载 / 操作符，将 Vec2d 向量除以标量Vec2d operator/(const double ratio) const;//! 重载 += 操作符，将另一个 Vec2d 向量加到当前向量上Vec2d &operator+=(const Vec2d &other);//! 重载 -= 操作符，将另一个 Vec2d 向量从当前向量上减去Vec2d &operator-=(const Vec2d &other);//! 重载 *= 操作符，将当前 Vec2d 向量乘以标量Vec2d &operator*=(const double ratio);//! 重载 /= 操作符，将当前 Vec2d 向量除以标量Vec2d &operator/=(const double ratio);//! 重载 == 操作符，比较两个 Vec2d 是否相等bool operator==(const Vec2d &other) const;//! 返回表示对象的可读字符串std::string DebugString() const;protected:double x_ = 0.0;double y_ = 0.0;
};//! 重载 * 操作符，将给定的 Vec2d 向量乘以给定的标量
Vec2d operator*(const double ratio, const Vec2d &vec);}  // namespace math
}  // namespace common
}  // namespace apollo

vec2d.cc

#include "modules/common/math/vec2d.h"#include <cmath>#include "absl/strings/str_cat.h"#include "cyber/common/log.h"namespace apollo {
namespace common {
namespace math {
// 给定角度创建单位向量
Vec2d Vec2d::CreateUnitVec2d(const double angle) {return Vec2d(std::cos(angle), std::sin(angle));
}double Vec2d::Length() const { return std::hypot(x_, y_); }
//! 获取向量的平方长度
double Vec2d::LengthSquare() const { return x_ * x_ + y_ * y_; }double Vec2d::Angle() const { return std::atan2(y_, x_); }void Vec2d::Normalize() {const double l = Length();if (l > kMathEpsilon) {x_ /= l;y_ /= l;}
}
//! 返回到给定向量的距离
double Vec2d::DistanceTo(const Vec2d &other) const {return std::hypot(x_ - other.x_, y_ - other.y_);
}double Vec2d::DistanceSquareTo(const Vec2d &other) const {const double dx = x_ - other.x_;const double dy = y_ - other.y_;return dx * dx + dy * dy;
}
// 叉乘
double Vec2d::CrossProd(const Vec2d &other) const {return x_ * other.y() - y_ * other.x();
}
// 点积
double Vec2d::InnerProd(const Vec2d &other) const {return x_ * other.x() + y_ * other.y();
}
// 给定角度旋转
Vec2d Vec2d::rotate(const double angle) const {return Vec2d(x_ * cos(angle) - y_ * sin(angle),x_ * sin(angle) + y_ * cos(angle));
}void Vec2d::SelfRotate(const double angle) {double tmp_x = x_;x_ = x_ * cos(angle) - y_ * sin(angle);y_ = tmp_x * sin(angle) + y_ * cos(angle);
}Vec2d Vec2d::operator+(const Vec2d &other) const {return Vec2d(x_ + other.x(), y_ + other.y());
}Vec2d Vec2d::operator-(const Vec2d &other) const {return Vec2d(x_ - other.x(), y_ - other.y());
}Vec2d Vec2d::operator*(const double ratio) const {return Vec2d(x_ * ratio, y_ * ratio);
}Vec2d Vec2d::operator/(const double ratio) const {CHECK_GT(std::abs(ratio), kMathEpsilon);return Vec2d(x_ / ratio, y_ / ratio);
}Vec2d &Vec2d::operator+=(const Vec2d &other) {x_ += other.x();y_ += other.y();return *this;
}Vec2d &Vec2d::operator-=(const Vec2d &other) {x_ -= other.x();y_ -= other.y();return *this;
}Vec2d &Vec2d::operator*=(const double ratio) {x_ *= ratio;y_ *= ratio;return *this;
}Vec2d &Vec2d::operator/=(const double ratio) {CHECK_GT(std::abs(ratio), kMathEpsilon);x_ /= ratio;y_ /= ratio;return *this;
}bool Vec2d::operator==(const Vec2d &other) const {return (std::abs(x_ - other.x()) < kMathEpsilon &&std::abs(y_ - other.y()) < kMathEpsilon);
}Vec2d operator*(const double ratio, const Vec2d &vec) { return vec * ratio; }std::string Vec2d::DebugString() const {return absl::StrCat("vec2d ( x = ", x_, "  y = ", y_, " )");
}}  // namespace math
}  // namespace common
}  // namespace apollo

这个二维向量相关操作，看相关源码一目了然，可以在test文件里验证相关操作的正确性

2 angle

sin函数查表，对应的是[0,pi/2]到[0,16384]之间的查表值进行查表
直接看.h相关，了解相关基本操作
通过查找sin_table，实现角度快速查找
angle.h

#pragma once#include <cmath>
#include <cstdint>
#include <limits>namespace apollo {
namespace common {
namespace math {
/**
*@类角度
*@brief Angle类使用整数表示角度，并支持
*常用的操作，如加法和减法，
*以及三角函数的使用。
*
*有一个专门的Angle类可以防止代码重复，特别是对于任务
*例如计算角度差或在某些情况下找到等效角度
*指定的间隔，通常为[-pi，pi）。
*用整数表示角度有以下优点：
*1）更精细的精度控制水平（<表示“精度低于”）：
*角度8<角度16<浮动<角度32<双<角度64。
*2）角度8和角度16允许超快速三角函数
*通过64KiB查找表。
*3）在相同的表示尺寸下，精度更高。
*鼓励使用Angle类。
*特别是，Angle32应用于纬度/经度（误差<1cm）。
*Angle16对于定位/目标检测来说足够精确。
*@param T带符号整数类型
*/
template <typename T>
class Angle {public:static_assert(std::numeric_limits<T>::is_integer &&std::numeric_limits<T>::is_signed,"T must be a signed integer type");/*** @brief Constructs an Angle object from an angle in degrees (factory).* @param value Angle in degrees* @return Angle object*/static Angle from_deg(const double value) {return Angle(static_cast<T>(std::lround(value * DEG_TO_RAW)));}/*** @brief Constructs an Angle object from an angle in radians (factory).* @param value Angle in radians* @return Angle object*/static Angle from_rad(const double value) {return Angle(static_cast<T>(std::lround(value * RAD_TO_RAW)));}explicit Angle(const T value = 0) : value_(value) {}// pi的内部表示static constexpr T RAW_PI = std::numeric_limits<T>::min();/// Internal representation of pi/2static constexpr T RAW_PI_2 =static_cast<T>(-(std::numeric_limits<T>::min() >> 1));// 转换单元static constexpr double DEG_TO_RAW = RAW_PI / -180.0;/// Used for converting angle unitsstatic constexpr double RAD_TO_RAW = RAW_PI * -M_1_PI;/// Used for converting angle unitsstatic constexpr double RAW_TO_DEG = -180.0 / RAW_PI;/// Used for converting angle unitsstatic constexpr double RAW_TO_RAD = -M_PI / RAW_PI;// 返回值T raw() const { return value_; }// 度double to_deg() const { return value_ * RAW_TO_DEG; }// raddouble to_rad() const { return value_ * RAW_TO_RAD; }// 两角之和Angle operator+=(Angle other) {value_ = static_cast<T>(value_ + other.value_);return *this;}// 两角相减Angle operator-=(Angle other) {value_ = static_cast<T>(value_ - other.value_);return *this;}/*** @brief Multiplies angle by scalar* @param s A scalar* @return Result of multiplication*/template <typename Scalar>Angle operator*=(Scalar s) {value_ = static_cast<T>(std::lround(value_ * s));return *this;}/*** @brief Divides angle by scalar* @param s A scalar* @return Result of division*/template <typename Scalar>Angle operator/=(Scalar s) {value_ = static_cast<T>(std::lround(value_ / s));return *this;}private:T value_;
};using Angle8 = Angle<int8_t>;
using Angle16 = Angle<int16_t>;
using Angle32 = Angle<int32_t>;
using Angle64 = Angle<int64_t>;/*** @brief Sums two angles* @param lhs An Angle object* @param rhs An Angle object* @return Result of addition*/
template <typename T>
Angle<T> operator+(Angle<T> lhs, Angle<T> rhs) {lhs += rhs;return lhs;
}/*** @brief Subtracts two angles* @param lhs An Angle object* @param rhs An Angle object* @return Result of subtraction*/
template <typename T>
Angle<T> operator-(Angle<T> lhs, Angle<T> rhs) {lhs -= rhs;return lhs;
}/*** @brief Multiplies an Angle by a scalar* @param lhs An Angle object* @param rhs A scalar* @return Result of multiplication*/
template <typename T, typename Scalar>
Angle<T> operator*(Angle<T> lhs, Scalar rhs) {lhs *= rhs;return lhs;
}/*** @brief Multiplies an Angle by a scalar* @param lhs An Angle object* @param rhs A scalar* @return Result of multiplication*/
template <typename T, typename Scalar>
Angle<T> operator*(Scalar lhs, Angle<T> rhs) {rhs *= lhs;return rhs;
}/*** @brief Divides an Angle by a scalar* @param lhs An Angle object* @param rhs A scalar* @return Result of division*/
template <typename T, typename Scalar>
Angle<T> operator/(Angle<T> lhs, Scalar rhs) {lhs /= rhs;return lhs;
}/*** @brief Divides an Angle by a scalar* @param lhs An Angle object* @param rhs A scalar* @return Result of division*/
template <typename T>
double operator/(Angle<T> lhs, Angle<T> rhs) {return static_cast<double>(lhs.raw()) / rhs.raw();
}/*** @brief Tests two Angle objects for equality* @param lhs An Angle object* @param rhs An Angle object* @return lhs == rhs*/
template <typename T>
bool operator==(Angle<T> lhs, Angle<T> rhs) {return lhs.raw() == rhs.raw();
}/*** @brief Tests two Angle objects for inequality* @param lhs An Angle object* @param rhs An Angle object* @return lhs != rhs*/
template <typename T>
bool operator!=(Angle<T> lhs, Angle<T> rhs) {return !(lhs == rhs);
}// 快速三角函数。对于Angle16和Angle8，单精度就足够了
float sin(Angle16 a);
float cos(Angle16 a);
float tan(Angle16 a);
float sin(Angle8 a);
float cos(Angle8 a);
float tan(Angle8 a);}  // namespace math
}  // namespace common
}  // namespace apollo

3 search算法

实现了一种简单但有效的数值优化算法，用于在指定区间内寻找函数的极小值点。黄金分割搜索算法通常用于一维函数优化问题，特别适用于连续、单峰函数的最小化

double GoldenSectionSearch(const std::function<double(double)> &func,const double lower_bound, const double upper_bound,const double tol) {static constexpr double gr = 1.618033989;  // (sqrt(5) + 1) / 2double a = lower_bound;double b = upper_bound;double t = (b - a) / gr;double c = b - t;double d = a + t;while (std::abs(c - d) > tol) {if (func(c) < func(d)) {b = d;} else {a = c;}t = (b - a) / gr;c = b - t;d = a + t;}return (a + b) * 0.5;
}

4 line_segment

比vec2d包含的信息多一些，基本思想类似，利用向量做一些运算
line_segment.h

#pragma once#include <string>#include "modules/common/math/vec2d.h"namespace apollo {
namespace common {
namespace math {class LineSegment2d {public:LineSegment2d();LineSegment2d(const Vec2d &start, const Vec2d &end);// 获取线段的起点const Vec2d &start() const { return start_; }// 获取线段的终点const Vec2d &end() const { return end_; }// 获取从起点到终点的方向const Vec2d &unit_direction() const { return unit_direction_; }// 获取线段的中心Vec2d center() const { return (start_ + end_) / 2.0; }// 旋转线段的终点Vec2d rotate(const double angle);// 返回线段的航向double heading() const { return heading_; }// cos(heading_)double cos_heading() const { return unit_direction_.x(); }// sin(heading_)double sin_heading() const { return unit_direction_.y(); }// 获取线段的长度double length() const;// 获取线段长度的平方double length_sqr() const;// 计算线段上的点到2-D中的点的最短距离。double DistanceTo(const Vec2d &point) const;// 计算线段上一点到二维中一点的最短距离，得到线段上最近的点double DistanceTo(const Vec2d &point, Vec2d *const nearest_pt) const;// 计算线段上的一点到2-D中的一点的最短距离的平方double DistanceSquareTo(const Vec2d &point) const;// 计算二维中线段上一点到一点的最短距离的平方，得到线段上最近的点double DistanceSquareTo(const Vec2d &point, Vec2d *const nearest_pt) const;// 检查一个点是否在线段内bool IsPointIn(const Vec2d &point) const;// 检查该线段是否与二维中的另一条线段相交bool HasIntersect(const LineSegment2d &other_segment) const;// 计算与二维中另一条线段的交点（如果有的话）bool GetIntersect(const LineSegment2d &other_segment,Vec2d *const point) const;// 计算矢量在线段上的投影double ProjectOntoUnit(const Vec2d &point) const;// 计算向量与线段的叉积double ProductOntoUnit(const Vec2d &point) const;// 计算从线段展开的直线上的二维点的垂直脚部double GetPerpendicularFoot(const Vec2d &point,Vec2d *const foot_point) const;// 获取包含基本信息的调试字符串std::string DebugString() const;private:Vec2d start_;Vec2d end_;Vec2d unit_direction_;double heading_ = 0.0;double length_ = 0.0;
};}  // namespace math
}  // namespace common
}  // namespace apollo

罗列几个重要的函数：
（1）计算点到直线的距离

double LineSegment2d::DistanceTo(const Vec2d &point) const {// 如果线段的长度小于等于一个阈值kMathEpsilon，那么点一定在线段上，直接返回点与线段起点的距离if (length_ <= kMathEpsilon) {return point.DistanceTo(start_);}const double x0 = point.x() - start_.x();const double y0 = point.y() - start_.y();// proj，表示点在方向向量上的投影const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();// 如果投影小于等于0，说明点在直线段的同侧，直接返回点到线段起点的距离if (proj <= 0.0) {return hypot(x0, y0);}// 如果投影大于等于线段长度，说明点在直线段的延长线上，直接返回点到线段终点的距离if (proj >= length_) {return point.DistanceTo(end_);}return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

（2）给定一个点，计算到线段的最短距离，同时返回最近的点（过给定点的垂线与原线段的交点，或者线段的端点）

double LineSegment2d::DistanceTo(const Vec2d &point,Vec2d *const nearest_pt) const {// 检查nearest_pt是否为空CHECK_NOTNULL(nearest_pt);// 如果线段的长度小于等于一个阈值kMathEpsilon，那么点一定在线段上，直接返回点与线段起点的距离if (length_ <= kMathEpsilon) {*nearest_pt = start_;return point.DistanceTo(start_);}// 计算点与线段起点的坐标差x0和y0const double x0 = point.x() - start_.x();const double y0 = point.y() - start_.y();// 计算proj，表示点在方向向量上的投影const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();// 如果投影小于等于0，说明点在直线段的同侧，直接返回点到线段起点的距离。如果投影大于等于线段长度，说明点在直线段的延长线上，直接返回点到线段终点的距离if (proj < 0.0) {*nearest_pt = start_;return hypot(x0, y0);}if (proj > length_) {*nearest_pt = end_;return point.DistanceTo(end_);}*nearest_pt = start_ + unit_direction_ * proj;// 计算点到线段的垂线与x轴正半轴的夹角，即x0 * unit_direction_.y() - y0 * unit_direction_.x()，取绝对值作为最终结果return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

（3）某个点到该线段垂点的距离

double LineSegment2d::GetPerpendicularFoot(const Vec2d &point,Vec2d *const foot_point) const {CHECK_NOTNULL(foot_point);if (length_ <= kMathEpsilon) {*foot_point = start_;return point.DistanceTo(start_);}const double x0 = point.x() - start_.x();const double y0 = point.y() - start_.y();const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();*foot_point = start_ + unit_direction_ * proj;return std::abs(x0 * unit_direction_.y() - y0 * unit_direction_.x());
}

（4）判断点是否在线段上

bool LineSegment2d::IsPointIn(const Vec2d &point) const {if (length_ <= kMathEpsilon) {return std::abs(point.x() - start_.x()) <= kMathEpsilon &&std::abs(point.y() - start_.y()) <= kMathEpsilon;}const double prod = CrossProd(point, start_, end_);if (std::abs(prod) > kMathEpsilon) {return false;}return IsWithin(point.x(), start_.x(), end_.x()) &&IsWithin(point.y(), start_.y(), end_.y());
}

长度检查：首先，检查线段的长度 length_ 是否小于等于一个极小的值 kMathEpsilon。如果线段的长度非常接近于零，就将该线段视为一个点，如果线段长度很小，只需检查点 point 是否与线段起点 start_ 非常接近（即坐标差的绝对值小于等于 kMathEpsilon）。如果是，则认为点在线段上。
叉乘检查：如果线段长度不是非常小，则通过叉乘判断点 point 是否在由起点 start_ 和终点 end_ 构成的线段上；如果叉乘的结果的绝对值大于 kMathEpsilon，则认为点 point 不在线段上，直接返回 false
范围检查：调用 IsWithin 函数来确保点的 x 坐标和 y 坐标都在线段的起点和终点之间

长度检查、叉乘检查、范围检查

5 math_utils

看一下math_utils的一些相关操作
向量点乘、叉乘，角度归一化，随机数产生，平方数，限幅，笛卡尔到极坐标转换，高斯，Sigmoid
math_utils.h

// 平方
double Sqr(const double x);// 叉乘
double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1,const Vec2d &end_point_2);// 点积
double InnerProd(const Vec2d &start_point, const Vec2d &end_point_1,const Vec2d &end_point_2);// 叉乘
double CrossProd(const double x0, const double y0, const double x1,const double y1);// 点积
double InnerProd(const double x0, const double y0, const double x1,const double y1);// Wrap angle to [0, 2 * PI).
double WrapAngle(const double angle);// Normalize angle to [-PI, PI).
double NormalizeAngle(const double angle);// 计算角度差
double AngleDiff(const double from, const double to);// 通过随机种子生成两个整数之间的随机整数
int RandomInt(const int s, const int t, unsigned int rand_seed = 1);// 通过随机种子生成两个double之间的随机整数
double RandomDouble(const double s, const double t, unsigned int rand_seed = 1);// 平方
template <typename T>
inline T Square(const T value) {return value * value;
}// 限幅
template <typename T>
T Clamp(const T value, T bound1, T bound2) {if (bound1 > bound2) {std::swap(bound1, bound2);}if (value < bound1) {return bound1;} else if (value > bound2) {return bound2;}return value;
}// Gaussian
// 高斯函数，计算给定均值和标准差下，输入值的概率密度函数值
double Gaussian(const double u, const double std, const double x);
// 计算Sigmoid函数的值，用于将实数映射到 (0, 1) 区间
inline double Sigmoid(const double x) { return 1.0 / (1.0 + std::exp(-x)); }// Rotate a 2d vector counter-clockwise by theta
// 将二维向量逆时针旋转给定角度 theta 弧度
Eigen::Vector2d RotateVector2d(const Eigen::Vector2d &v_in, const double theta);
// 将RFU坐标系下的 (x, y) 转换为FLU坐标系下的坐标
inline std::pair<double, double> RFUToFLU(const double x, const double y) {return std::make_pair(y, -x);
}
// 将FLU坐标系下的 (x, y) 转换为RFU坐标系下的坐标
inline std::pair<double, double> FLUToRFU(const double x, const double y) {return std::make_pair(-y, x);
}
// L2范数归一化
// 对特征数据进行L2范数归一化，确保特征向量的长度为1
inline void L2Norm(int feat_dim, float *feat_data) {if (feat_dim == 0) {return;}// 计算特征向量的L2范数平方float l2norm = 0.0f;for (int i = 0; i < feat_dim; ++i) {l2norm += feat_data[i] * feat_data[i];}// 如果L2范数平方为0，则特征向量长度为0，进行均匀分布归一化if (l2norm == 0) {float val = 1.f / std::sqrt(static_cast<float>(feat_dim));for (int i = 0; i < feat_dim; ++i) {feat_data[i] = val;}} else {// 否则，计算L2范数并对特征向量进行归一化l2norm = std::sqrt(l2norm);for (int i = 0; i < feat_dim; ++i) {feat_data[i] /= l2norm;}}
}// Cartesian coordinates to Polar coordinates
// 笛卡尔坐标系到极坐标系的转换
std::pair<double, double> Cartesian2Polar(double x, double y);template <class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
almost_equal(T x, T y, int ulp) {// the machine epsilon has to be scaled to the magnitude of the values used// and multiplied by the desired precision in ULPs (units in the last place)// unless the result is subnormalreturn std::fabs(x - y) <=std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp ||std::fabs(x - y) < std::numeric_limits<T>::min();
}
// 检查是否为负数
double check_negative(double input_data);

6 笛卡尔和Frenet互转

在这里插入图片描述
以Cartesian坐标为准

$\vec{r_n}$ ：车的位矢
$\vec{v}$ ：车的速度
$\vec{a}$ ：车的加速度
$k_h$ ：车的位矢在车的轨迹上的曲率
$\vec{\tau _h}$ ：车的位矢在车的轨迹上的切线方向单位向量
$\vec{ n_h}$ ：车的位矢在车的轨迹上的法线方向单位向量
$\vec{r_r}$ ：投影位矢
$\dot S$ ：投影速率
$k_r$ ：投影位矢在道路几何上的曲率
$\vec{\tau _r}$ ：投影位矢在道路几何上的切线方向单位向量
$\vec{n _r}$ ：投影位矢在道路几何上的法线方向单位向量

已知 $\vec{r}_{h}, \vec{v}_{h}, \vec{a}_{h}, k_{h}, \vec{\tau}_{h}, \vec{n}_{h}$
己知 frenet 坐标系起点 $\left(x_{0}, y_{0}\right)$
求 $\dot{s}, \ddot{s}, l, l^{\prime}, l^{\prime \prime} \quad \dot{s}=\frac{d s}{d t} \quad l^{\prime}=\frac{d l}{d s} \quad$ $d s$ 为frenet坐标轴的弧长

主要思路：
（1）找车在Frenet坐标系下的投影点在笛卡尔坐标下的坐标，记为 $(x_r,y_r,k_r,\theta_r)$
（2）利用向量三角形，以及微积分求 $s,\dot s,\ddot s,l,l_{}',l_{}'$
核心公式： $\vec r_r+l\vec n_r=\vec r_h$

7个辅助公式：
$\begin{array}{l} \overrightarrow{\dot{r}}_{h}=\vec{v}=|\vec{v}| \vec{\tau}_{h} \\ \vec{\dot r}_{r}=\dot{s} \vec{\tau}_{r} \\ \vec{\tau}_{h}=k_{h}|\vec{v}| \vec{n}_{h} \\ \vec{n}_{h}=-k_{h}|\vec{v}| \vec{\tau}_{h} \\ \vec{\dot \tau}_{r}=k_{r} \dot{s} \vec{n}_{r} \\ \overrightarrow{\dot{n}}_{r}=-k_{r} \dot{s} \vec{\tau}_{r} \\ \vec{a}=|\vec{\dot v}| \vec{\tau}_{h}+|\vec{v}|^{2} k_{h} \vec{n}_{h} \end{array}$
向量法详细推导见博客
$s,\dot s,\ddot s,l,\dot l,\ddot l,l_{}',l_{}''$ 八个变量，独立的只有6个，因为 $\dot l,\ddot l$ 可以由 $l_{}',l_{}''$ 推导过来
$\dot l= l'*\dot s\\ \\ \begin{aligned} \ddot{l}=\frac{d l}{d t}=\frac{d\left(l^{\prime} \dot{s}\right)}{d t} & =\frac{d l^{\prime}}{d t} \dot{s}+l^{\prime} \cdot \ddot{s} \\ & =\frac{d l^{\prime}}{d s} \cdot \frac{d s}{d t} \cdot \dot{s}+l^{\prime} \ddot{s} \\ & =l^{\prime \prime} \dot{s}^{2}+l^{\prime} \ddot{s} \end{aligned}$
EM planner： $s,\dot s,\ddot s,l,l_{}',l_{}'$
Lattice planner： $s,\dot s,\ddot s,l,\dot l,\ddot l$

s的计算方法：
主要思路：
用直线长度代替弧长，当路径点足够密集，误差可以忽略
在这里插入图片描述
这里直接给出Frenet和Cartesian互转的公式：

百度Apollo Frenet和Cartesian互转的代码：
modules/common/math/cartesian_frenet_conversion.h

static void cartesian_to_frenet(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const double x, const double y,const double v, const double a,const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition);static void frenet_to_cartesian(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition,double* const ptr_x, double* const ptr_y,double* const ptr_theta,double* const ptr_kappa, double* const ptr_v,double* const ptr_a);

modules/common/math/cartesian_frenet_conversion.cpp

void CartesianFrenetConverter::cartesian_to_frenet(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa, const double x, const double y,const double v, const double a, const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition) {const double dx = x - rx;const double dy = y - ry;const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;ptr_d_condition->at(0) =std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);const double delta_theta = theta - rtheta;const double tan_delta_theta = std::tan(delta_theta);const double cos_delta_theta = std::cos(delta_theta);const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;const double kappa_r_d_prime =rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);ptr_d_condition->at(2) =-kappa_r_d_prime * tan_delta_theta +one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *(kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);ptr_s_condition->at(0) = rs;ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;ptr_s_condition->at(2) =(a * cos_delta_theta -ptr_s_condition->at(1) * ptr_s_condition->at(1) *(ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /one_minus_kappa_r_d;
}void CartesianFrenetConverter::frenet_to_cartesian(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition, double* const ptr_x,double* const ptr_y, double* const ptr_theta, double* const ptr_kappa,double* const ptr_v, double* const ptr_a) {ACHECK(std::abs(rs - s_condition[0]) < 1.0e-6)<< "The reference point s and s_condition[0] don't match";const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);*ptr_x = rx - sin_theta_r * d_condition[0];*ptr_y = ry + cos_theta_r * d_condition[0];const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);const double cos_delta_theta = std::cos(delta_theta);*ptr_theta = NormalizeAngle(delta_theta + rtheta);const double kappa_r_d_prime =rdkappa * d_condition[0] + rkappa * d_condition[1];*ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *cos_delta_theta * cos_delta_theta) /(one_minus_kappa_r_d) +rkappa) *cos_delta_theta / (one_minus_kappa_r_d);const double d_dot = d_condition[1] * s_condition[1];*ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *s_condition[1] * s_condition[1] +d_dot * d_dot);const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;*ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +s_condition[1] * s_condition[1] / cos_delta_theta *(d_condition[1] * delta_theta_prime - kappa_r_d_prime);
}

7 linear_interpolation

线性插值：

根据点坐标进行插值
根据sl坐标及权重值进行插值
根据PathPoint坐标以及s值进行插值
根据TrajectoryPoint坐标及其时间进行插值

linear_interplation.h

#pragma once#include <cmath>#include "cyber/common/log.h"
#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"/*** @namespace apollo::common::math* @brief apollo::common::math*/
namespace apollo {
namespace common {
namespace math {/*** @brief Linear interpolation between two points of type T.* @param x0 The coordinate of the first point.* @param t0 The interpolation parameter of the first point.* @param x1 The coordinate of the second point.* @param t1 The interpolation parameter of the second point.* @param t The interpolation parameter for interpolation.* @param x The coordinate of the interpolated point.* @return Interpolated point.*/
// 两点之间线性插值
template <typename T>
T lerp(const T &x0, const double t0, const T &x1, const double t1,const double t) {if (std::abs(t1 - t0) <= 1.0e-6) {AERROR << "input time difference is too small";return x0;}const double r = (t - t0) / (t1 - t0);const T x = x0 + r * (x1 - x0);return x;
}/*** @brief Spherical linear interpolation between two angles.*        The two angles are within range [-M_PI, M_PI).* @param a0 The value of the first angle.* @param t0 The interpolation parameter of the first angle.* @param a1 The value of the second angle.* @param t1 The interpolation parameter of the second angle.* @param t The interpolation parameter for interpolation.* @param a The value of the spherically interpolated angle.* @return Interpolated angle.*/
// 两角之间的线性插值
double slerp(const double a0, const double t0, const double a1, const double t1,const double t);
// 使用线性拟合的插值
SLPoint InterpolateUsingLinearApproximation(const SLPoint &p0,const SLPoint &p1, const double w);
// 使用线性拟合的插值
PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,const PathPoint &p1,const double s);
// 使用线性拟合的插值
TrajectoryPoint InterpolateUsingLinearApproximation(const TrajectoryPoint &tp0,const TrajectoryPoint &tp1,const double t);}  // namespace math
}  // namespace common
}  // namespace apollo

linear_interpolation.cc

#include "modules/common/math/linear_interpolation.h"#include <cmath>#include "cyber/common/log.h"
#include "modules/common/math/math_utils.h"namespace apollo {
namespace common {
namespace math {
// 这段代码实现了一个双线性插值函数，用于角度值的平滑过渡，确保角度变化在周期性角度范围内进行有效的插值计算
double slerp(const double a0, const double t0, const double a1, const double t1,const double t) {if (std::abs(t1 - t0) <= kMathEpsilon) {ADEBUG << "input time difference is too small";return NormalizeAngle(a0);}const double a0_n = NormalizeAngle(a0);const double a1_n = NormalizeAngle(a1);double d = a1_n - a0_n;if (d > M_PI) {d = d - 2 * M_PI;} else if (d < -M_PI) {d = d + 2 * M_PI;}const double r = (t - t0) / (t1 - t0);const double a = a0_n + d * r;return NormalizeAngle(a);
}
// 在两个 SLPoint 结构（p0 和 p1）之间进行线性插值，产生一个新的 SLPoint
SLPoint InterpolateUsingLinearApproximation(const SLPoint &p0,const SLPoint &p1, const double w) {CHECK_GE(w, 0.0);SLPoint p;p.set_s((1 - w) * p0.s() + w * p1.s());p.set_l((1 - w) * p0.l() + w * p1.l());return p;
}
// 根据路径长度 s 在两个给定路径点之间进行线性插值，生成一个新的路径点对象，实现路径上平滑的过渡
PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,const PathPoint &p1,const double s) {double s0 = p0.s();double s1 = p1.s();PathPoint path_point;double weight = (s - s0) / (s1 - s0);double x = (1 - weight) * p0.x() + weight * p1.x();double y = (1 - weight) * p0.y() + weight * p1.y();double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s);double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa();double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa();double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa();path_point.set_x(x);path_point.set_y(y);path_point.set_theta(theta);path_point.set_kappa(kappa);path_point.set_dkappa(dkappa);path_point.set_ddkappa(ddkappa);path_point.set_s(s);return path_point;
}
// 适用于在路径规划或轨迹生成中，根据两个已知的轨迹点及其属性，在给定的插值参数 t 下进行线性插值，生成一个新的轨迹点对象，以实现平滑的路径过渡
TrajectoryPoint InterpolateUsingLinearApproximation(const TrajectoryPoint &tp0,const TrajectoryPoint &tp1,const double t) {if (!tp0.has_path_point() || !tp1.has_path_point()) {TrajectoryPoint p;p.mutable_path_point()->CopyFrom(PathPoint());return p;}const PathPoint pp0 = tp0.path_point();const PathPoint pp1 = tp1.path_point();double t0 = tp0.relative_time();double t1 = tp1.relative_time();TrajectoryPoint tp;tp.set_v(lerp(tp0.v(), t0, tp1.v(), t1, t));tp.set_a(lerp(tp0.a(), t0, tp1.a(), t1, t));tp.set_relative_time(t);tp.set_steer(slerp(tp0.steer(), t0, tp1.steer(), t1, t));PathPoint *path_point = tp.mutable_path_point();path_point->set_x(lerp(pp0.x(), t0, pp1.x(), t1, t));path_point->set_y(lerp(pp0.y(), t0, pp1.y(), t1, t));path_point->set_theta(slerp(pp0.theta(), t0, pp1.theta(), t1, t));path_point->set_kappa(lerp(pp0.kappa(), t0, pp1.kappa(), t1, t));path_point->set_dkappa(lerp(pp0.dkappa(), t0, pp1.dkappa(), t1, t));path_point->set_ddkappa(lerp(pp0.ddkappa(), t0, pp1.ddkappa(), t1, t));path_point->set_s(lerp(pp0.s(), t0, pp1.s(), t1, t));return tp;
}}  // namespace math
}  // namespace common
}  // namespace apollo

8 linear_quadratic_regulator

LQR：解决离散时间线性二次问题求解器
linear_quadratic_regulator.h

#pragma once#include "Eigen/Core"namespace apollo {
namespace common {
namespace math {/*** @brief 求解离散时间线性二次型问题的求解器.* @param A 系统动态矩阵* @param B 控制矩阵* @param Q 系统状态的成本矩阵* @param R 控制输出的成本矩阵* @param M 状态和控制之间的交叉项，即 x'Qx + u'Ru + 2x'Mu* @param tolerance 求解离散代数黎卡提方程的数值公差* @param max_num_iteration 求解黎卡提的最大迭代次数* @param ptr_K 反馈控制矩阵（指针）*/
void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,const Eigen::MatrixXd &M, const double tolerance,const uint max_num_iteration, Eigen::MatrixXd *ptr_K);/*** @brief 求解离散时间线性二次型问题的求解器.* @param A 系统动态矩阵* @param B 控制矩阵* @param Q 系统状态的成本矩阵* @param R 控制输出的成本矩阵* @param tolerance 求解离散代数黎卡提方程的数值公差* @param max_num_iteration 求解黎卡提的最大迭代次数* @param ptr_K 反馈控制矩阵（指针）*/
void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,const double tolerance, const uint max_num_iteration,Eigen::MatrixXd *ptr_K);}  // namespace math
}  // namespace common
}  // namespace apollo

linear_quadratic_regulator.cc

#include <limits>#include "Eigen/Dense"#include "cyber/common/log.h"
#include "modules/common/math/linear_quadratic_regulator.h"namespace apollo {
namespace common {
namespace math {using Matrix = Eigen::MatrixXd;// solver with cross term
// 求解带有交叉项的求解器
void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q,const Matrix &R, const Matrix &M, const double tolerance,const uint max_num_iteration, Matrix *ptr_K) {// 检查ABQRif (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() ||Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() ||M.rows() != Q.rows() || M.cols() != R.cols()) {AERROR << "LQR solver: one or more matrices have incompatible dimensions.";return;}// 转置矩阵Matrix AT = A.transpose();Matrix BT = B.transpose();Matrix MT = M.transpose();// 求解离散时间代数黎卡提方程（DARE）// 计算矩阵差分黎卡提方程，初始化P和QMatrix P = Q;uint num_iteration = 0;double diff = std::numeric_limits<double>::max();while (num_iteration++ < max_num_iteration && diff > tolerance) {Matrix P_next =AT * P * A -(AT * P * B + M) * (R + BT * P * B).inverse() * (BT * P * A + MT) + Q;// check the difference between P and P_next// 检查P和P_next之间的差异diff = fabs((P_next - P).maxCoeff());P = P_next;}if (num_iteration >= max_num_iteration) {// LQR求解器无法收敛到一个解决方案，最后一次连续结果差异是:ADEBUG << "LQR solver cannot converge to a solution, ""last consecutive result diff is: "<< diff;} else { // LQR求解器在迭代次数：时收敛，最大连续结果差异是：ADEBUG << "LQR solver converged at iteration: " << num_iteration<< ", max consecutive result diff.: " << diff;}*ptr_K = (R + BT * P * B).inverse() * (BT * P * A + MT);
}void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q,const Matrix &R, const double tolerance,const uint max_num_iteration, Matrix *ptr_K) {// 创建大小合适的零矩阵M：// M.rows() == Q.rows() && M.cols() == R.cols()Matrix M = Matrix::Zero(Q.rows(), R.cols());SolveLQRProblem(A, B, Q, R, M, tolerance, max_num_iteration, ptr_K);
}}  // namespace math
}  // namespace common
}  // namespace apollo

9 Box2d

在二维平面上, Box 特指矩形包围盒. 在 Planning 模块, 经常将自车和障碍物抽象成一个矩形框, 从而简化计算
在这里插入图片描述

box2d.h

#pragma once#include <limits>
#include <string>
#include <vector>#include "modules/common/math/aabox2d.h"
#include "modules/common/math/line_segment2d.h"
#include "modules/common/math/vec2d.h"namespace apollo {
namespace common {
namespace math {class Box2d {public:Box2d() = default;// 计算中心点、航向、长度和宽度Box2d(const Vec2d &center, const double heading, const double length,const double width);// 取轴上的点，前长，后长，航向和宽度Box2d(const Vec2d &point, double heading, double front_length,double back_length, double width);// 获取航向轴和框的宽度Box2d(const LineSegment2d &axis, const double width);// 接受一个AABox2d（轴对齐的框explicit Box2d(const AABox2d &aabox);// 从两个相对的角创建一个对齐的轴Box2dstatic Box2d CreateAABox(const Vec2d &one_corner,const Vec2d &opposite_corner);// 获取box的中心const Vec2d &center() const { return center_; }// 获取box中心的x坐标double center_x() const { return center_.x(); }// 获取box中心的y坐标double center_y() const { return center_.y(); }// 获取航向轴的长度double length() const { return length_; }// 获取box的宽度double width() const { return width_; }// 获取航向轴长度的一半double half_length() const { return half_length_; }// 获取宽度的一半double half_width() const { return half_width_; }// 获取航向角double heading() const { return heading_; }// 获取航向的余弦值double cos_heading() const { return cos_heading_; }// 获取航向的正弦值double sin_heading() const { return sin_heading_; }// 获取box的面积double area() const { return length_ * width_; }// 获取box的对角线长度double diagonal() const { return std::hypot(length_, width_); }// 获取box的所有端点void GetAllCorners(std::vector<Vec2d> *const corners) const;// 获取box的所有端点const std::vector<Vec2d> &GetAllCorners() const;// 测试点是否在box里bool IsPointIn(const Vec2d &point) const;// 测试点是否在box的边界上bool IsPointOnBoundary(const Vec2d &point) const;// 给定点到box的距离double DistanceTo(const Vec2d &point) const;// 确定框和给定线段之间的距离double DistanceTo(const LineSegment2d &line_segment) const;// 确定两个box之间的距离double DistanceTo(const Box2d &box) const;// 确定此框是否与给定的线段重叠，重叠返回Truebool HasOverlap(const LineSegment2d &line_segment) const;// 确定两个框是否有重叠bool HasOverlap(const Box2d &box) const;// 获取包含当前轴的最小轴对齐框AABox2d GetAABox() const;// 从中心旋转void RotateFromCenter(const double rotate_angle);// 按给定向量移动此框void Shift(const Vec2d &shift_vec);// 纵向延展boxvoid LongitudinalExtend(const double extension_length);// 横向延展boxvoid LateralExtend(const double extension_length);// Debugstd::string DebugString() const;// 初始化端点void InitCorners();double max_x() const { return max_x_; }double min_x() const { return min_x_; }double max_y() const { return max_y_; }double min_y() const { return min_y_; }private:// 用于检查给定的二维点 point 是否位于一个矩形区域内inline bool is_inside_rectangle(const Vec2d &point) const {return (point.x() >= 0.0 && point.x() <= width_ && point.y() >= 0.0 &&point.y() <= length_);}Vec2d center_;double length_ = 0.0;double width_ = 0.0;double half_length_ = 0.0;double half_width_ = 0.0;double heading_ = 0.0;double cos_heading_ = 1.0;double sin_heading_ = 0.0;std::vector<Vec2d> corners_;double max_x_ = std::numeric_limits<double>::lowest();double min_x_ = std::numeric_limits<double>::max();double max_y_ = std::numeric_limits<double>::lowest();double min_y_ = std::numeric_limits<double>::max();
};}  // namespace math
}  // namespace common
}  // namespace apollo

box2d.cc

#include "modules/common/math/box2d.h"#include <algorithm>
#include <cmath>
#include <utility>#include "absl/strings/str_cat.h"#include "cyber/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/polygon2d.h"namespace apollo {
namespace common {
namespace math {
namespace {
// 点到线段的最短距离
double PtSegDistance(double query_x, double query_y, double start_x,double start_y, double end_x, double end_y,double length) {const double x0 = query_x - start_x;const double y0 = query_y - start_y;const double dx = end_x - start_x;const double dy = end_y - start_y;// 计算点 query 在线段方向上的投影长度const double proj = x0 * dx + y0 * dy;if (proj <= 0.0) {return hypot(x0, y0);}if (proj >= length * length) {return hypot(x0 - dx, y0 - dy);}// 点到线段的叉积（即点在线段法向量上的投影长度），除以线段的长度 lengthreturn std::abs(x0 * dy - y0 * dx) / length;
}}  // namespace
// 初始化Box
Box2d::Box2d(const Vec2d &center, const double heading, const double length,const double width): center_(center),length_(length),width_(width),half_length_(length / 2.0),half_width_(width / 2.0),heading_(heading),cos_heading_(cos(heading)),sin_heading_(sin(heading)) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);// 初始化端点InitCorners();
}
// 初始化box
Box2d::Box2d(const Vec2d &point, double heading, double front_length,double back_length, double width): length_(front_length + back_length),width_(width),half_length_(length_ / 2.0),half_width_(width / 2.0),heading_(heading),cos_heading_(cos(heading)),sin_heading_(sin(heading)) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);double delta_length = (front_length - back_length) / 2.0;center_ = Vec2d(point.x() + cos_heading_ * delta_length,point.y() + sin_heading_ * delta_length);InitCorners();
}
// 初始化box
Box2d::Box2d(const LineSegment2d &axis, const double width): center_(axis.center()),length_(axis.length()),width_(width),half_length_(axis.length() / 2.0),half_width_(width / 2.0),heading_(axis.heading()),cos_heading_(axis.cos_heading()),sin_heading_(axis.sin_heading()) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);InitCorners();
}
// 这段代码主要用于初始化一个二维盒子的四个角点坐标，并计算盒子在 x 和 y 方向上的边界值
void Box2d::InitCorners() {const double dx1 = cos_heading_ * half_length_;const double dy1 = sin_heading_ * half_length_;const double dx2 = sin_heading_ * half_width_;const double dy2 = -cos_heading_ * half_width_;corners_.clear();corners_.emplace_back(center_.x() + dx1 + dx2, center_.y() + dy1 + dy2);corners_.emplace_back(center_.x() + dx1 - dx2, center_.y() + dy1 - dy2);corners_.emplace_back(center_.x() - dx1 - dx2, center_.y() - dy1 - dy2);corners_.emplace_back(center_.x() - dx1 + dx2, center_.y() - dy1 + dy2);for (auto &corner : corners_) {max_x_ = std::fmax(corner.x(), max_x_);min_x_ = std::fmin(corner.x(), min_x_);max_y_ = std::fmax(corner.y(), max_y_);min_y_ = std::fmin(corner.y(), min_y_);}
}
// 初始化aabox
Box2d::Box2d(const AABox2d &aabox): center_(aabox.center()),length_(aabox.length()),width_(aabox.width()),half_length_(aabox.half_length()),half_width_(aabox.half_width()),heading_(0.0),cos_heading_(1.0),sin_heading_(0.0) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);
}
// 创建aabox
Box2d Box2d::CreateAABox(const Vec2d &one_corner,const Vec2d &opposite_corner) {const double x1 = std::min(one_corner.x(), opposite_corner.x());const double x2 = std::max(one_corner.x(), opposite_corner.x());const double y1 = std::min(one_corner.y(), opposite_corner.y());const double y2 = std::max(one_corner.y(), opposite_corner.y());return Box2d({(x1 + x2) / 2.0, (y1 + y2) / 2.0}, 0.0, x2 - x1, y2 - y1);
}
// 获取所有端点
void Box2d::GetAllCorners(std::vector<Vec2d> *const corners) const {if (corners == nullptr) {return;}*corners = corners_;
}
// 获取所有端点
const std::vector<Vec2d> &Box2d::GetAllCorners() const { return corners_; }
// 判断点是否在box里
// 主要用于判断一个二维点是否在一个已知方向的盒子（或矩形）内部，是进行碰撞检测或位置判断时常用的函数
bool Box2d::IsPointIn(const Vec2d &point) const {// 计算点相对于盒子的位置const double x0 = point.x() - center_.x();const double y0 = point.y() - center_.y();// 计算点到盒子边界的距离const double dx = std::abs(x0 * cos_heading_ + y0 * sin_heading_);const double dy = std::abs(-x0 * sin_heading_ + y0 * cos_heading_);// 判断点是否在盒子内部return dx <= half_length_ + kMathEpsilon && dy <= half_width_ + kMathEpsilon;
}
// 判断点是否在边界上
// 用于检测一个点是否在盒子的边界上，用于碰撞检测或者区域判定的情况
bool Box2d::IsPointOnBoundary(const Vec2d &point) const {const double x0 = point.x() - center_.x();const double y0 = point.y() - center_.y();const double dx = std::abs(x0 * cos_heading_ + y0 * sin_heading_);const double dy = std::abs(x0 * sin_heading_ - y0 * cos_heading_);return (std::abs(dx - half_length_) <= kMathEpsilon &&dy <= half_width_ + kMathEpsilon) ||(std::abs(dy - half_width_) <= kMathEpsilon &&dx <= half_length_ + kMathEpsilon);
}
// 点到box的距离
// 计算一个点到一个盒子的最近距离，考虑了点在盒子内部、在边界上以及在盒子外部的不同情况
double Box2d::DistanceTo(const Vec2d &point) const {const double x0 = point.x() - center_.x();const double y0 = point.y() - center_.y();const double dx =std::abs(x0 * cos_heading_ + y0 * sin_heading_) - half_length_;const double dy =std::abs(x0 * sin_heading_ - y0 * cos_heading_) - half_width_;if (dx <= 0.0) {return std::max(0.0, dy);}if (dy <= 0.0) {return dx;}return hypot(dx, dy);
}
// 判断一个二维线段与一个盒子（或矩形）是否有重叠或相交的情况
bool Box2d::HasOverlap(const LineSegment2d &line_segment) const {// 如果线段长度小于等于 kMathEpsilon，则将其视为点，并检查该点是否在盒子内部if (line_segment.length() <= kMathEpsilon) {return IsPointIn(line_segment.start());}// 如果线段的端点都在盒子的外部，则没有重叠if (std::fmax(line_segment.start().x(), line_segment.end().x()) < min_x() ||std::fmin(line_segment.start().x(), line_segment.end().x()) > max_x() ||std::fmax(line_segment.start().y(), line_segment.end().y()) < min_y() ||std::fmin(line_segment.start().y(), line_segment.end().y()) > max_y()) {return false;}// 使用盒子的左下角作为坐标系原点构建新的坐标系，y 轴沿着盒子的方向Vec2d x_axis(sin_heading_, -cos_heading_);Vec2d y_axis(cos_heading_, sin_heading_);// 将线段起点映射到新的坐标系中Vec2d start_v = line_segment.start() - corners_[2];Vec2d start_point(start_v.InnerProd(x_axis), start_v.InnerProd(y_axis));// 如果映射后的起点在盒子内部，则线段与盒子有重叠if (is_inside_rectangle(start_point)) {return true;}// 如果映射后的终点在盒子内部，则线段与盒子有重叠Vec2d end_v = line_segment.end() - corners_[2];Vec2d end_point(end_v.InnerProd(x_axis), end_v.InnerProd(y_axis));if (is_inside_rectangle(end_point)) {return true;}// 排除线段两个端点在盒子同一侧的情况if ((start_point.x() < 0.0) && (end_point.x() < 0.0)) {return false;}if ((start_point.y() < 0.0) && (end_point.y() < 0.0)) {return false;}if ((start_point.x() > width_) && (end_point.x() > width_)) {return false;}if ((start_point.y() > length_) && (end_point.y() > length_)) {return false;}// 检查线段是否与盒子相交Vec2d line_direction = line_segment.end() - line_segment.start();Vec2d normal_vec(line_direction.y(), -line_direction.x());Vec2d p1 = center_ - line_segment.start();Vec2d diagonal_vec = center_ - corners_[0];// 如果投影长度小于对角线的投影长度，表示线段与盒子相交double project_p1 = fabs(p1.InnerProd(normal_vec));if (fabs(diagonal_vec.InnerProd(normal_vec)) >= project_p1) {return true;}diagonal_vec = center_ - corners_[1];if (fabs(diagonal_vec.InnerProd(normal_vec)) >= project_p1) {return true;}// 否则，线段与盒子没有重叠return false;
}
// 计算一个二维线段与一个盒子（或矩形）之间的距离
double Box2d::DistanceTo(const LineSegment2d &line_segment) const {// 如果线段长度小于等于 kMathEpsilon，则将其视为点，并计算点到盒子的距离if (line_segment.length() <= kMathEpsilon) {return DistanceTo(line_segment.start());}// 将线段起点相对于盒子中心进行坐标变换const double ref_x1 = line_segment.start().x() - center_.x();const double ref_y1 = line_segment.start().y() - center_.y();double x1 = ref_x1 * cos_heading_ + ref_y1 * sin_heading_;double y1 = ref_x1 * sin_heading_ - ref_y1 * cos_heading_;// 盒子的半长和半宽double box_x = half_length_;double box_y = half_width_;// 判断起点在盒子内的位置关系int gx1 = (x1 >= box_x ? 1 : (x1 <= -box_x ? -1 : 0));int gy1 = (y1 >= box_y ? 1 : (y1 <= -box_y ? -1 : 0));if (gx1 == 0 && gy1 == 0) {return 0.0;// 起点在盒子内部，距离为0}// 将线段终点相对于盒子中心进行坐标变换const double ref_x2 = line_segment.end().x() - center_.x();const double ref_y2 = line_segment.end().y() - center_.y();double x2 = ref_x2 * cos_heading_ + ref_y2 * sin_heading_;double y2 = ref_x2 * sin_heading_ - ref_y2 * cos_heading_;// 判断终点在盒子内的位置关系int gx2 = (x2 >= box_x ? 1 : (x2 <= -box_x ? -1 : 0));int gy2 = (y2 >= box_y ? 1 : (y2 <= -box_y ? -1 : 0));if (gx2 == 0 && gy2 == 0) {return 0.0;}// 调整坐标系，使得 x1 >= y1if (gx1 < 0 || (gx1 == 0 && gx2 < 0)) {x1 = -x1;gx1 = -gx1;x2 = -x2;gx2 = -gx2;}// 调整坐标系，使得 y1 >= y2if (gy1 < 0 || (gy1 == 0 && gy2 < 0)) {y1 = -y1;gy1 = -gy1;y2 = -y2;gy2 = -gy2;}// 根据相对位置判断不同情况下的距离计算if (gx1 < gy1 || (gx1 == gy1 && gx2 < gy2)) {std::swap(x1, y1);std::swap(gx1, gy1);std::swap(x2, y2);std::swap(gx2, gy2);std::swap(box_x, box_y);}// 根据不同的相对位置情况计算距离if (gx1 == 1 && gy1 == 1) {switch (gx2 * 3 + gy2) {case 4:return PtSegDistance(box_x, box_y, x1, y1, x2, y2,line_segment.length());case 3:return (x1 > x2) ? (x2 - box_x): PtSegDistance(box_x, box_y, x1, y1, x2, y2,line_segment.length());case 2:return (x1 > x2) ? PtSegDistance(box_x, -box_y, x1, y1, x2, y2,line_segment.length()): PtSegDistance(box_x, box_y, x1, y1, x2, y2,line_segment.length());case -1:return CrossProd({x1, y1}, {x2, y2}, {box_x, -box_y}) >= 0.0? 0.0: PtSegDistance(box_x, -box_y, x1, y1, x2, y2,line_segment.length());case -4:return CrossProd({x1, y1}, {x2, y2}, {box_x, -box_y}) <= 0.0? PtSegDistance(box_x, -box_y, x1, y1, x2, y2,line_segment.length()): (CrossProd({x1, y1}, {x2, y2}, {-box_x, box_y}) <= 0.0? 0.0: PtSegDistance(-box_x, box_y, x1, y1, x2, y2,line_segment.length()));}} else {switch (gx2 * 3 + gy2) {case 4:return (x1 < x2) ? (x1 - box_x): PtSegDistance(box_x, box_y, x1, y1, x2, y2,line_segment.length());case 3:return std::min(x1, x2) - box_x;case 1:case -2:return CrossProd({x1, y1}, {x2, y2}, {box_x, box_y}) <= 0.0? 0.0: PtSegDistance(box_x, box_y, x1, y1, x2, y2,line_segment.length());case -3:return 0.0;}}// 如果出现未实现的状态，则报错ACHECK(0) << "unimplemented state: " << gx1 << " " << gy1 << " " << gx2 << " "<< gy2;return 0.0;
}
// 计算当前盒子到另一个盒子的距离，利用多边形的距离函数
double Box2d::DistanceTo(const Box2d &box) const {return Polygon2d(box).DistanceTo(*this);
}
// 检查当前盒子与另一个盒子是否有重叠
bool Box2d::HasOverlap(const Box2d &box) const {// 如果两个盒子在x轴或y轴上不相交，直接返回falseif (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||box.min_y() > max_y()) {return false;}// 计算两个盒子中心的位移const double shift_x = box.center_x() - center_.x();const double shift_y = box.center_y() - center_.y();// 计算当前盒子在自身坐标系下的各个边向量const double dx1 = cos_heading_ * half_length_;const double dy1 = sin_heading_ * half_length_;const double dx2 = sin_heading_ * half_width_;const double dy2 = -cos_heading_ * half_width_;// 计算另一个盒子在其自身坐标系下的各个边向量const double dx3 = box.cos_heading() * box.half_length();const double dy3 = box.sin_heading() * box.half_length();const double dx4 = box.sin_heading() * box.half_width();const double dy4 = -box.cos_heading() * box.half_width();// 检查当前盒子在另一个盒子坐标系下的投影是否相交return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +half_length_ &&std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +half_width_ &&std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +box.half_length() &&std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +box.half_width();
}
// 获得AABox
AABox2d Box2d::GetAABox() const {const double dx1 = std::abs(cos_heading_ * half_length_);const double dy1 = std::abs(sin_heading_ * half_length_);const double dx2 = std::abs(sin_heading_ * half_width_);const double dy2 = std::abs(cos_heading_ * half_width_);return AABox2d(center_, (dx1 + dx2) * 2.0, (dy1 + dy2) * 2.0);
}
// 中心为原点自转rotate_angle
void Box2d::RotateFromCenter(const double rotate_angle) {heading_ = NormalizeAngle(heading_ + rotate_angle);cos_heading_ = std::cos(heading_);sin_heading_ = std::sin(heading_);InitCorners();
}
// 将盒子沿着指定向量移动
void Box2d::Shift(const Vec2d &shift_vec) {center_ += shift_vec;// 更新盒子中心位置for (size_t i = 0; i < 4; ++i) {corners_[i] += shift_vec;// 更新每个角点的位置}// 更新盒子的最大最小坐标for (auto &corner : corners_) {max_x_ = std::fmax(corner.x(), max_x_);// 更新最大x坐标min_x_ = std::fmin(corner.x(), min_x_);// 更新最小x坐标max_y_ = std::fmax(corner.y(), max_y_);// 更新最大y坐标min_y_ = std::fmin(corner.y(), min_y_);// 更新最小y坐标}
}
// 沿着盒子长度方向扩展盒子
void Box2d::LongitudinalExtend(const double extension_length) {length_ += extension_length;half_length_ += extension_length / 2.0;InitCorners();
}
// 沿着盒子宽度方向扩展盒子
void Box2d::LateralExtend(const double extension_length) {width_ += extension_length;half_width_ += extension_length / 2.0;InitCorners();
}
// 返回盒子的调试信息字符串
std::string Box2d::DebugString() const {return absl::StrCat("box2d ( center = ", center_.DebugString(),"  heading = ", heading_, "  length = ", length_,"  width = ", width_, " )");
}}  // namespace math
}  // namespace common
}  // namespace apollo

10 AABox2d

aabox2d.h

#pragma once#include <string>
#include <vector>#include "modules/common/math/vec2d.h"namespace apollo {
namespace common {
namespace math {// 在二维中实现了一类（无向）轴对齐的边界框
class AABox2d {public:AABox2d() = default;AABox2d(const Vec2d &center, const double length, const double width);// 从两个相对的角创建轴对齐的框AABox2d(const Vec2d &one_corner, const Vec2d &opposite_corner);// 给定向量中所有点的轴对齐框explicit AABox2d(const std::vector<Vec2d> &points);// 中心const Vec2d &center() const { return center_; }// 中心xdouble center_x() const { return center_.x(); }// 中心ydouble center_y() const { return center_.y(); }// box的长度double length() const { return length_; }// box的宽度double width() const { return width_; }// 半长double half_length() const { return half_length_; }// 半宽double half_width() const { return half_width_; }// 盒子的面积double area() const { return length_ * width_; }// 返回框的最小x坐标double min_x() const { return center_.x() - half_length_; }// 返回box的最大x坐标double max_x() const { return center_.x() + half_length_; }// 返回box的最小y坐标double min_y() const { return center_.y() - half_width_; }// 返回box的最大y坐标double max_y() const { return center_.y() + half_width_; }// 获取所有端点void GetAllCorners(std::vector<Vec2d> *const corners) const;// 判断点是否在box内bool IsPointIn(const Vec2d &point) const;// 判断点是否在边界上bool IsPointOnBoundary(const Vec2d &point) const;// 点到box的距离double DistanceTo(const Vec2d &point) const;// 两个Box的距离double DistanceTo(const AABox2d &box) const;// 两个box是否重合bool HasOverlap(const AABox2d &box) const;// 将AABox的中心移动输入向量void Shift(const Vec2d &shift_vec);// 更改框以包含另一个给定框以及当前框void MergeFrom(const AABox2d &other_box);// 更改框以包含给定点以及当前框void MergeFrom(const Vec2d &other_point);std::string DebugString() const;private:Vec2d center_;double length_ = 0.0;double width_ = 0.0;double half_length_ = 0.0;double half_width_ = 0.0;
};}  // namespace math
}  // namespace common
}  // namespace apollo

aabox2d.cc

#include "modules/common/math/aabox2d.h"#include <algorithm>
#include <cmath>#include "absl/strings/str_cat.h"
#include "cyber/common/log.h"#include "modules/common/math/math_utils.h"namespace apollo {
namespace common {
namespace math {
// 初始化AABox2d
AABox2d::AABox2d(const Vec2d &center, const double length, const double width): center_(center),length_(length),width_(width),half_length_(length / 2.0),half_width_(width / 2.0) {CHECK_GT(length_, -kMathEpsilon);CHECK_GT(width_, -kMathEpsilon);
}
// 根据两个端点初始化AAbox2d
AABox2d::AABox2d(const Vec2d &one_corner, const Vec2d &opposite_corner): AABox2d((one_corner + opposite_corner) / 2.0,std::abs(one_corner.x() - opposite_corner.x()),std::abs(one_corner.y() - opposite_corner.y())) {}
// 根据四个端点初始化aabox2d
AABox2d::AABox2d(const std::vector<Vec2d> &points) {ACHECK(!points.empty());double min_x = points[0].x();double max_x = points[0].x();double min_y = points[0].y();double max_y = points[0].y();for (const auto &point : points) {min_x = std::min(min_x, point.x());max_x = std::max(max_x, point.x());min_y = std::min(min_y, point.y());max_y = std::max(max_y, point.y());}center_ = {(min_x + max_x) / 2.0, (min_y + max_y) / 2.0};length_ = max_x - min_x;width_ = max_y - min_y;half_length_ = length_ / 2.0;half_width_ = width_ / 2.0;
}
// 获取所有端点
void AABox2d::GetAllCorners(std::vector<Vec2d> *const corners) const {CHECK_NOTNULL(corners)->clear();corners->reserve(4);corners->emplace_back(center_.x() + half_length_, center_.y() - half_width_);corners->emplace_back(center_.x() + half_length_, center_.y() + half_width_);corners->emplace_back(center_.x() - half_length_, center_.y() + half_width_);corners->emplace_back(center_.x() - half_length_, center_.y() - half_width_);
}
// 判断点是否在box里
bool AABox2d::IsPointIn(const Vec2d &point) const {return std::abs(point.x() - center_.x()) <= half_length_ + kMathEpsilon &&std::abs(point.y() - center_.y()) <= half_width_ + kMathEpsilon;
}
// 判断点是否在边界上
bool AABox2d::IsPointOnBoundary(const Vec2d &point) const {const double dx = std::abs(point.x() - center_.x());const double dy = std::abs(point.y() - center_.y());return (std::abs(dx - half_length_) <= kMathEpsilon &&dy <= half_width_ + kMathEpsilon) ||(std::abs(dy - half_width_) <= kMathEpsilon &&dx <= half_length_ + kMathEpsilon);
}
// 点到box的距离
double AABox2d::DistanceTo(const Vec2d &point) const {const double dx = std::abs(point.x() - center_.x()) - half_length_;const double dy = std::abs(point.y() - center_.y()) - half_width_;if (dx <= 0.0) {return std::max(0.0, dy);}if (dy <= 0.0) {return dx;}return hypot(dx, dy);
}
// 两个box之间的距离
double AABox2d::DistanceTo(const AABox2d &box) const {const double dx =std::abs(box.center_x() - center_.x()) - box.half_length() - half_length_;const double dy =std::abs(box.center_y() - center_.y()) - box.half_width() - half_width_;if (dx <= 0.0) {return std::max(0.0, dy);}if (dy <= 0.0) {return dx;}return hypot(dx, dy);
}
// 两个box是否重合
bool AABox2d::HasOverlap(const AABox2d &box) const {return std::abs(box.center_x() - center_.x()) <=box.half_length() + half_length_ &&std::abs(box.center_y() - center_.y()) <=box.half_width() + half_width_;
}void AABox2d::Shift(const Vec2d &shift_vec) { center_ += shift_vec; }
// 更改框以包含另一个给定框以及当前框
void AABox2d::MergeFrom(const AABox2d &other_box) {const double x1 = std::min(min_x(), other_box.min_x());const double x2 = std::max(max_x(), other_box.max_x());const double y1 = std::min(min_y(), other_box.min_y());const double y2 = std::max(max_y(), other_box.max_y());center_ = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0);length_ = x2 - x1;width_ = y2 - y1;half_length_ = length_ / 2.0;half_width_ = width_ / 2.0;
}
// 更改框以包含给定点以及当前框
void AABox2d::MergeFrom(const Vec2d &other_point) {const double x1 = std::min(min_x(), other_point.x());const double x2 = std::max(max_x(), other_point.x());const double y1 = std::min(min_y(), other_point.y());const double y2 = std::max(max_y(), other_point.y());center_ = Vec2d((x1 + x2) / 2.0, (y1 + y2) / 2.0);length_ = x2 - x1;width_ = y2 - y1;half_length_ = length_ / 2.0;half_width_ = width_ / 2.0;
}
// 调试
std::string AABox2d::DebugString() const {return absl::StrCat("aabox2d ( center = ", center_.DebugString(),"  length = ", length_, "  width = ", width_, " )");
}}  // namespace math
}  // namespace common
}  // namespace apollo

11 Path_match

path_matcher.h

#pragma once#include <utility>
#include <vector>#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"namespace apollo {
namespace common {
namespace math {
// 用于路径匹配和坐标转换
class PathMatcher {public:PathMatcher() = delete;// 用于从给定的参考路径 reference_line 中找到与给定点 (x, y) 最匹配的路径点 PathPoint。它返回一个 PathPoint 对象，表示最匹配的路径点static PathPoint MatchToPath(const std::vector<PathPoint>& reference_line,const double x, const double y);// 从给定的参考路径 reference_line 中获取点 (x, y) 的弗雷内坐标系（Frenet coordinates）static std::pair<double, double> GetPathFrenetCoordinate(const std::vector<PathPoint>& reference_line, const double x,const double y);// 用来从给定的参考路径 reference_line 中找到与给定纵向距离 s 最匹配的路径点 PathPointstatic PathPoint MatchToPath(const std::vector<PathPoint>& reference_line,const double s);private:// 在给定的路径段 (p0, p1) 上找到与点 (x, y) 最匹配的投影点 PathPointstatic PathPoint FindProjectionPoint(const PathPoint& p0, const PathPoint& p1,const double x, const double y);
};}  // namespace math
}  // namespace common
}  // namespace apollo

path_matcher.cc

#include "modules/common/math/path_matcher.h"#include <algorithm>
#include <cmath>
#include <vector>#include "glog/logging.h"#include "modules/common/math/linear_interpolation.h"namespace apollo {
namespace common {
namespace math {
// 用于从给定的参考路径 reference_line 中找到与给定点 (x, y) 最匹配的路径点 PathPoint。它返回一个 PathPoint 对象，表示最匹配的路径点
PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,const double x, const double y) {CHECK_GT(reference_line.size(), 0U);auto func_distance_square = [](const PathPoint& point, const double x,const double y) {double dx = point.x() - x;double dy = point.y() - y;return dx * dx + dy * dy;};double distance_min = func_distance_square(reference_line.front(), x, y);std::size_t index_min = 0;for (std::size_t i = 1; i < reference_line.size(); ++i) {double distance_temp = func_distance_square(reference_line[i], x, y);if (distance_temp < distance_min) {distance_min = distance_temp;index_min = i;}}std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;std::size_t index_end =(index_min + 1 == reference_line.size()) ? index_min : index_min + 1;if (index_start == index_end) {return reference_line[index_start];}return FindProjectionPoint(reference_line[index_start],reference_line[index_end], x, y);
}
// 从给定的参考路径 reference_line 中获取点 (x, y) 的弗雷内坐标系（Frenet coordinates）
std::pair<double, double> PathMatcher::GetPathFrenetCoordinate(const std::vector<PathPoint>& reference_line, const double x,const double y) {auto matched_path_point = MatchToPath(reference_line, x, y);double rtheta = matched_path_point.theta();double rx = matched_path_point.x();double ry = matched_path_point.y();double delta_x = x - rx;double delta_y = y - ry;double side = std::cos(rtheta) * delta_y - std::sin(rtheta) * delta_x;std::pair<double, double> relative_coordinate;relative_coordinate.first = matched_path_point.s();relative_coordinate.second =std::copysign(std::hypot(delta_x, delta_y), side);return relative_coordinate;
}
// 用来从给定的参考路径 reference_line 中找到与给定纵向距离 s 最匹配的路径点 PathPoint
PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,const double s) {auto comp = [](const PathPoint& point, const double s) {return point.s() < s;};auto it_lower =std::lower_bound(reference_line.begin(), reference_line.end(), s, comp);if (it_lower == reference_line.begin()) {return reference_line.front();} else if (it_lower == reference_line.end()) {return reference_line.back();}// interpolate between it_lower - 1 and it_lower// return interpolate(*(it_lower - 1), *it_lower, s);return InterpolateUsingLinearApproximation(*(it_lower - 1), *it_lower, s);
}
// 在给定的路径段 (p0, p1) 上找到与点 (x, y) 最匹配的投影点 PathPoint
PathPoint PathMatcher::FindProjectionPoint(const PathPoint& p0,const PathPoint& p1, const double x,const double y) {double v0x = x - p0.x();double v0y = y - p0.y();double v1x = p1.x() - p0.x();double v1y = p1.y() - p0.y();double v1_norm = std::sqrt(v1x * v1x + v1y * v1y);double dot = v0x * v1x + v0y * v1y;double delta_s = dot / v1_norm;return InterpolateUsingLinearApproximation(p0, p1, p0.s() + delta_s);
}}  // namespace math
}  // namespace common
}  // namespace apollo

12 polygon2d

polygon2d.h

#pragma once#include <string>
#include <vector>#include "modules/common/math/box2d.h"
#include "modules/common/math/line_segment2d.h"namespace apollo {
namespace common {
namespace math {class Polygon2d {public:Polygon2d() = default;// 盒子构建多边形explicit Polygon2d(const Box2d &box);// 点构建多边形explicit Polygon2d(std::vector<Vec2d> points);// 返回多边形的端点const std::vector<Vec2d> &points() const { return points_; }// 多边形的边const std::vector<LineSegment2d> &line_segments() const {return line_segments_;}// 多边形端点个数int num_points() const { return num_points_; }// 判断多边形是否凸bool is_convex() const { return is_convex_; }// 计算多边形面积double area() const { return area_; }// 点到多边形边界的距离double DistanceToBoundary(const Vec2d &point) const;/*** @brief Compute the distance from a point to the polygon. If the point is*        within the polygon, return 0. Otherwise, this distance is*        the minimal distance from the point to the edges of the polygon.* @param point The point to compute whose distance to the polygon.* @return The distance from the point to the polygon.*/double DistanceTo(const Vec2d &point) const;// 线段到多边形的距离double DistanceTo(const LineSegment2d &line_segment) const;// box到多边形的距离double DistanceTo(const Box2d &box) const;// 多边形到多边形的距离double DistanceTo(const Polygon2d &polygon) const;// 点到多边形距离的平方double DistanceSquareTo(const Vec2d &point) const;// 点是否在多边形里面bool IsPointIn(const Vec2d &point) const;// 点是否在多边形的边界上bool IsPointOnBoundary(const Vec2d &point) const;// 检查线段是否是多边形上的bool Contains(const LineSegment2d &line_segment) const;// 检查多边形是否包含另一个多边形bool Contains(const Polygon2d &polygon) const;// 计算一组点的凸包static bool ComputeConvexHull(const std::vector<Vec2d> &points,Polygon2d *const polygon);// 检查线段是否与此多边形重叠bool HasOverlap(const LineSegment2d &line_segment) const;// 获取线段和此多边形的重叠。如果他们有重叠，输出重叠线段的两端bool GetOverlap(const LineSegment2d &line_segment, Vec2d *const first,Vec2d *const last) const;// 获取多边形的所有点void GetAllVertices(std::vector<Vec2d> *const vertices) const;// 获取多边形的所有点std::vector<Vec2d> GetAllVertices() const;// 获取线段和此多边形的所有重叠线段// 如果此多边形不是凸的，则可能存在多条重叠的线段std::vector<LineSegment2d> GetAllOverlaps(const LineSegment2d &line_segment) const;// 检查此多边形是否与另一个多边形重叠bool HasOverlap(const Polygon2d &polygon) const;// 仅计算两个凸多边形之间的重叠bool ComputeOverlap(const Polygon2d &other_polygon,Polygon2d *const overlap_polygon) const;// 仅计算两个凸多边形之间的交集与并集比double ComputeIoU(const Polygon2d &other_polygon) const;// 设置多边形的轴对齐边界框AABox2d AABoundingBox() const;// 根据朝向获取boxBox2d BoundingBoxWithHeading(const double heading) const;// 获取box的最小面积Box2d MinAreaBoundingBox() const;// 沿航向获得极值点void ExtremePoints(const double heading, Vec2d *const first,Vec2d *const last) const;// 将此多边形展开一段距离。Polygon2d ExpandByDistance(const double distance) const;Polygon2d PolygonExpandByDistance(const double distance) const;// 计算端点void CalculateVertices(const Vec2d &shift_vec);// 调试字符串std::string DebugString() const;double min_x() const { return min_x_; }double max_x() const { return max_x_; }double min_y() const { return min_y_; }double max_y() const { return max_y_; }protected:void BuildFromPoints();int Next(int at) const;int Prev(int at) const;static bool ClipConvexHull(const LineSegment2d &line_segment,std::vector<Vec2d> *const points);std::vector<Vec2d> points_;int num_points_ = 0;std::vector<LineSegment2d> line_segments_;bool is_convex_ = false;double area_ = 0.0;double min_x_ = 0.0;double max_x_ = 0.0;double min_y_ = 0.0;double max_y_ = 0.0;
};}  // namespace math
}  // namespace common
}  // namespace apollo