NICP算法介绍

NICP算法介绍

算法提出原文：NICP-Dense Normal Based Point Cloud Registration (from IEEE)

NICP是基于ICP算法的，由于ICP算法再点云的匹配关系上，是计算同一坐标系下的两帧点云上的点的距离最小作为比配点，这样的计算得到匹配关系，经常是错误的，会出现将不同位置(不同物体或者不同物体部分的扫描点)点进行匹配的情况，从而陷入局部最优。

激光雷达是将激光发射到环境表面，通过计算反射激光及发出激光的时间差得到机器人到环境表面的距离。因为可以通过点云得到环境表面的一些信息，比如是墙，还是弯曲的物体表面。通过对比点所在面的法向量，以及该点处的表面曲率，可以很大程度去筛除掉错误的点云匹配关系。

NICP在点云匹配上，除了约束点间的距离，另外加入了法向量及点云所在曲面的曲率的约束。

NICP迭代算法是基于ICP的，就是交叉迭代，即点云匹配与运动估计交叉迭代优化。如下图：

NICP的一次迭代计算主要包括：

1. 3D点云的位置p解算。

使用深度相机，得到环境的深度图，即每个点对应向量(u v d)。(u, v)点在图像中的像素坐标，d为点的深度信息。

使用激光雷达得到环境的点云分布情况，每个点对应向量(θ φ d)，激光雷达的FOV指的雷达发射的激光能够覆盖环境的水平和竖直方向的角度。θ代表点相对于雷达的水平角度(方位角)，φ表示点相对于雷达的竖直角度(高度角)，即雷达在(θ φ)方向扫描到点，距离为d。

但是无论是深度图像表示(u v d)，还是雷达扫描数据表示(θ φ d)，都不是点相对于机器人的位姿坐标向量，我们统称为测量值，因此需要将这些测量值中，恢复点云的坐标表示。

文中用来表示点的位姿坐标到测量值得变换，用来表示点的测量值到位姿坐标得变换。

2. 提取点所在环境表面处的特征(面的法向量及曲率)

通过点云计算处各点所在面的曲率及法向(面在该点处的曲率和法向量)。

---

文中有公式应该是有误解的地方：在计算协方差时矩阵时表述有**，如下：

应为：

以下为纠正解释：

如果采用方差(不是协方差)来计算各点处点云分布的离散情况，比如，一堵墙，这堵墙上扫描得到的点云只在这个墙所代表的平面上对应的两个轴方向上协方差比较大。而与墙垂直的方向轴上的协方差比较小。如下图所示，该簇点云在x，y轴上的协方差比较大，z轴上较小。

采用方差只能判定出点云集在x,y,z三轴上的扩散程度，不能得到曲面的法向量。因为实际曲面不是都是墙面那样的平面。因此文中采用协方差来计算曲率及法向量，采用协方差矩阵能够表示点集各轴之间的相关性，即通过计算协方差矩阵的特征值及对应的特征向量，则利用3个特征向量组成的坐标系可以更加清楚的描述点云集特征，最小特征值对应的特征向量，则就表示曲面的法向量。如下图所示。


以上图片来自LOAM论文和程序代码的解读
设，x，y为三维向量，$x={\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]}^T$, 其中心向量为$c={\left[\begin{array}{ccc}{c}_1& c_2& c_3\end{array}\right]}^T$
令：
$$x_c=x-c={\left[\begin{array}{ccc}{x}_{c1}& x_{c2}& x_{c3}\end{array}\right]}^T={\left[\begin{array}{ccc}{x}_1-c_1& x_2-c_2& x_3-c_3\end{array}\right]}^T$$
x的方差：$x_c^T{x}_c={\left[\begin{array}{ccc}{x_{c1}}^2& {x_{c2}}^2& {x_{c3}}^2\end{array}\right]}^T$
x与y协方差：$x{y}^T=\left[\begin{array}{ccc}{x}_1{y}_1& x_1{y}_2& x_1{y}_3\\ x_2{y}_1& x_2{y}_2& x_2{y}_3\\ x_3{y}_1& x_3{y}_2& x_3{y}_3\end{array}\right]$
x自身协方差：$x_c{x}_c^T$

---

文中协方差矩阵计算公式如下：
1）计算以点pi为中心的球域中所有点的重心:

2）计算该球域点集的协方差矩阵：

其中$V_i$为以点pi为中心的球域中所有点的集合.

协方差矩阵满足：

其中：
${\lambda }_1,{\lambda }_2,{\lambda }_3$按照升序排列，即${\lambda }_1<{\lambda }_2<{\lambda }_3$
${\lambda }_1,{\lambda }_2,{\lambda }_3$对应的特征向量为$n_1,n_2,n_3$
${\lambda }_1<{\lambda }_2<{\lambda }_3$表示该点云在$n_1$方向上比较集中，$n_2,n_3$上相对发散。

得：pi点处曲面法向量为，曲面的曲率可以表示为：

3. 点云匹配计算

以下几种情况，删除匹配关系：

1） 如果没有well define的法向量；
2） 两点间的距离大于阈值：

3） 两点处所在面的曲率之差距大于阈值：

4）两点处所在面的的法向量角度之差大于阈值：

向量点乘值越大，表明两个向量的夹角越小，$a\bullet b=\left|a\right|\left|b\right|cos\theta$, $\theta$为两向量夹角。

相比ICP的距离最小匹配，增加了对点所在曲面的曲率及法向量的约束，筛除更多的错误匹配点。

4. 运动估计

相比ICP的约束：

NICP的运动估计也加入了曲面法向量的约束。

令：⊕用来表示位姿+法向量的坐标变换过程，r代表原帧，c代表匹配帧。


得误差向量：

定义二乘正定形式：(相当于单维最小二乘法)：

其中，${\tilde{\Omega }}_{ij}$是6*6得信息矩阵，相当于优化中得惩罚性，通过它可以调整我们对不同元素偏差的重视程度。

论文中使用阻尼高斯牛顿来误差完成迭代最小化：
迭代步：$T\leftarrow \Delta T$⊕$T$

$\Delta T$求解：

其中：H为hessian阵，高斯牛顿法采用e函数的jacobine矩阵拟合H阵，详细解释见后端优化介绍内容。加$\lambda I$的并不是前边的特征根，是hessian的补偿项，保证hessian阵的正定性，因为求$\Delta T$要对H求逆，因此H不能等于0。

$J_{ij}$就是$e$函数的jacobine阵。

$[p{]}_X$是向量的叉乘阵，对应高翔博士说的符号∧，就是通过自我叉乘得到一个反对称矩阵，对应如下：