Apollo common math模块解读
[TOC]
本文是对 Apollo common math 模块进行研读的记录.
研读这个模块的意义在于 3 点:
- 了解哪些数学运算是较常用的, 可以针对性地学习在自己的代码里独立出来维护.
- 学习基本的几何/数学分析的算法以及编码技术, 毕竟比较轻量, 不用去看动辄几万行的数学库, 以及网上不一定靠谱的自学输出.
- Apollo 的其他模块依赖于这个模块, 有必要深入了解一下. 算法的性能与精度的瓶颈有可能就是卡在这个地方.
分类
- 平面几何算法
- 数值运算
- 常用工具函数
- 求解器
平面几何算法
vec2d
用途
定义一个二维向量及其方法
方法
- 构造函数(包括单位向量)
- 长度
- 正则化
- 内外积
- 标量积
- 加减法
- 向量间距离
- 与 x 轴正向夹角
- 旋转/自身旋转
- 判断相等
一些 tips:
std::hypot(): 计算欧式距离absl::StrCat(): 拼接字符串
LineSegment2d
用途
描述 2d 的直线线段及其方法
数据
起点/终点/单位向量, 角度, 长度
方法
- 构造函数
- 无参数: 单位向量
Vec2d(1, 0). - 始终点向量
- 无参数: 单位向量
- 获取单位向量
- 获取中点
- 围绕起点旋转某角度后的向量
- 获取 heading 的 cos/sin, 这些在构造函数里已经计算好了.
- 获取距离点最近(平方最近)的距离以及线上对应的点(仅包含投影在线段上的, 使用内积求解)
- 判断点是否在线上(外积)
- 判断两线是否相交, 以及交点
- 点在线上的投影
- 计算点向量与线段向量的 cross product
- 求点在线上的垂足点
一些学习点:
始终考虑到始终点足够接近的情况(一个点).
求解 2 条线相交点使用外积判断: 2 线段的各端点是否在线段 2 侧. 求交点的算法, 按我从几何角度的理解下面的是不是更好懂一些…
即从起点开始加上总坐标轴长度乘以系数, 系数可以由外积是平行四变形面积(正比与坐标轴长度, 分正负)的特性得来.1
2
3
4
5
6
7
8
9
10
11const double cc3 =
CrossProd(other_segment.start(), other_segment.end(), start_);
const double cc4 =
CrossProd(other_segment.start(), other_segment.end(), end_);
if (cc3 * cc4 >= -kMathEpsilon) {
return false;
}
const double ratio = -cc3 / (cc4 - cc3);
*point = Vec2d(start_.x() + (end_.x()-start_.x()) ratio,
start_.y() + (end_.y()-start_.y()) ratio);
return true;
Box2D
用途
定义一个矩形及其相关的方法, 并引出 AABox.
数据
矩形的中心点
长宽高,半长/宽
heading 及其 cos/sin(x 轴夹角, 逆时针为正)
四个角点以及 x/y 轴投影极值
方法
这里省略简单的 set/get 函数
构造函数3种
- 常规:中心点+长宽+heading
- 一个向量线段做中轴+宽度
- AABox
CreateAABox
IsPointIn
IsPointOnBoundary
DistanceTo点/线段/Box2D
HasOverlap线段/Box2D
RotateFromCenter中心旋转
Shift中心平移一个向量
LongitudinalExtend/LateralExtend横纵向拓展
学习点
矩形的构造使用中心点+类成员函数初始化四个角点的形式.好处是对外接口简单,任何方法只要看中心点即可.
使用坐标系转换的形式将点/向量与矩形的关系简化(从绝对坐标系转到矩形坐标系).
向量到矩形的距离使用了很多简化剪枝的思路值得借鉴.
上面提到的坐标系转换.
由于矩形有2个对称轴,把矩形外的区域划成8块部分,把几何性质与距离不变的区域转换到少数几个区域内.如下图.
再通过y=x直线将矩形/向量进一步旋转90度,合并一个区域.这时候向量起始点只剩下2个区域(黄色与粉色)需要考虑.
通过switch case的形式算出不同终点位置时候的距离.这里case的设计使用了2维数据压缩为1维的散列函数,即最大个数进制法(螺旋线计算的2维矩阵散列表也是使用的这种散列函数).
挺精妙的用法是利用外积可以判断向量之间旋转方向的正负来判断向量是否穿过矩形.
源码如下:1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93double Box2d::DistanceTo(const LineSegment2d &line_segment) const {
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;
}
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;
}
if (gx1 < 0 || (gx1 == 0 && gx2 < 0)) {
x1 = -x1;
gx1 = -gx1;
x2 = -x2;
gx2 = -gx2;
}
if (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;
}
两个Box2D是否重合先用AABox粗判断,AABox一旦重合使用分离轴定理判断是否有空隙(依旧利用坐标系旋转简化).
除了分离轴定理判断距离以外,还可以考虑构造超平面间距来计算,同样地上面判断向量到矩阵的距离也可以构造超平面来解决.
但是其剪枝的思想以及较高的执行效率还是值得学习的(毕竟里面都是随时计算结束的,而不是必须算完才清楚).
AABox2d
用途
用于障碍物检测等用处的特殊矩形.
数据
中心点,长宽,半长宽
方法
- 这里省略简单的set/get函数
- 构造函数
- default,初始为Origin点
- 中心点+长宽
- 对角2个点
- 四个点角点
- IsPointIn/IsPointOnBoundary
- DistanceTo点/AABox
- HasOverlap与AABox
- Shift一个vector
- MergeFrom与一个AABox/Point融合成一个新的AABox
Polygon2d
用途
描述2D多边形(不包括三角形).
数据
1 | std::vector<Vec2d> points_; //顶点 |
方法
省略get/set函数.
- 构造函数
1 | explicit Polygon2d(const Box2d &box);//通过box的四个角点构造,BuildFromPoints() |
通过顶点构造:BuildFromPoints(),要求点列逆时针排部,循环点列求三相邻点间构成的2个向量的外积的一半的和为多边形面积,面积为负的话,需要把点列反转过来.
同时两相邻点间构成边,通过循环点列求三相邻点间外积正负判断是否为凸.最后求得极值,得 AABBox.
- 包含
- IsPointOnBoundary点是否在边上.
1 | return std::any_of( |
- IsPointIn 点是否在内部
在边上返回true,否则运行下面的代码
1 | int j = num_points_ - 1; |
此处的算法使用的 W. Randolph Franklin 的算法.
对于多边形包含点的算法总结可以参考文章.
求线段与多边形的所有overlap:
std::vector<LineSegment2d> Polygon2d::GetAllOverlaps(const LineSegment2d &*line_segment*) const首先这里不假定多边形为凸(凸的情况下只会于一条线段最多相交于2点),图解如下,始终点分别为S与E点的线段与凹多边形相交于A,B,C,D四点.
如果输入线段为点,并且点在多边形内部返回该点(点也被视为线段的特殊情况).
通过求多边形所有边与线段的交点然后投影到线段本身上得到分段的各段长度(起点在多边形内部的化投影为0,终点在多边形内部的化投影为线段长度).如图中所示各段投影长度L.
然后sort所有投影长度,然后通过交点之间的中点是否在多边形内(BC间)进而判断这两个交点之间是否在多边形内部.
最后把所有在多边形内部的线段放入返回值(线段的vector).
Contains LineSegment2d:通过GetAllOverlaps函数得到所有线段与多边形相交的分段长度,其总和如果为线段长度则说明在多边形内部,否则说明不是.
Contains Polygon2d
如果输入多边形面积为0或者是输入多边形不是所有的顶点都在多边形内部的话,直接返回不不包含.
然后通过输入多边形的所有边是否在多边形内部最终判断包含与否.
1
2
3
4
5const auto &line_segments = polygon.line_segments();
return std::all_of(line_segments.begin(), line_segments.end(),
[&](const LineSegment2d &line_segment) {
return Contains(line_segment);
});
- 求距
- 点到边
double DistanceToBoundary(const Vec2d &point) const;
通过遍历点与每个边的距离,选最小值. - 点到整体
先判断 IsPointIn,是的话,距离为0,否则求点到边距离. - Polygon2d 到 Polygon2d
取所有边中到多边形距离最小值.
1 | double Polygon2d::DistanceTo(const Polygon2d &polygon) const { |
- 到Box2d
Box2d构造一个Polygon2d求距离.
计算包络凸多边形ComputeConvexHull.
检查输入点列个数大于3.
对输入点列进行排序,先按照x坐标大小排序,然后按照y坐标大小排序.
1
2
3
4
5
6
7
8
9
10std::sort(sorted_indices.begin(), sorted_indices.end(),
[&](const int idx1, const int idx2) {
const Vec2d &pt1 = points[idx1];
const Vec2d &pt2 = points[idx2];
const double dx = pt1.x() - pt2.x();
if (std::abs(dx) > kMathEpsilon) {
return dx < 0.0;
}
return pt1.y() < pt2.y();
});对排好序的点列依次进行三点间外积运算,把导致为凹的坏点滤除.
最后收尾相连用好点作为顶点得到凸多边形.
重叠overlap:
基本上是先通过AABbox的思路滤除明显的不合格,然后再通过求解距离的方式判断是否有重叠.判断的对象为LineSegment2d,Polygon2d,还会返回交点以及相交的面积(通过相交点构造出ComputeConvexHull凸多边形然后求面积的方式).ComputeIoU函数会返回重叠面积占原多边形面积的比例.
特化成特定类型的多边形
AABox2d:简单取x,y坐标值的极值即可.
BoundingBoxWithHeading:如下图下通过ExtremePoints成员函数得到heanding $u$以及其垂直方向上的$u^T$上的投影极值点(内积正负值),最后通过长度,宽度,heading角,以及中心点坐标得到Box2d.
1
2
3
4
5
6
7
8
9
10ExtremePoints(heading, &px1, &px2);
ExtremePoints(heading - M_PI_2, &py1, &py2);
const double x1 = px1.InnerProd(direction_vec);
const double x2 = px2.InnerProd(direction_vec);
const double y1 = py1.CrossProd(direction_vec);
const double y2 = py2.CrossProd(direction_vec);
return Box2d(
(x1 + x2) / 2.0 * direction_vec +
(y1 + y2) / 2.0 * Vec2d(direction_vec.y(), -direction_vec.x()),
heading, x2 - x1, y2 - y1);
MinAreaBoundingBox：找到对多边形最小面积的HeadingBound ingBox.
- 如果原多边形为凹,通过ComputeConvexHull函数计算其最小包络凸多边形得到其面积,返回该面积.
- 如果原多边形为凸,通过遍历所有边与所有顶点得到面积最小的边的heading,然后通过BoundingBoxWithHeading函数得到其Heading BoundingBox.
- 其中的数学原理,参考下图,对于最下面的边(方向为向量$\vec{BC}$),遍历所有顶点,得到所有顶点在其上的最大的投影($\vec{CD}$)最小的投影($\vec{AB}$),两者相减得到bounding box的一边$MN$.对于bounding box的另一个边$NP$,可以通过遍历各边向量与底边$\vec{BC}$单位向量$\vec{u_{BC}}$的外积的最大值得到(外积为两向量组成的平行四边形的面积,其中一边为单位1,则其高度等于面积).这里只是底边的bouding box,对于所有的边重复以上操作,得到最小面积的方向heading.
- 这里会有个疑问,为什么用多边形边的heading遍历一遍就可以,怎么保证最优性呢.对上面的图把底边旋转$\alpha$度(绕B点,这里只能顺时针)得到如下图橙色的bounding box.易观察得知在$0 \leq \alpha \leq \pi/2$的范围内旋转前后的bounding box的面积不变.而整个heading的空间搜索范围也只有$[0, \pi]$,这样就已经确定一半的空间了.而凸多边形所有边对某一固定坐标原点的旋转角度必定等于$2\pi$,也就是说另外半个$\pi/2$必然可以通过遍历所有边搜索到.这样就保证了最优性.(应该有更简洁的算法,这个算法的确冗余一些,另外Apollo里的代码更冗余,顶点遍历了两遍.
膨胀特定长度得到一个凸多边形ExpandByDistance:
- 如果原多边形为凹,先得到其凸边形包络在递归使用膨胀特定长度得到一个凸多边形ExpandByDistance.
- 如果原多边形为凸,则在每个边上对顶点沿边方向上延伸输入距离distance得到新的顶点,即可得到新的膨胀后的凸多边形.
欧拉角 EulerAnglesZXY
暂时不涉及到3D,暂不介绍.
AABoxKDTree2dNode
AABox的K-d树结构.规划中暂时不用,略过.
数值运算
数值积分integral
Simpson法则
输入分别为函数值vector,步长,步数.函数声明如下:
1 | double IntegrateBySimpson(const std::vector<double>& func, const double dx, const std::size_t nsteps) |
梯形法则
输入同上.函数声明如下:
1 | double IntegrateByTrapezoidal(const std::vector<double>& func, const double dx, const std::size_t nsteps) |
高斯勒让德法则
这里采用了模板函数的方式获取N点求值方式(而不是查表),即GetGaussLegendrePoints<N>().同时输入积分上下限会在函数里进行归一化.函数声明如下:
1 | template <std::size_t N> |
线性插值linear_interpolation
线性插值模板
1 | template <typename T> |
球面线性插值slerp
如下图,如果需要在圆/球上插值需要考虑到半径.
规划点的线性插值
- SLPoint
- PathPoint
插值系数为s.
其中theta是使用slerp插值,x/y/kappa/dkappa/ddkappa使用普通线性插值lerp. - TrajectoryPoint
插值系数为t.
其中theta/steer是使用slerp插值,v/a/x/y/kappa/dkappa/ddkappa/s使用普通线性插值lerp.
sin_table查表
对$[0,\pi/2]$也就是sin值$[0,1]$分段16385段,用来加速三角函数计算.
用在了Angle类的相关运算中.
1 |
|
矩阵运算 matrix_operations
求Moore–Penrose逆矩阵
分为方阵与非方阵.
- 方阵
1 | template <typename T, unsigned int N> |
使用Eigen::JacobiSVD库对输入的$n \times n$方阵$A$进行奇异值分解,其中对奇异值小于epsilon的按0处理,得到Moore–Penrose逆矩阵$A^+$.
- 非方阵
1 | template <typename T, unsigned int M, unsigned int N> |
对输入的$m \times n$矩阵$M$先构造方阵$MM^T$,然后对方阵$MM^T$进行进行奇异值分解得到其逆$B$,维度为$m \times m$,最后$M^+ = M^T B$得到Moore–Penrose逆矩阵.
对状态空间方程进行离散化
线性连续时间的状态空间方程如下:
$$
x˙(t) = Ax(t) + Bu(t) \\
y(t) = Cx(t) + Du(t)
$$
离散形式如下:
$$ \mathrm{x}((k+1) T)=\mathrm{A}_{d} \mathrm{x}(k T)+\mathrm{B}_{d} \mathrm{f}(k T) \\\\ \mathrm{y}(k T)=\mathrm{C}_{d} \mathrm{x}(k T)+\mathrm{D}_{d} \mathrm{f}(k T) $$ 积分可得: $$ \mathrm{x}(T)=e^{\mathrm{A} T} \mathrm{x}(0)+\int_{0}^{T} e^{\mathrm{A}(T-\tau)} \operatorname{Bf}(0) d \tau \\\\ =e^{\mathrm{A} T} \mathrm{x}(0)+e^{\mathrm{A} T} \int_{0}^{T} e^{-\mathrm{A} \tau} d \tau \mathrm{Bf}(0)=\Phi(T) x(0)+\int_{0}^{T} \Phi(T-\tau) d \tau \mathrm{Bf}(0) \\\\ \begin{aligned} &\mathbf{A}_{d}=e^{\mathbf{A} T}=\Phi(T) \\\\ &\mathbf{B}_{d}=e^{\mathbf{A} T} \int_{0}^{T} e^{-\mathbf{A} \tau} d \tau \mathbf{B}=\int_{0}^{T} e^{\mathbf{A}(T-\tau)} d \tau &\cdot \mathbf{B}=\int_{0}^{T} e^{\mathbf{A} \sigma} d \sigma \cdot \mathbf{B} \\\\ &\mathbf{C}_d = \mathbf{C} \\\\ &\mathbf{D}_d = \mathbf{D} \end{aligned} $$ 对矩阵指数函数$A_d$进行泰勒展开得到如下: $$ \mathbf{A}_{d}=e^{\mathbf{A} T}=\mathbf{I}+\mathbf{A} T+\frac{\mathbf{1}}{\mathbf{2 !}} \mathbf{A}^{2} T^{2}+\cdots=\sum_{i=0}^{\infty} \frac{\mathbf{1}}{i !} \mathbf{A}^{i} T^{i} \\\\ \mathbf{B}_{d}=e^{\mathbf{A} T}\left(-e^{-\mathbf{A} T} \mathbf{A}^{-1}+\mathbf{A}^{-1}\right) \mathbf{B}=\left(\mathbf{A}_{d}-\mathbf{I}\right) \mathbf{A}^{-1} \mathbf{B} $$对比公式推导,Apollo里的计算公式就没有那么直接(我暂时没有实战经验,所以还不理解这种算法,需要持续学习跟进).
$$
\begin{aligned}
&\mathbf{A}_d = (\mathbf{I}-\frac{1}{2}(t\mathbf{A}))^{-1}(\mathbf{I}+\frac{1}{2}(t\mathbf{A})) \\
&\mathbf{B}_d = t^{\frac{1}{2}}(\mathbf{I}-\frac{1}{2}(t\mathbf{A}))^{-1}\mathbf{B} \\
&\mathbf{C}_d = t^{\frac{1}{2}}\mathbf{C}(\mathbf{I}-\frac{1}{2}(t\mathbf{A}))^{-1} \\
&\mathbf{D}_d = \frac{1}{2} \mathbf{C} (\mathbf{I}-\frac{1}{2}(t\mathbf{A}))^{-1} \mathbf{B} + \mathbf{D}
\end{aligned}
$$
1 | template <typename T, unsigned int L, unsigned int N, unsigned int O> |
其他基础数学运算 math_utils
内外积CrossProd/InnerProd
常规实现,略过.
角度化简
- WrapAngle:$[0, 2 * \pi)$
- NormalizeAngle:
使角度约束在$[-\pi, \pi)$,这个计算很有必要否则例如计算两个角度之差时,本来旋转$-0.5\pi$就可以,实际计算值为$1.5\pi$,
导致绝对误差太大,无法满足约束/提高目标函数值导致优化计算失败.1
2
3
4
5
6
7double NormalizeAngle(const double angle) {
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
if (a < 0.0) {
a += (2.0 * M_PI);
}
return a - M_PI;
} - AngleDiff:角度差后NormalizeAngle
随机数
RandomInt, RandomDouble 位运算有点骚.
1 | double RandomDouble(const double s, const double t, unsigned int rand_seed) { |
高斯分布 Gaussian
基本公式描述, 略过.
RotateVector2d
常规实现, 略过.
笛卡尔转换为极坐标系 Cartesian2Polar
常规实现, 略过.
Frenet/笛卡尔坐标系相互转换 CartesianFrenetConverter
推导过程参考链接, 包括几何以及动力学.
常用工具函数
PathMatcher 找最近点
给定点列,找已知点距离点列最近的位置.如果最近的位置不在点列上,线性插值在点间.
使用欧式距离求解最近点.
分为笛卡尔坐标系点与Frenet坐标系点2中.
学习点:std::copysign()函数用于拷贝正负号,省略if-else.
curve_fitting 曲线拟合
目前只有多项式拟合 FitPolynomial.
1 | // Fit a Nth order polynomial using M 2d points. |
使用 Eigen 的广义逆矩阵来求解:
1 | Eigen::Matrix<double, N + 1, 1> t = PseudoInverse<double, N + 1, N + 1>(X.transpose() * X) * X.transpose() *Y; |
$$
X \cdot C = Y
$$
$X$ 为 x 坐标的 0-N 阶数, $C$ 为求解所得的系数向量, $Y$ 为 y 坐标向量.
误差为 $||\cdot||_2$, 存在 ptr_error_square 里.
GoldenSectionSearch 黄金分割搜索
黄金分割搜索的原理:
为了确定 $[x_1,x_3]$ 间单峰函数 $f(x)$ 的极值, 选定 $x_2$ 后的下一个迭代点 $x_4$, 因为不知道迭代点 2 侧那一侧是下一次迭代的选择. 因此需要保证每次迭代两侧是机会均等的. 可得如下式子:
$$
\frac{b}{a} = \frac{a}{c} \\
b = a + c \\
\phi = \frac{b}{a} \\
\phi = \frac{1+\sqrt{5}}{2} = 1.618033989
$$
黄金分割搜索与二分法的区别, 前者用来求解单峰函数的区间极值, 后者用来求解单调函数的零点. 计算复杂度自然黄金分割搜索更高.
1 | double GoldenSectionSearch(const std::function<double(double)> &func, |
参数分别为函数指针, 区间上下限, 误差精度.
HermiteSpline
Hermite Spline 是 Bezier Curve 的同义. 对于三阶, 输入为首($\boldsymbol{p}_0$)末($\boldsymbol{p}_1$)点, 及其切线向量($\boldsymbol{m}_0$, $\boldsymbol{m}_1$ 分别为首末点处的 tangent). 公式如下:
$$ \boldsymbol{p}(t)=\left(2 t^{3}-3 t^{2}+1\right) \boldsymbol{p}_{0}+\left(t^{3}-2 t^{2}+t\right) \boldsymbol{m}_{0}+\left(-2 t^{3}+3 t^{2}\right) \boldsymbol{p}_{1}+\left(t^{3}-t^{2}\right) \boldsymbol{m}_{1} $$ $t \in [0,1]$.Apollo 里仅支持 3 阶和 5 阶的计算.
1 | template <typename T, std::size_t N> |
参数解读: x0 为首末控制点, x1 为切线向量, z0, z1 为 knot 的区间, 默认为 $[0,1]$.
Evaluate 函数为在 z 处, 求得 order 阶(3/5)下的 HermiteSpline 插值.
求解器
QP通用求解器接口
QpSolver类,目前还没有被使用.
头文件如下:
1 | class QpSolver { |
卡尔曼滤波器 KalmanFilter
常规实现,略过.
LQR求解器
linear_quadratic_regulator:通过两个重载函数实现,后面学习后专门整理.
1 | /** |
MPC的OSQP求解器
MpcOsqp类.内容比较多后面单独整理.
单元测试
Apollo对math这种常用公用的依赖库进行了详细的单元测试.测试用例在每个模块后缀_test的.cc文件里. Apollo采用了Google Test(可以参考本人的博文介绍)的测试框架进行单元测试.
例如Vec2d的测试用例如下:
分为3个测试场景一般功能,旋转,自旋转.对每个场景选择了数个不同的case进行测试.
1 | TEST(Vec2dTest, NomralCases) { |
Apollo common math模块解读