求解问题
在进行激光 SLAM 时，在前端里程计时要面临的一个问题是：给定前后两帧点云（假设已经去畸变），我们如何求出载体（激光雷达）之间的相对位姿变换。为了解决这个问题有两种思路：
- 通过对两帧点云进行整体配准，求出相对位姿，根据这个思路又有两个方向：
- 基于 ICP 的方法
- 基于 NDT 的方法
- 参考视觉 SLAM 的思想，两帧图像中我们只需要提取一部分特征点，通过特征点之间的匹配也可以获得比较好的位姿估计结果，且更加节省计算时间，关于这个方向的工作主要集中在 LOAM 及其变种系列，可以参考我关于 LOAM 的学习笔记：LOAM 相关整理
本篇博客主要整理点云配准中，基于 ICP 的一系列方法。要求解的问题：给定两个激光点云，如何求解这两帧点云之间的相对位姿变化，从而使得这两帧点云之间的匹配关系最好。
点对点 ICP 匹配方法
先介绍 ICP 的方法，ICP(Itrative Cloest Point) 迭代最近点的方法的要点是，给定两个两个点云：
$$
\begin{aligned}
\boldsymbol{X} &= \{x_1, x_2, ..., x_N\}\\
\boldsymbol{P} &= \{p_1, p_2, ..., p_N\}
\end{aligned}
$$
我们需要找到一个相对位姿变化 $(R, t)$ 使得下式最小，这个过程叫点云配准，或者匹配点云。式子里面有旋转矩阵 $R$ 所以会引入三角量，因此不能用线性最小二乘解来求：
$$
E(R, t) = \frac{1}{N}\sum_{i=1}^{N}||x_i-Rp_i-t||^2
$$
要求这个问题，首先考虑到式子中耦合了旋转 $R$ 和平移 $t$，比较不好处理，如果我们将点云去中心化之后，相当于我们去除了点云在整体空间中的变化，而只关注它本身的形状，因此更容易计算出相对旋转。因此对两帧进行去中心化，如下所示：
先求出两个点云的几何中心
$$
\begin{aligned}
u_x &= \sum_{i=1}^{N}x_i\\
u_p &= \sum_{i=1}^{N}p_i
\end{aligned}
$$
此时，原来求解的问题可以引入中心点，在旋转的部分将中心点去掉（相当于去掉平移约束），并在平移的部分将中心加回来（引入平移约束）：
$$
\begin{aligned}
E(R, t) &= \frac{1}{N}\sum_{i=1}^{N}||(x_i - u_x)-R(p_i - u_y) + (u_x - Ru_y - t)||^2\\
&= \frac{1}{N}\sum_{i=1}^{N}\left(||(x_i - u_x)-R(p_i - u_y)||^2 + ||u_x - Ru_y - t||^2 + 2((x_i - u_x)-R(p_i - u_y))^T(u_x - Ru_y - t)\right)
\end{aligned}
$$
上式中，不难发现最后一项中，$\frac{1}{N}\sum_{i=1}^N((x_i - u_x)-R(p_i - u_y))$ 为零，因此最后一项可以消掉。通过这种方式，我们将 $R, t$ 在问题中分离，变成求解以下两部分：
$$
\begin{aligned}
R^* &= \arg\min_{R}E_1(R) = \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}||(x_i - u_x)-R(p_i - u_y)||^2\right)\\
t^* &= \arg\min_{t}E_2(t) = \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}||u_x - R^*u_y - t||^2\right)
\end{aligned}
$$
由于对 $t$ 的问题通过通过解析求解使 $E_2(t) = 0$ 因此，先求出最优的 $R$ 再求 $t$ 即可。因此，下面集中对 $E_1(R)$ 进行处理。
$$
\begin{aligned}
R^* &= \arg\min_{R}E_1(R) \\
&= \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}||(x_i - u_x)-R(p_i - u_y)||^2\right)\\
&= \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}||x'_i-Rp'_i ||^2\right)\\
&= \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}(x'_i-Rp'_i)^T(x'_i-Rp'_i)\right)\\
&= \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}({x'}_i^Tx'_i-{p'}_i^TR^Tx'_i-{x'}_i^TRp'_i + {p'}_i^TR^TRp'_i)\right)\\
&= \arg\min_{R}\left(\frac{1}{N}\sum_{i=1}^{N}({x'}_i^Tx'_i-2{p'}_i^TR^Tx'_i + {p'}_i^Tp'_i)\right)\\
&= \arg\max_{R}\left(\frac{1}{N}\sum_{i=1}^{N}{p'}_i^TR^Tx'_i\right)\\
&= \arg\max_{R}\left(\frac{1}{N}\sum_{i=1}^{N}{x'}_i^TRp'_i\right)\\
\end{aligned}
$$
通过展开并消除无关变量，最后得到了上述结果。接下来要求解这个问题，要引入矩阵迹的相关性质：
${p'}_i^TR^Tx'_i \in \mathbb{R}$，因此$\text{tr}({p'}_i^TR^Tx'_i) = {p'}_i^TR^Tx'_i$$\text{tr}(AB) = \sum_{i=1}^m\sum_{j=1}^n(a_{ij}b_{ji}) = \sum_{i=1}^m\sum_{j=1}^n(b_{ji}a_{ij}) = \text{tr}(BA)$
因此上式转换为：
$$
\begin{aligned}
R^* &= \arg\min_{R}E_1(R) \\
&= \arg\max_{R}\left(\frac{1}{N}\sum_{i=1}^{N}{x'}_i^TRp'_i\right)\\
&= \arg\max_{R}\left(\frac{1}{N}\sum_{i=1}^{N}\text{tr}({x'}_i^TRp'_i)\right)\\
&= \arg\max_{R}\left(\sum_{i=1}^{N}\text{tr}(Rp'_i{x'}_i^T)\right)
\end{aligned}
$$
令 $H = \sum(p'_i{x'}_i^T)$，则上式的累加符号可以结合进迹的求解中：
$$
\begin{aligned}
R^* &= \arg\min_{R}E_1(R) \\
&= \arg\max_{R}\left(\sum_{i=1}^{N}\text{tr}(Rp'_i{x'}_i^T)\right)\\
&= \arg\max_{R}\left(\text{tr}(RH)\right)
\end{aligned}
$$
这里引入一条引理(这里不证了)：对于任何正定矩阵 $AA^T$ 和 任何正交矩阵 $B$，满足以下不等式：
$$
\text{tr}(AA^T) \geq \text{tr}(BAA^T)
$$
因此，如果我们能将 $\text{tr}(RH)$ 转换为 $\text{tr}(AA^T)$ 则我们得到了其最大值，因为此时无论对 $R$ 做什么变化都不会超过 $\text{tr}(AA^T)$，由于通过 SVD 分解我们可以构建正交矩阵，因此下面对 $H$ 进行 SVD 分解
SVD 分解
$$
H = \sum_{i=1}^{N}x_i'p_i'^T = U
\begin{bmatrix}
\sigma_1 & 0 & 0 \\
0 & \sigma_2 & 0 \\
0 & 0 & \sigma_3
\end{bmatrix}V^T
$$
因此有：
$$
\text{tr}(RH) = \text{tr}(RU\Sigma V^T）
$$
其中，$U, V$ 都是正交矩阵，$\Sigma$ 是对角阵，因此令 $R = VU^T, A = V\Sigma^{1/2}$，则有：
$$
\text{tr}(RH) = \text{tr}(VU^TU\Sigma V^T）= \text{tr}(V\Sigma V^T) = \text{tr}(AA^T)
$$
可以获得最大值，总结一下，ICP 的解是：
$$
\begin{aligned}
H &= \sum(p'_i{x'}_i^T)\\
\text{svd}(H) &\rightarrow U, V \\
R &= VU^T\\
t &= u_x - Ru_p
\end{aligned}
$$
实际过程中，我们大概率并不知道对应点关系，这个时候不可以一步计算出 $R$ 和 $t$ 的最佳结果，需要用到期望最大化 (Expectation Maxmization, EM) 的思想，先给定一个初值（匹配关系），利用初值进行计算 R 和 t, 再根据 R 和 t 对点云进行转换，计算误差和新的匹配关系，根据新的匹配关系进项迭代计算，直至最后算出误差小于一个阈值，这个时候可以认为匹配关系和点云之间的转换关系都计算比较准确了，大概过程如下图所示：
点对线 （PL-ICP） 匹配方法
基本思想
相对于原始的 ICP 算法中用点与点之间的距离作为误差， PL-ICP 是把点到线的距离作为误差。PL-ICP 的基本思想是：激光点是对实际环境中曲面的离散采样，在匹配过程中重要的不是激光点，而是隐藏在激光点中的曲面。这个很好理解，因为在前后两帧激光扫描数据中，我们不一定会准确扫描到上一帧的每个点（这个时候用点和点的距离作为误差就是有问题的），更大可能是扫到了同一个物体（曲面）上的不同点，所以相较于用点对点的距离作为误差，把点到实际曲面的距离作为误差尺度应该是更符合实际的。所以问题就变成了如何恢复曲面。不同算法有不同的恢复方法，PL-ICP 的方法是用分段线性的方法对实际曲面进行近似，以此来定义点到曲面的距离，不同方法的误差表示如下图所示。
数学描述
这里论文中，将位姿用以下形式表示：
$$
\begin{aligned}
q &= (\mathbf{t}, \theta)\\
(\mathbf{p} \oplus (\mathbf{t}, \theta) &\triangleq \mathbf{R}(\theta)\mathbf{p} + \mathbf{t})
\end{aligned}
$$
我们可以将原始 ICP 算法的目标函数看成以下形式：
$$
\boldsymbol{q}* = \arg\min_{\mathbf{q}}\sum_{i}||\mathbf{p}_i \oplus \mathbf{q} - \prod\{S^{ref}, \mathbf{p}_i \oplus \mathbf{q}\}||^2
$$
其中，
$S^{ref}$表示和该点对应的线，具体实现中是从上一帧找到距离转换后的点最近的两个点，因此构建直线$\prod\{S^{ref}, \mathbf{p}_i \oplus \mathbf{q}\}$表示对转换后的点求该直线的投影，并用来和转换后的点进行比较。（在点对点 ICP 中，这一步是直接用距离最近的点作为比较点）。
然后给定初始解对下式进行迭代：
$$
\min_{q_{k+1}}\sum_{i}||\mathbf{p}_i \oplus \mathbf{q}_{k+1} - \prod\{S^{ref}, \mathbf{p}_i \oplus \mathbf{q}_{k}\}||^2
$$
在具体实现中，求点到线的距离是PL-ICP 目标函数如下，通过引入了法向量将误差从两个点的距离（差）变成了两点连线在曲面上的法向量的投影，下式中 $\boldsymbol{n}^T$ 为构建线段的法向量，如果 $\boldsymbol{q}$ 足够好，两点的连线应该和构建的线段是同一条线，因此和法向量的正交性也就越好，因此能够用来衡量 $\boldsymbol{q}$ 的精度：
$$
\min_{q_{k+1}}\sum_{i}(\mathbf{n}^T[\mathbf{p}_i \oplus \mathbf{q}_{k+1} - \prod\{S^{ref}, \mathbf{p}_i \oplus \mathbf{q}_{k}\}])^2
$$
求解过程 & 具体算法
我们的已知数据有：
- 参考激光帧
$y_{t-1}$ - 当前激光帧
$y_t$ - 参考激光帧生成的曲面
$S^{ref}$ - 初始解
$q_o$
需要求解的是两帧激光之间机器人的相对位姿关系 $q^*$
算法过程跟原始 ICP 类似：
- 将当前帧的数据根据初始位姿
$q_k$投影到参考帧坐标系下 - 对于当前帧的点
$i$， 在参考帧中找到最近的两个点（因为要连成直线）$(j_1, j_2)$ - 计算直线误差，去除误差过大的点
- 最小化误差函数
$\sum_{i}(\mathbf{n}^T[\mathbf{R}(\theta)\mathbf{p} + \mathbf{t} - p_{j_1^i}])^2$
跟 ICP 的区别
首先是误差形式不同，原始 ICP 将点对点的距离作为误差，PL-ICP 对点到线的距离作为误差；
其次两种算法收敛速度不同， ICP 为一阶收敛， PL-ICP 为二阶收敛，速度更快：
$$
\begin{aligned}
||q_{k} - q_{\infty}|| &< c ||q_{k-1} - q_{\infty}||\\
||q_{k} - q_{\infty}||^2 &< c ||q_{k-1} - q_{\infty}||^2
\end{aligned}
$$
PL-ICP 的求解精度更高（更符合实际），尤其是在人工结构化环境中（直线多）；
PL-ICP 对初值更敏感，通常要结合里程计一起使用
NICP 匹配方法
NICP 基本思想
相较于前两种 ICP 算法而言，他们都是考虑将距离（点到点，点到线）作为误差尺度， NICP 更关注面与面（2d 里是线与线）的误差；因此它还利用实际曲面的特征来对错误匹配点进行过滤，主要考虑的参数为法向量和曲率；因此在误差项里除了考虑对应点的欧氏距离以外，还考虑对应点法向量的角度差。
NICP 数学描述
首先有以下变量：
$p_i$ 表示激光点坐标 $(x, y, z)$
$n_i$ 表示对应点的法向量
$\sigma_i$ 对应点曲率
$\tilde{p}_i = (p_i, n_i)^T$ 为扩展点（坐标加法向量）
$T$ 表示欧式变换矩阵
$\oplus$ 对于 $\tilde{p}$ 的操作符（对于法向量只需要考虑旋转）：
$$
T\oplus \tilde{p}_i = \begin{bmatrix}Rp_i + t \\ Rn_i\end{bmatrix}
$$
则 NICP 的误差函数定义为：
$$
\mathbf{e}_{ij}(\mathbf{T}) = (\tilde{p}_i^c - \mathbf{T}\oplus \mathbf{\tilde{p}}_j^r)
$$
目标函数为：
$$
\sum_c\mathbf{e}_{ij}(\mathbf{T})^T\mathbf{\tilde{\Omega}}_{ij}\mathbf{e}_{ij}(\mathbf{T})
$$
其中，$\tilde{\Omega}_{ij}$ 为信息矩阵（表示权重）：
$$
\mathbf{\tilde{\Omega}}_{ij} = \begin{bmatrix}\mathbf{\Omega}_i^s & \mathbf{0} \\
\mathbf{0} & \mathbf{\Omega}_i^n\end{bmatrix}
$$
法向量和曲率的计算
先找到点 $p_i$ 周围半径 $R$ 范围内的所有点 $V_i$，然后计算均值和方差
$$
\begin{aligned}
\mu_i^s &= \frac{1}{|V_i|}\sum_{p_j\in V_i}p_j \\
\Sigma_i^s &= \frac{1}{|V_i|}\sum_{p_j \in V_i}(p_j - \mu_i)(p_j - \mu_i)^T\\
\end{aligned}
$$
然后对方差进行特征分解（2维情况下）,用类似于 PCA （主成分分析）的方法找出这堆点对应曲面的曲率和法向量：
$$
\Sigma_i^s = R\begin{bmatrix}\lambda_1 & 0 \\0 & \lambda_2\end{bmatrix}R^T
$$
对曲率的定义为： $\sigma_i = \frac{\lambda_1}{\lambda_1 + \lambda_2}$
对法向量的定义为：最小特征值对应的特征向量
对点进行过滤
如果没有 well-define 的法向量（杂点），拒绝匹配该点
两点间距离大于阈值，拒绝： $||p_i^c - T\oplus p_j^r|| > \epsilon_d$
两点曲率之差大于阈值，拒绝： $|\log{\sigma_i^c}-\log{\sigma_j^r}| > \epsilon_\sigma$
两点法向量角度之差，拒绝： $\mathbf{n}_i^c\mathbf{T}\oplus\mathbf{n}_j^r < \epsilon_n$
目标函数求解
目标函数为：
$$
\sum_c\mathbf{e}_{ij}(\mathbf{T})^T\mathbf{\tilde{\Omega}}_{ij}\mathbf{e}_{ij}(\mathbf{T})
$$
没办法求解析解，只能通过 LM 求解，关于非线性优化的具体求解过程可以参考：最小二乘问题和非线性优化。
NICP 总结
由于在寻找点匹配过程中考虑了法向量和曲率，可以提前排除一些错误的匹配；在开源领域是效果最好的匹配方法。
具体算法流程为：
计算参考激光帧和当前激光帧中每一个点的法向量和曲率（通过特征分解）。
根据当前解,把当前激光帧的点转换到参考坐标系中,并且根据欧式距离、法向量、曲率等信息来选择匹配点(也有可能没有匹配点)。
根据上面介绍的方法,用LM方法进行迭代求解,迭代收敛即可得到两帧激光数据之间的相对位姿。
IMLS-ICP 匹配方法 （Implicit Moving Least Square）
IMLS-ICP 基本思想
跟前面两种方法一样，还是考虑从点云重建曲面，重要的是怎么重建平面使其更符合实际； IMLS-ICP 的思想是选择具有代表性的激光点来进行匹配,既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移（在3d情况下尤其重要，因为点太多了）。
代表点选取
具有丰富特征的点,即为结构化的点:具有良好的曲率和法向量的定义。
曲率越小的点越好,因为曲率为0代表着直线,代表着最结构化的点,也代表着具有非常好的法向量定义,能够提供足够的约束。
选点的时候需要注意选取的激光点的均衡以保证可观性,因为是平面匹配,不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的,才能让结果不出现偏移。
曲面重建
已知点云集合 $P_k$ 中每一个点 $p_i$ 的法向量 $n_i$ , 则 $P_k$ 中隐藏的曲面为:
$$
I^{P_k}(x) = \frac{\sum_{p_i\in P_k}W_i(x)((x-p_i)^T\vec{n_i})}{\sum_{p_j\in P_k}W_j(x)}
$$
其中，
$$
W_i(x) = e^{-||x-p_i||^2/h^2}
$$
$I^{P_k}(x) = 0$ 对应的集合表示曲面，空间中点到曲面的距离代入方程计算即可，证明过程看参考文献 Provably good moving least square。
匹配求解
- 当前帧中一点
$x_i$到曲面的距离为$I^{P_k}(x_i)$ $P_k$中离点$x_i$最近点的法向量为$n_i$- 点
$x_i$在曲面的投影为：$y_i = x_i - I^{P_k}(x_i)n_i$ - 点
$x_i$和 点$y_i$为对应的匹配点，求解误差：$\sum((Rx_i + t - y_i)n_i)^2$