今天面试的时候被问到了ICP的内容,想想之前自己做了那么多的工作,竟然忘记了,所以决定来重读一下。打算是以Q&A的形式来进行回顾,这样才能清楚的知道到底是哪儿不明白。
Q1:
ICP用的最多的是点云的匹配,一般都是PCL中的ICP算法直接进行实现了,那具体ICP的基本过程到底是怎么样的呢?
A1:
ICP算法包括以下步骤:
1) 根据点集P1中的点坐标,在曲面S上搜索相应最近点点集P2;这个搜索完全就是基于距离的搜索。后期在 此基础上进行kd-tree加速。
2) 计算两个点集的重心位置坐标,并进行点集中心化生成新的两个点集;
3) 由新的两个点集计算矩阵
W=∑ni=1qiq′Ti
W
=
∑
i
=
1
n
q
i
q
i
′
T
,并对这个矩阵进行分解为
W=U∑VT
W
=
U
∑
V
T
,
R=U∗VT
R
=
U
∗
V
T
.
4) 得到旋转矩阵和平移矩阵Rt,就可以计算点集P2进行刚体变换之后的新点集P3,由计算P2到P3的距离平方和,以连续两次距离平方和之差绝对值,作为是否收敛的依据。若小于阈值,就收敛,停止迭代。
5)重复a-e,直到收敛或达到既定的迭代次数。
Q2:
基本步骤里面,去中心化的目的是什么呢?
A2:
个人理解,因为在旋转的时候是根据质心旋转的,先去除质心是为了先计算出R,然后才能求出t,两步分开求解。(不知道这个理解对不对)
在一个群里面抛出这个问题之后,引起了一点争论:
观点一:去中心让函数更凸,更容易收敛。(我个人不是很明白这个意思)
因为:一个去中心化,不过是做了一个平移。何以做到让原本“绝大多数”的“凹的”东西更凸?
观点二:好像就是认同我的这个观点了。
Q3:
那么找到匹配点,去中心化之后,怎么计算两个点之间的旋转R和平移t呢?
A3:
假设找到
P={p1,p2,...,pn}
P
=
{
p
1
,
p
2
,
.
.
.
,
p
n
}
的匹配点
P′={p′1,p′2,...,p′n}
P
′
=
{
p
1
′
,
p
2
′
,
.
.
.
,
p
n
′
}
那么两者之间的关系就会用如下方程式表示:
然后根据所有的匹配点构造最小二乘问题,求使得误差平方和达到极小的 R,t R , t .
求解这个问题,分为三步:
- 计算两组点的质心位置
p,p′
p
,
p
′
,公式如下:
p=1n∑i=1n(pi) p = 1 n ∑ i = 1 n ( p i )
p′=1n∑i=1n(p′i) p ′ = 1 n ∑ i = 1 n ( p i ′ )
那么误差函数可以写成:
12∑i=1n||(pi−(Rp′i+t))||2=12∑i=1n||(pi−Rp′i−t−p+Rp′+p−Rp′))||2 1 2 ∑ i = 1 n | | ( p i − ( R p i ′ + t ) ) | | 2 = 1 2 ∑ i = 1 n | | ( p i − R p i ′ − t − p + R p ′ + p − R p ′ ) ) | | 2
最终化解之后得到如下目标函数:
minR,tJ=12∑i=1n||pi−p−R(p′i−p′))||2+||p−Rp′−t|| min R , t J = 1 2 ∑ i = 1 n | | p i − p − R ( p i ′ − p ′ ) ) | | 2 + | | p − R p ′ − t | |
上诉公式中的两部分,前面部分只与 R R 有关,后面部分和两个都相关,那么整体优化的话是不是可以转为先优化前面部分,然后再优化后面部分。 - 根据第一步求出的改为先优化前面部分,即如下:
- 再根据第二步求出的
R
R
,来算出.
t∗=p−Rp′ t ∗ = p − R p ′
Q4:
那么在Q3中的第二步,如何求解R呢?
A4:
解法一:
化解Q3中的第二步,得到如下公式:
其中的第一项和R无关,第二项由于 RRT=I R R T = I ,所以最后只剩下最后一项 ∑ni=1−qTiRq′i ∑ i = 1 n − q i T R q i ′ ,根据矩阵运算的关系可知:
接下来就是SVD求解R的过程了,先令上式中除R的部分未 W=∑ni=1q′iqTi W = ∑ i = 1 n q i ′ q i T , W W 是一个3*3的矩阵,对进行SVD分解,得到如下公式
解法二:
使用非线性优化的方法。以迭代的方式去找最优解。
目标函数写成:
这就转为了四元数表示的旋转和平移了。求解四元素的旋转与平移就可以使用求导操作,然后转为李代数的扰动模型来求,这样会更简化一些。
Q5:
ICP的优缺点是什么呢?
A5:
- 在较好的初值情况下,可以得到很好的算法收敛性。但是这也是一个缺点,就是初始值不好的话,结果也会受影响。
- 在找匹配点的时候,认为距离最近的点就是对应点,这点比较果断。CPD会在每个距离之前加上权值,所以相比ICP有一定的优化效果。
- ICP对于形状相似,但是角度偏差比较大的情况,匹配效果比较差。因为是基于最近点找的。CPD的话是全局的考虑,如果是局部和全局进行匹配的话,CPD不一定能取得比ICP更好的结果。