0%

SLAM常见问题(四):求解ICP,利用SVD分解得到旋转矩阵

今天讲一篇关于利用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. 计算平移量: