今天讲一篇关于利用SVD
方法求解ICP
问题的文献《Least-Squares Rigid Motion Using SVD》,这篇文章非常精彩地推导出将$3D$点对齐问题的解析解,同时总结了求解该问题的统一范式。
问题描述
已知以及是空间中(文中说的更加普适,,可以表示$d$维空间)的匹配点集,我们试图找到这样的旋转矩阵$R$和平移向量$\mathbf{t}$最小化如下对齐误差(即ICP
问题的形式):
接下来文章分别推导了平移向量$\mathbf{t}$以及旋转矩阵$R$的解析解。
计算平移量
此时假定旋转矩阵$R$是固定的,令,我们可以通过$F$对$\mathbf{t}$求导的方式得到平移量的最优解,如下:
令:
于是我们得到$\mathbf{t}$的解:
从上式看出最优的平移量$\mathbf{t}$将$\mathcal{P}$点集的加权中心映射到了$\mathcal{Q}$点集的中心。接下来将上式带入优化方程,得:
由此我们将原问题转换成了无平移量的优化问题,令:
我们把问题简写成如下形式:
计算旋转量
简化上式:
又因为旋转矩阵的正交性:$R^TR=I$;另外$ \mathbf{x}_i^TR^T\mathbf{y}_i$是标量:$\mathbf{x}_i$维度为$1 \times d$,$R^T$维度为$d \times d$,$\mathbf{y}_i$维度为$d \times 1$。于是有下式:
得:
将整理好的上式带入简化后的$R$优化问题,得:
接下来将要利用到如下关于迹的技巧:
上式就是对的完美解释。
利用上式,式$(11)$可以整理得:
这里说明一下维度:$W = diag(w_1,w_2,…,w_n)$维度为$n \times n$,$Y^T$维度为$n \times d$,$R$维度为$d \times d$,$X$维度为$d \times n$。
接下来回顾一下迹的性质:$\operatorname{tr}(AB) = \operatorname{tr}(BA)$,因此有下式:
令$d\times d$的“covariance”矩阵$S = XWY^T$,求$S$的SVD
分解:
于是式$(13)$变为:
由于$V,T,R$均为正交矩阵,因此$M = V^TRU$也是正交阵,也就是说$M$的列向量$\mathbf{m}_j$是互相正交的单位向量,即$\mathbf{m}_j^T\mathbf{m}_j=1$,于是:
由于SVD
分解的性质可知$\sigma$的元素均为非负数:${\sigma}_1,{\sigma}_2,{\sigma}_d \geq 0$,于是式$(18)$变为如下形式:
可见,当迹最大时$m_{ii} = 1 $,又由于$M$是正交阵,这使得$M$为单位阵!
看到没,R的解析解竟然如此简单,并且与SVD
分解产生了联系,让人感觉到了数学的美妙。不过到这里还没完,后面作者进行了一步方向矫正,大意是这样的:利用公式$(18)$得到的矩阵并不一定是一个旋转矩阵,也可能为反射矩阵
,此时可以通过验证$VU^T$的行列式来判断到底是旋转(行列式 = 1)还是反射(行列式 = -1)。但我们要求的是旋转矩阵,这时需要对公式$(18)$进行一步处理。
假设$\operatorname{det}(VU^T) = -1$,则限制$R$为旋转就意味着$M = V^TRU $为反射矩阵
, 于是我们试图找到一个反射矩阵
$M$最大化下式:
即$f$是以为变量的线性函数,由于,其极大值肯定在其定义域的边界处。于是当时,$f$取得极大值,但是此时的$R$为反射矩阵
,所以并不能这样取值。然后我们看第二个极大值点$(1,1,…,-1)$,有:
这个值大于任何其它的自变量取值$(\pm 1,\pm 1,…,\pm 1)$的组合(除了$( 1, 1,…, 1)$),因为奇异值是经过排序的,${\sigma}_d$是最小的一个奇异值。
综上,为了将解转换为旋转矩阵要进行如下处理:
可以总结的套路
为了得到ICP
问题的最优解,我们可以采取如下套路:
step1. 计算两组匹配点的加权中心:
step2. 得到去中心化的点集:
step3. 计算$d \times d$的covariance矩阵:
其中,$X,Y$为$d \times n$的矩阵,$\mathbf{x}_i,\mathbf{y}_i$分别是它们的列元素,另外$W = diag(w_1,w_2,…,w_n)$。
step4. 对$S$进行SVD
分解$S = U\Sigma V^T$,得到旋转矩阵:
step5. 计算平移量: