VICP：速度更新的迭代最近点算法

VICP：Velociy Updating Iterative Closest Point Algorithm

ICP算法

ICP算法在计算过程中会给定两次扫描的点云结果以及一个初始的位姿，如果这个初始位姿给的不够接近真实值，那么ICP的估计结果就会陷入局部最优，因此实际上我们会采用不断迭代的方法来避免这种情况。

这里定义$X=x_i$为一帧扫描的点云，$Y=y_i$为另一帧扫描的点云，$x_i$和$y_i$分别为其中的第$i$个点，这里我们希望求得从$X$点云到$Y$点云的旋转矩阵$R$和平移向量$p$，那么有：

我们会重复上述过程，即求得变换$T_i$，然后将$y_i$通过$T_i$进行更新，然后再重新进行最近匹配，再求得$T_{i+1}$，而每个$T_i$可以通过SVD算法来求解。详细过程如下：

示意图为：

最终我们将得到$Y$到$X$的变换矩阵$T$，则

至于为什么上面的过程可以通过SVD算法来求解，…

VICP

A. Scan Distortion 扫描畸变

对于像激光雷达这样的rangefinder的传感器，是通过雷达的旋转来离散地获得每个角度的点的距离。那么不同角度点的距离的获得就有时差，若传感器本身处在不可忽略的运动当中，那么测得点的距离就存在畸变。

（a）的灰色线是实际的环境，并且机器人在往上运动。假设机器人中激光雷达的不同角度的测量是在$\Delta t$的时间差之后瞬时得到的，那么（b）中的红色线就是得到的测量值。但是实际上并不是这样，而是机器人在不断往上运动，雷达在不断逆时针旋转测量，这样得到的是蓝色线。可以看到，蓝色右侧是偏远的，因为这是最开始测得的，而左边则是比较近的，贴近红色线，因为这是快运动结束时测得的。（c）是灰色线和蓝色线的对比。（d）是蓝色线矫正之后得到红色线，红色线与灰色线的对比。

B. Velocity Estimation and Data Compensation 速度估计和数据补偿

假设$X_i$和$X_{i-1}$分别是$i$时刻和$i-1$时刻测得的点云，两者的时间差为$\Delta t$，并且两者对应的位姿分别为$T_i$和$T_{i-1}$（从机器人坐标系到世界坐标系）。那么两者的相对位姿变换为$T_{i-1}^{-1}{T}_i$，则有：

为了便于计算，我们假设机器人是匀速运动的，那么可以求得前后帧的平均速度$V_i$为：

【注意这个速度是从后往前的反速度】

由于矩阵不能直接用来进行平均计算，因此这里利用对数匀速将变换矩阵转化为其对应的李代数的反对称矩阵，该反对称矩阵可以直接进行平均计算。最后求出的平均速度$V_i$也是李代数的反对称矩阵，因此还得转化一下才能变成变换矩阵。

后面的计算将分为forward compensation和backward compensation两种。所谓forward指的是时间$t_i$是第$i$次测量的起始时刻，那么就需要把第$i$测量的点利用平均速度往后矫正；而backward指的是视时间$t_i$是第$i$测量的终止时间，因此需要把测量点往前矫正。

可以看到两图中的蓝色线是一样的，但是红色线是不同的，因为它们的时间点不同，前者是测量起始时间，后者是测量终止时间，因此看到前者离物体远些，后者离物体进些。（a）是forward compensation，由于采用测量起始时间作为时间点，因此可以看到刚开始测量时比较准，逆时针旋转后慢慢变得不准；而(b)图刚好相反。

这个图是我自己画的，可以非常直观地辨析两种不同的补偿方式，详细的解释看后面。

forward compensation

同一次测量中相邻点的间隔时间 $\Delta t_s=\Delta t/n$

那么第$i$次测量中第$j$个点的实际位姿为：

利用反速度将测量起始时间后的点往后拨，把它们都统一为测量起始时间：

但是这么做有个问题，就是这里的平均速度实际上跟要补偿的点的实际速度有比较大的延迟，延迟刚好是两侧测量的时间间隔，从上面那个我自己画的图中可以看出，求解平均速度的红色线并没有覆盖我们要补偿的点（绿色线）。

因此为了更好地矫正，我们也可以采用backward compensation。

backward compensation

在backward compensation中每个点的实际位姿为：

因此，应该把点往前推进：

这里从我上面那个图中也可以看容易看懂，所以不加解释。

C. Accelerating Convergence Speed

下面给出利用平均速度进行补偿后的ICP算法：【这里应该是backward compesation】

这个算法与ICP算法的差异在于，每次ICP算法收敛到一个新的变换矩阵$T$后，会求出一个新的平均速度$V_i$，利用这个新的平均速度来重新对测量点进行补偿，然后重新进行新的ICP算法，知道平均速度的变化不大。
【这个伪代码里边似乎有一些小错误，这里就不仔细说了】

虽然这里由于速度补偿更新使得算法时间复杂度增加了不少，但是由于速度补偿更新会加速ICP的收敛，所以实际的耗时并没有很多。

并且，我们可以采用外点剔除（outlier rejection）的方法来加速收敛。每次利用速度补偿更新测量点之后，把测量距离超过传感器测量范围的点给剔除，从而加速算法收敛。