利用SVD的方法求解ICP（详细推导）

引用资料（理论部分其实就是把第一个的不详细和错误的地方说了一下，翻译了一下第二个文献以及不明了的地方说一下O(∩_∩)O哈哈~）：

高翔《视觉SLAM十四讲》
K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN
Least-Squares Fitting of Two 3-D Point Sets

问题描述

  假设存在两个点云集合{pi}和{p′}
  求：一个欧氏变换R,t使得
    

∀i,pi=Rp′i+t

求解问题

解：
假设误差项为
  

ei=pi−(Rp′i+t)

那么问题转化为优化问题：
  

minR,tJ=12∑i=1n∥(pi−(Rp′i+t))∥2

定义质心为：

p=1n∑i=1n(pi),p′=1n∑i=1n(p′i)

那么有：

12∑i=1n∥pi−(Rp′i+t)∥=12∑i=1n∥pi−Rp′i−t−p+Rp′+p−Rp′∥2

 

=12∑i=1n∥(pi−p−R(p′i−p′))+(p−Rp′−t)∥2

=12∑i=1n(∥pi−p−R(p′i−p′)∥2+∥p−Rp′−t∥2+2(pi−p−R(p′i−p′)(p−Rp′−t))

因为

∑i=1n(pi−p−R(p′i−p′)(p−Rp′−t)=0

所以问题转化为：

minR,tJ=12∑i=1n∥pi−p−R(p′i−p′)∥2+∥p−Rp′−t∥2

因为左右两边都大于等于零，而且左边只和R相关，可以先求出R在利用R求解第二项

那么按照书里计算过程

>
1. 计算两组质心位置p,p’,然后计算每个点的去质心坐标：

qi=pi−p,q′i=p′i−p′

2.根据以下优化问题计算旋转矩阵：

R∗=argminR12∑∥qi−Rq′i∥2

3.根据2的结果计算t

t∗=p−Rp′

展开关于R的误差项有：

12∑∥qi−Rq′i∥2=12∑qTiqi+q′TiRTRq′i−2qTiRq′i

因为第一项与R无管，第二项由于RTR=I与R也无关那么问题转化为

∑i=1n−qTiRq′i=∑i=1n−tr(Rq′iqTi)=−tr(R∑i=1nq′iqTi)

令

H=∑i=1nq′iqTi

因为问题是求解

minR.−tr(RH)

即：

maxR.tr(RH)

假设最优解R∗
那么

tr(R∗H)≥tr(RH)=tr(BR∗H)

（因为R是正交矩阵）
对H进行SVD分解

H=UΣVT

可以得到

R∗=VUT

那么

R∗H=VUTUΣVT=VΣVT

令

A=VΣ12

因为

tr(R∗H)=tr(AAT)≥tr(BAAT),(BBT=I)

所以

R∗=VUT

是

maxR.tr(RH)

最优解

现在只要证明

tr(AAT)≥tr(BAAT),(BBT=I)

ai是A的第i列，因为tr(AB)=tr(BA)那么有

tr(BAAT)=tr(ATBA)=∑ati(Bai)

根据Schwarz不等式

ati(Bai)≤(atiai)(atiBtBai)−−−−−−−−−−−−−√=atiai

即

tr(BAAT)=tr(ATBA)≤∑atiai=tr(AAT)

注意这个计算需要H是满秩，

几个情况需要考虑
1.H是满秩，{p′}上的点非共平面
2.{p′}上的点共平面，可以对H求出的解的为0特征值的特征向量计算取反，使得det|H|=1
3.{p′}上的点共线，不能用SVD求解

代码

有空传吧

如果您觉得此博客对您有用，欢迎对我进行小额赞助。

不筹钱娶媳妇的程序员不是好程序员！