本文最后更新于2020年4月18日，已超过 1 年没更新！

前端配准是实现匹配两帧点云数据，从而得到传感器(激光雷达)前后的位姿差，其是激光SLAM中重要的一环，对SLAM的性能有很大的影响。
前端配准在视觉SLAM里为Tracking，在激SLAM里便是帧间匹配。
(注：帧间匹配并不一定是进行前后两帧的匹配)

帧间匹配算法

1. ICP匹配方法

见运动畸变去除

2. PL-ICP匹配方法

2.1 示意图

2.2 数学描述

目标函数：

$\min\limits_{q_k+1}\sum({n^T_i}\lbrack p_i{\oplus}q_{k+1}-\prod \lbrace S^{ref},p_i{\oplus}q_k \rbrace \rbrack)^2$

$q=(t,\theta)$

$(p{\oplus}(t,\theta) \triangleq R(\theta)p+t)$

其中， $n_i$ 为法向量， $\prod\lbrace S^{ref},\cdot \rbrace$ 表示在 $S^{ref}$ 的投影。
注： $p_i{\oplus}q_k$ 为上一帧的位姿， $\prod \lbrace S^{ref},p_i{\oplus}q_k \rbrace \rbrack$ 为上一帧的位姿转换在前面的投影，也即参考帧。 $p_i{\oplus}q_{k+1}$ 为当前帧； $n^T_i$ 为点 $p_{j^i_1}$ 和点 $p_{j^i_2}$ 所连直线的法向量；
目标函数的物理意义便是点 $p_i^{\omiga}$ 到直线 $\overline{p_{j^i_1}p_{j^i_2}}$ 的距离，也即PL(Point-Line)。

2.3 算法流程

把当前帧的数据根据初始位姿投影到参考帧坐标系下；
对于当前帧的点i，在参考帧中找到最近的两个点；

计算误差，并去除误差过大的点；
最小化误差函数: $\sum_i({n^T_i}[R(\theta_{k+1})p_i+t_{k+1}-p_{j^i_1}])^2$ 。

2.4 与ICP算法区别

误差函数的形式不同，ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差；
收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛；
PL-ICP的求解精度高于ICP，更能反映实际情况。
PL-ICP对初始值更敏感。

3. 基于优化的匹配方法

Hector-SLAM是典型的纯基于优化的SLAM算法，实现了高斯牛顿方法进行求解；Cartographer-SLAM算法里是在CSM后加上优化算法，使用ceres直接进行求解。

3.1 示意图

梯度下降示意图

3.2 数学描述

给定一个目标函数，把激光的帧间匹配问题转换为求解目标函数的极值问题：

$E(T) = arg \min\limits_T \sum[1-M(S^i(T))]^2$

$T=(T_x,T_y,T_\theta)$

其中， $S_i(T)$ 表示把激光数据用位姿 $T$ 进行转换（转换道地图坐标系下）； $M(x)$ 表示的到坐标的地图占用概率。

由于似然场和地图都已经栅格化，是离散的，无法求梯度，因此要对地图进行插值。
因此现在主要需要解决的有两个问题：

怎么求解目标函数？
怎么对地图进行插值？

3.3 优化方法的求解

$E(T) = arg \min\limits_T \sum \left[ 1-M(S^i(T)) \right]^2$

$p_i = (p_{ix},p_{iy})$ 表示第i个激光点的坐标；

$s_i(T)= \begin{bmatrix} cosT_\theta & -sinT_\theta & T_x\\ sinT_\theta & cosT_\theta & T_y\\ 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} p_{ix}\\ p_{iy}\\ 1\\ \end{bmatrix}$

为非线性函数，进行一阶泰勒展开，可得：

$E(T+\Delta{T}) = arg \min\limits_{\Delta{T}} \sum \left[ 1-M(S_i(T))-\nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \Delta{T} \right]^2$

注： $\frac{\partial{M(S^i(T))}}{\partial{T}} = \frac{\partial{M(S^i(T))}}{\partial{S^i(T)}} \cdot \frac{\partial{S_i(T)}}{\partial{T}} = \nabla{M(S_i(T))} \cdot \frac{\partial{S_i(T)}}{\partial{T}}$
$\nabla{M(S_i(T))}$ 即为地图的梯度，暂时还求不出来。

对于线性系统，求其对 $\Detla{T}$ 的导数，并令其等于0：

$\sum \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]^{T} \left[ 1-M(S_i(T))-\nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \Delta{T} \right] = 0$

求解上式，即可得到 $\Delta{T}$ 。
之后，令 $T=T+\Delta{T}$ ，不断进行迭代计算，等到收敛时，便可得到机器人位姿。（注：此也即为高斯-牛顿迭代法的全部过程）

继续进行求解，对上式展开移项得：

$\sum \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]^{T} \left[ 1-M(S_i(T)) \right] =$

$\sum \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]^{T} \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \Delta{T} \right]$

令：

$H = \sum \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]^{T} \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]$

等式两边同时乘以 $H^{-1}$ ，得：

$\Delta{T} = H^{-1} \sum \left[ \nabla{M(S_i(T))} \frac{\partial{S_i(T)}}{\partial{T}} \right]^{T} \left[ 1-M(S_i(T)) \right]$

（这是实质很简单，只是公式比较复杂。）

现在已经求出了 $\Delta{T}$ 得表达式，但是式子里得 $\nabla{M(S_i(T))}$ ， $\frac{\partial{S_i(T)}}{\partial{T}}$ 这两项，目前我们不知道，因此以下我们就进行这两项得求解。

$s_i(T)= \begin{bmatrix} cosT_\theta & -sinT_\theta & T_x\\ sinT_\theta & cosT_\theta & T_y\\ 0 & 0 & 1\\ \end{bmatrix} \begin{bmatrix} p_{ix}\\ p_{iy}\\ 1\\ \end{bmatrix} = \begin{bmatrix} cosT_{\theta} \cdot p_{ix} - sinT_{\theta} \cdot p_{iy} + T_x\\ sinT_{\theta} \cdot p_{ix} + cosT_{\theta} \cdot p_{iy} + T_y\\ 1\\ \end{bmatrix}$

$\frac{\partial{S_i(T)}}{\partial{T}} = \begin{bmatrix} \frac{\partial{S_i(T)}}{\partial{T_x}} & \frac{\partial{S_i(T)}}{\partial{T_y}} & \frac{\partial{S_i(T)}}{\partial{T_\theta}}\\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & cosT_{\theta} \cdot p_{ix} - sinT_{\theta} \cdot p_{iy}\\ 0 & 1 & sinT_{\theta} \cdot p_{ix} + cosT_{\theta} \cdot p_{iy}\\ \end{bmatrix}$

其中， $S_i(T)$ 表示地图坐标点； $\nabla{M(S_i(T))}$ 表示地图的梯度。
现在， $\Delta{T}$ 表达式中除了 $\nabla{M(S_i(T))}$ 之外，所有量都已知道。
$\nabla{M(S_i(T))}$ 的求解需要对地图进行插值。

3.4 地图双线性插值

拉格朗日插值法
X,Y两个方向进行插值
一维线性插值的推广

3.5 拉格朗日插值方法

3.5.1 一维线性插值

插值的定义：
设函数 $y=f(x)$ 在区间上有定义，且存在已知点：

$a \le x_0 < x_1 < \cdot \cdot \cdot < x_n \le b$

处的函数值 $y_i = f(x_i)$ ，若存在次多项式：

$L_n(x) = a_0 + a_1x + a_2x^2 + \cdot \cdot \cdot + a_nx^n$

使得值成立，则称L_n(x)为f(x)的插值多项式。
可以证明：存在且唯一。

拉格朗日插值法：
把插值多项式表示为基函数的线性组合：

$L_n(x) = \sum \limits^n_{i=0} l_i(x)y_i$

基函数满足以下条件：

$L_i(x_k) = \left\lbrace \begin{array}{ccc} 1 & k = i\\ 0 & k \ne i\\ \end{array} \right$

3.5.2 基函数构造

基函数 $l_i(x)$ 在除𝑥𝑖以外的所有插值点都为0，即点 $x_0,\cdot \cdot \cdot ,x_{i-1}, x_{i+1}, \cdot \cdot \cdot, x{n}$ 都是的解，因此可以构造函数:

$l_i(x) = c(x - x_0) \cdot \cdot \cdot (x - x_{i-1})(x - x_{i+1})(x - x_n)$

显然 $l_i(x)$ 满足上述条件。同时，可得：

$l_i(x_i) = c(x_i - x_0) \cdot \cdot \cdot (x_i - x_{i-1})(x_i - x_{i+1})(x_i - x_n) = 1$

因此：

$c = \frac{1}{(x_i - x_0) \cdot \cdot \cdot (x_i - x_{i-1})(x_i - x_{i+1})(x_i - x_n)}$

代入构造函数化简可得基函数为：

$l_i(x) = \prod \limits^n_{k=0,k \ne i} \frac{x-x_k}{x_i-x_k}$

这就是拉格朗日插值法在一维线性情况下的进行插值的方法。

3.5.3 双线性插值

现在，我们将拉格朗日法推广到二维，也即地图插值。

设平面中有四个点:

$Z_1 = f(x_0,y_0), \quad Z_2 = f(x_1,y_0)$

$Z_3 = f(x_1,y_1), \quad Z_2 = f(x_0,y_1)$

令：

$u=\frac{x-x_0}{x_1-x_0} \qquad v=\frac{y-y_0}{y_1-y_0}$

则对应的四个点的坐标变为：

$(x_0,y_0) = (0,0) \quad (x_1,y_0) = (1,0)$

$(x_1,y_1) = (1,1) \quad (x_0,y_1) = (0,1)$

构造四个基函数：

$l_1(u,v) = (1-u)(1-v)$

$l_2(u,v) = u(1-v)$

$l_3(u,v) = uv$

$l_4(u,v) = (1-u)$

插值函数为：

$L_4(u,v) = Z_1l_1(u,v) + Z_2l_2(u,v) + Z_3l_3(u,v) + Z_4l_4(u,v)$

上式中把， $(u,v)$ 替换回可得：

$\begin{split} L(x,y) & = \frac{y-y_0}{y_1-y_0}(\frac{x-x_0}{x_1-x_0} M(P_{11})+ \frac{x_1-x}{x_1-x_0} M(P_{01})) \\ & + \frac{y_1-y}{y_1-y_0}(\frac{x-x_0}{x_1-x_0} M(P_{10}) + \frac{x_1-x}{x_1-x_0} M(P_{00})) \end{split}$

对的偏导数为:

$\frac{\partial{L(x,y)}}{\partial{x}} = \frac{y-y_0}{y_1-y_0}(M(P_{11}) - M(P_{01})) + \frac{y_1-y}{y_1-y_0}(M(P_{10}) - M(P_{00}))$

对的偏导数为:

$\frac{\partial{L(x,y)}}{\partial{y}} = \frac{x-x_0}{x_1-x_0}(M(P_{11}) - M(P_{10})) + \frac{x_1-x}{x_1-x_0}(M(P_{01}) - M(P_{00}))$

此插值方法对初始值敏感，但是精度很高。因此在实际算法中用到的比较多。在Hector-SLAM和Cartographer-SLAM中都有应用。

4. 相关匹配方法及分枝定界加速

4.1 帧间匹配似然场

似然场示意图

高度非凸，存在很多的局部极值
对初值非常敏感
进行暴力匹配，排除初值影响
通过加速策略，降低计算量
计算位姿匹配方差（非常重要）

4.2 算法流程

构造似然场；
在指定的搜索空间内，进行搜索，计算每一个位姿的得分（最优得分）；
根据步骤2中位姿的得分，计算本次位姿匹配的方差。

问题的主要在搜索的过程，需要通过一些方法技巧对搜索进行加速。
那么，怎么对搜索进行加速呢？

4.3 位姿搜索

4.3.1 暴力搜索

三层𝑓𝑜𝑟循环 $(x,y,\theta)$ 枚举每一个位姿，分别计算每一个位姿的得分，计算量巨大。因为激光雷达数据在每一个位姿都要重新投影（投影到世界坐标系），投影需要计算sin和cos函数。
此方法每一个位姿都需要进行一次投影，每次投影都需要计算两次三角函数，因此非常耗时间。

4.3.2 预先投影搜索

把暴力搜索中的三层𝑓𝑜𝑟循环 $(x,y,\theta)$ 交换一下顺序，最外层对𝜃进行搜索，这样内层𝑥,𝑦的投影变成的加法，需要计算sin和cos函数的位姿数从 $n_x n_y n_{\theta}$ 降为 $n_{\theta}$ 。
此方法能极大的加速算法的运行速度。但是由于还是要每个位姿都进行投影，因此速度还是很慢。

4.3.3 多分辨率搜索

构造粗分辨率(25𝑐𝑚)和细分辨率(2.5𝑐𝑚)两个似然场；
首先在粗分辨率似然场上进行搜索，获取最优位姿；
把粗分辨率最优位姿对应的栅格进行细分辨率划分，然后再进行细分辨率搜索，再次得到最优位姿；
粗分辨率地图的栅格的似然值为对应的细分辨率地图对应空间的所有栅格的最大值。

此方法使得搜索的范围小了很多，采用这种方法能够在计算机甚至上GHz的Arm芯片上可以实时运行。
注：粗分辨率的位置得分时细分辨率位置得分的上界，这样可以保证不错过最大值，但是有可能会错过最优值。在实际中，使用此方法即使的得到的解不是最优，但也能保证是次优，因此在实际情况下此方法的效果还是不错的。若在其之后再进行优化，效果会更好。

4.4 分枝定界算法

常用的树形搜索剪枝算法；
求解整数规划问题；
解的数量为有限个；（很像栅格搜索）
把最优解求解问题转换为树形搜索问题，根节点表示整个解空间，叶子节点表示具体的解，中间的节点表示解空间的某一部分子空间。

4.4.1 分枝

即根节点表示整个解空间空间，深度为1的节点表示解空间的子空间，深度为2的节点表示深度1空间的子空间，这样层层划分，直到划分到真实解，也就是叶子节点为止。

4.4.2 定界

对于搜索树种的每一个节点，确定以该节点为根节点的子树的界。对于最小值问题，确定下界；对于最大值问题，确定上界。(SLAM中为上界）

4.4.3 分枝定界算法流程

4.5 分枝定界在相关方法的加速作用

以Cartographer为例：
搜索树中的节点表示一个正方形的搜索范围: $(c_x,c_y,c_{\theta},c_h)$

$\overline{\overline{W_c}} = (\lbrace (j_x,j_y) \in Z^2 : \begin{array}{ccc} c_x \leq j_x \leq c_x + 2^{c_h}\\ c_y \leq j_y \leq c_y + 2^{c_h}\\ \end{array} \rbrace \times \lbrace c_{\theta} \rbrace )$

分枝：
对于节点 $(c_x,c_y,c_{\theta},c_h)$ 分枝为4个子节点，四个子节点为：

$C_c = ((\lbrace c_x,c_x+2^{c_{h-1}} \rbrace \times \lbrace c_y,c_y+2^{c_{h-1}} \rbrace \times c_{\theta}) \cap \overline{W}) \times \lbrace c_h - 1 \rbrace$

定界：
构造得分函数，使得得分函数对于节点I的打分，是以节点I为根节点的子树的上界：

$\begin{split} score(c) = \sum \limits^K_{k=1} \max \limits_{j \in \overline{\overline{W_c}}} M_{nearest} (T_{\xi_{j}} h_k) & \geq \sum \limits^K_{k=1} \max \limits_{j \in \overline{W_c}} M_{nearest} (T_{\xi_{j}} h_k) \\ & \geq \max \limits_{j \in \overline{\overline{W_c}}} \sum \limits^K_{k=1} M_{nearest} (T_{\xi_{j}} h_k) \end{split}$

$\overline{W_c} = \overline{\overline{W_c}} \cap \overline{W}$

其中， $\sum \limits^K_{k=1} M_{nearest} (T_{\xi_{j}} h_k)$ 是最细分辨率得分。

4.5.1 分枝定界加速流程

下图为分枝定界在激光SLAM前端匹配中加速的全过程。

用了分枝定界的方程和树形搜索的方法使得整个搜索的过程加快了许多，速度相比多分辨率算法速度快了大约6倍，同时性质也没有弱化，并且可以保证解为最优解。

5. 总结

以上的四种方法都有开源实现的案例：

ICP在PCL中有实现；
PL-ICP可以直接二进制安装CSM；
优化方法在HectorSLAM和Cartographer中有实现；
相关方法在Cartographer中有实现。

みんなようこそ!

[激光SLAM理论]激光的前端配准方法

帧间匹配算法

1. ICP匹配方法