ICP(迭代最近点)算法推导详解

2022-04-21  本文已影响0人  瀚文文文问问

概述

ICP用来求解3D-3D的位姿估计问题。假设有一组配对好的3D点,例如对两幅RGB-D图像进行匹配,或者是EPnP方法中已知参考的在相机坐标系中的坐标和在世界坐标系的坐标(参考我PnP的博文):
P=\left\{ p_{1},...,p_{n} \right\} , P^{'}=\left\{ p^{'}_{1},...,p^{'}_{n} \right\}

现在想要找到R,t使得
\forall i , p_{i} = Rp^{'}_{i}+t

对应到EPnP问题的时候,就是已知一系列的点在相机坐标系下和世界坐标系下的坐标,求相机坐标系相对于世界坐标系的位姿R,t。
这个问题可以用迭代最近点(Iterative Closest Point, ICP)求解。这里可以看到在3D到3D的映射模型中并没有相机模型,也就是说,仅考虑两组3D点之间的变换时,和相机没有关系。因此在激光SLAM中也会使用ICP,不过由于激光数据特征不够丰富,我们无从知道两个点集之间的匹配关系,只能认为距离最近的两个点为同一个点,所以这种方法被成为迭代最近点。在视觉中,我们用ICP指代匹配好的两组点间的运动估计问题。

求解方法

SVD代数方法

SVD方法是通过构建最小二乘法进行求解。
首先定义第i对点的误差项:
e_{i}=p_{i}-(Rp^{'}_{i}+t) \ \ \ (1)

然后构建最小二乘问题,求使误差平方和达到极小的R,t:
min_{R,t}\frac{1}{2}\sum_{i=1}^{n}\left\|p_{i}-(Rp_{i}^{'}+t) \right\|_{2}^{2} \ \ (2)

下面进行求解的推导。
首先,定义两组点的质心:
p=\frac{1}{n}\sum_{i=1}^{n}(p_{i}) \ \ p^{'} =\frac{1}{n}\sum_{i=1}^{n}(p^{'}_{i}) \ \ (3)

注意质心是没有下标的。随后在误差函数中做如下处理:

(4)

上图中的等式应该不用解释,至于为什么这么做我在后面会说!
注意交叉项的部分可以进行化简:
\sum_{i=1}^{n}p_{i}-p-R(p_{i}^{'}-p^{'})=p_{1}+p_{2}+...+p_{n}-np+R(p^{'}_{1}+p^{'}_{2}+...+p^{'}_{n}-np^{'})

=np-np+R(np^{'}-np^{'})=0

根据上式交叉项为0,神奇不,真神奇,因此优化的目标函数可以简化为:
min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left\| p_{i}-p-R(p^{'}_{i}-p^{'}) \right\|^{2}+\left\|p-Rp^{'}-t \right\|^{2} \ (5)

我之前没有说(4)式为什么要那么处理,现在我给出我的理解,p_{i}-p以及p^{'}_{i}-p^{'}是去质心化的过程,这个过程将p_{i}p_{i}^{'}之间距离的衡量转化为每个点与各自质心的相对位置关系,其实相当于抹取了平移这一尺度,所以最终我们观察公式(5)可以发现左边只和旋转矩阵R相关,而右边既有R,又有t,但只和质心有关。只要我们获得了R,令第二项为零就能得到t
于是,ICP可以分为以下三个步骤求解

我们看到,只要求出了两组点之间的旋转,平移量是非常容易得到的。所以我们重点关注R的计算。展开R的误差项,得
\frac{1}{2}\sum_{i=1}^{n}\left\| q_{i} -Rq_{i}^{'} \right\|^{2}=\frac{1}{2}\sum_{i=1}^{n}(q^{T}_{i}q_{i}+(q_{i}^{'})^{T}R^{T}Rq_{i}^{'}-2q_{i}^{T}Rq_{i}^{'}) \ \ (6)

注意到第一项与R无关,第二项由于R^{T}R=I,亦与R无关。因此,实际上优化目标函数变为:

(7)
(7式中第一个等号,是将矩阵的二次型用迹表示,数学公式为x^{T}Ax=tr(Axx^{T})
为了解R,先定义矩阵:
W=\sum_{i=1}^{n}q_{i}(q_{i}^{'})^{T}

W是一个3x3的矩阵,对W进行SVD分解,得
W=U\sum V^{T}

解得R=\pm UV^{T}
上面这个SVD解很多人肯定有疑问,这个我在PnP算法详解(超详细公式推导)DLT手写推导处进行了推导,道理都是一样的,如果你不懂一定去看我的PnP文章,况且我感觉你学了ICP也得学PnP。

这里是两个解,选择R的行列式为正的解。解得R后求t即可。

非线性优化法

这个方法我还有些问题,等我弄明白再补上去

参考:
视觉SLAM十四讲第二版

上一篇下一篇

猜你喜欢

热点阅读