（ICP-SVD）Least-Squares Fitting of Two 3-D Point Sets

摘要

两个点集 { p i } \{p_i\} {pi​}和 { p i ′ } \{p_i^{'}\} {pi′​};$i=1,2,...,N$由$p_i^{{}^{\prime }}=R{p}_i+T+N_i$所关联，这里$R$是旋转矩阵，$T$是平移向量，$N_i$是噪声向量。给定 { p i } \{p_i\} {pi​}和 { p i ′ } \{p_i^{'}\} {pi′​}，我们提出了一种算法来找到$R$和$T$的最小二乘解，这个方法基于$3\times 3$矩阵的奇异值分解(SVD)。在计算时间方面，将此算法与两种较早提出的算法进行了比较。

引言

在许多计算机视觉的应用，尤其是使用3-D点对的对应关系[1]来估计刚体的运动参数以及刚体相对于参考坐标系的位姿[2]中，我们遇到了下面的数学问题。我们给定两个3-D点集 { p i } \{p_i\} {pi​}和 { p i ′ } \{p_i^{'}\} {pi′​};$i=1,2,...,N$(这里$p_i$和$p_i^{{}^{\prime }}$是视为$3\times 1$的列矩阵)
$$p_i^{{}^{\prime }}=R{p}_i+T+N_i\text{\hspace{1em}(1)}$$
这里$R$是$3\times 3$的旋转矩阵，$T$是平移向量($3\times 1$的列矩阵)，$N_i$是噪声向量(我们假设旋转是沿着一个通过原点的旋转轴进行的)。我们想要找到一个$R$和$T$来最小化
$${\Sigma }^2=\sum _{i=1}^N||p_i-(R{p}_i+T)|{|}^2\text{\hspace{1em}(2)}$$
Huang,Blostein和Margerum[3]提出了一种用于寻找解的迭代算法，Faugeras和Hebert提出了一种基于四元数的非迭代算法。在本文中，我们提出了一种新的非迭代算法，该算法涉及到$3\times 3$矩阵的奇异值分解(SVD)。我们比较了三种算法的计算时间。

解耦平移和旋转

根据[3]中所述，如果公式(1)的最小二乘解为$\hat{R}$和$\hat{T}$，则 { p i ′ } \{p_i^{'}\} {pi′​}和 { p i ′ ′ ≡ R ^ p i + T ^ } \{p_i^{''} \equiv \hat{R}p_i +\hat{T}\} {pi′′​≡R^pi​+T^}有着相同的质心，也就是说
$$p^{{}^{\prime }}=p^{{}^{\prime \prime }}.\text{\hspace{1em}(3)}$$
这里
$$p^{{}^{\prime }}=\frac{1}{N}\sum _{i=1}^N{p}_i^{{}^{\prime }}.\text{\hspace{1em}(4)}$$

$$p^{{}^{\prime \prime }}=\frac{1}{N}\sum _{i=1}^N{p}_i^{{}^{\prime \prime }}=\hat{R}p+\hat{T}\text{\hspace{1em}(5)}$$

$$p=\frac{1}{N}\sum _{i=1}^N{p}_i$$

令
$$q_i=p_i-p\text{\hspace{1em}(7)}$$

$$q_i^{{}^{\prime }}=p_i^{{}^{\prime }}-p^{{}^{\prime }}\text{\hspace{1em}(8)}$$

我们有
$$\sum _{i=1}^N||q_i^{{}^{\prime }}-R{q}_i|{|}^2\text{\hspace{1em}(9)}$$

因此，原始的最小二乘问题可以被分为两部分
(i) 寻找$\hat{R}$来最小化公式(9)中的${\Sigma }^2$
(ii) 之后平移向量可以根据以下公式计算得到
$$\hat{T}=p^{{}^{\prime }}-\hat{R}p\text{\hspace{1em}(10)}$$
在下一部分，我们将会描述一种使用奇异值分解来求解第i部分的算法。

一种使用奇异值分解来寻找$\hat{R}$的算法

A.算法

Step 1:

从 { p i } \{p_i\} {pi​}， { p i ′ } \{p_i^{'}\} {pi′​}计算$p$和$p^{{}^{\prime }}$，之后计算 { q i } \{q_i\} {qi​}和 { q i ′ } \{q_i^{'}\} {qi′​}

Step 2：

计算$3\times 3$的矩阵
$$H=\sum _{i=1}^N{q}_i{q_i^{{}^{\prime }}}^T\text{\hspace{1em}(11)}$$
这里上标$T$定义为矩阵的转置

Step 3：

对矩阵H进行SVD分解
$$H=U\Lambda V^T\text{\hspace{1em}(12)}$$
Step 4：

计算
$$X=V{U}^T\text{\hspace{1em}(13)}$$
Step 5：

计算矩阵X的行列式$det(X)$
如果$det(X)=+1$，则$\hat{R}=X$
如果$det(X)=-1$，则算法失败(这种情况通常不会出现)

再十四讲中当行列式为负值，取相反数，具体证明有待研究

B.推导

展开公式(9)的右侧
$$\begin{gathered} {\Sigma }^2=\sum _{i=1}^N(q_i^{{}^{\prime }}-R{q}_i{)}^T(q_i^{{}^{\prime }}-R{q}_i) \\ =\sum _{i=1}^N({q_i^{{}^{\prime }}}^T{q}_i^{{}^{\prime }}+q_i^T{q}_i-{q_i^{{}^{\prime }}}^{T}R{q}_i-q_i^T{R}^T{q}_i^{{}^{\prime }}) \\ =\sum _{i=1}^N({q_i^{{}^{\prime }}}^T{q}_i^{{}^{\prime }}+q_i^T{q}_i-2{q_i^{{}^{\prime }}}^{T}R{q}_i) \end{gathered}$$
因此，最小化${\Sigma }^2$等价于最大化
$$\begin{gathered} F=\sum _{i=1}^N{q_i^{{}^{\prime }}}^{T}R{q}_i \\ =Trace(\sum _{i=1}^{N}R{q}_i{q_i^{{}^{\prime }}}^T) \\ =Trace(RH)\text{\hspace{1em}(14)} \end{gathered}$$
这里
$$H=\sum _{i=1}^N{q}_i{q_i^{{}^{\prime }}}^T\text{\hspace{1em}(15)}$$

定理：

对于任何正定的矩阵$A{A}^T$以及任何正交矩阵B
$$Trace(A{A}^T)\ge Trace(BA{A}^T)$$
定理证明：

令$a_i$为矩阵$A$的第$i$列，所以
$$Trace(BA{A}^T)=Trace(A^{T}BA)=\sum _{i=1}{a}_i^T(B{a}_i)$$
但是，根据施瓦茨不等式
$$a_i^T(B{a}_i)\le \sqrt{(a_i^T{a}_i)(a_i^T{B}^{T}B{a}_i)}=a_i^T{a}_i$$
因此$Trace(BA{A}^T)\le {\sum }_i{a}_i^T{a}_i=Trace(A{A}^T)$

对$H$进行$SVD$分解
$$H=U\Lambda V^T\text{\hspace{1em}(16)}$$
这里$U$和$V$为$3\times 3$的正交矩阵，$\Lambda$为具有非负元素的$3\times 3$的对角矩阵，现在令
$$X=V{U}^T$$
我们有
$$XH=V{U}^{T}U\Lambda V^T=V\Lambda V^T\text{\hspace{1em}(17)}$$
这里$XH$为正定对称矩阵，因此，根据定理，对于任何的$3\times 3$正交矩阵B
$$Trace(XH)\ge Trace(BXH)\text{\hspace{1em}(18)}$$
因此，在所有的$3\times 3$正交矩阵中，$X$最大化了公式(14)中的$F$，并且如果$det(X)=+1$，$X$是一个旋转，这正是我们想要的。

然而，如果$det(X)=-1$，$X$是一个反射，这并不是我们想要的。幸运的是，这种退化的情况通常来说并不会发生，我们将会在下面的两个章节中关于这种退化的细节。

退化：无噪声的情况

假设公式(1)中对于所有$i$，$N_i=0$。那么显然存在一个解$\hat{R}$(这里$\hat{R}$是一个旋转，也就是$det(\hat{R})=+1$)，并且这个$\hat{R}$对于 { q i ′ } \{q_i^{'}\} {qi′​}和 { R ^ q i } \{\hat{R}q_i\} {R^qi​}是全等的，因此${\Sigma }^2=0$。从几何层面来考虑，很容易看出存在三种可能性。

1） { q i } \{q_i\} {qi​}不共面，那么旋转的解是唯一的。进一步，没有反射$X$可以使得${\Sigma }^2=0$。因此，$SVD$算法给出了一个期望的解。
2） { q i } \{q_i\} {qi​}是共面但是不共线的，这里存在一个唯一的旋转和一个唯一的反射可以使得${\Sigma }^2=0$。因此，$SVD$算法可能给出另外一个解。我们将看到，这种情况很容易解决。
3） { q i } \{q_i\} {qi​}是共线的，这里有无数种旋转和反射可以使得${\Sigma }^2=0$.

现在我们回到共面的情况，通过检查$3\times 3$矩阵$H$的元素，可以很容易发现当且仅当矩阵H的三个奇异值之中的一个为0时点集 { q i } \{q_i\} {qi​}是共面的。令三个奇异值为${\lambda }_1>{\lambda }_2>{\lambda }_3=0$。然后
$$H={\lambda }_1{u}_i{v}_1^T+{\lambda }_2{u}_2{v}_2^T+0\cdot u_3{v}_3^T.\text{\hspace{1em}(19)}$$
这里$u_i$和$v_i$分别是矩阵$U$和$V$对应的列。注意，改变$u_3$或者$v_3$的符号不会改变$H$。因此，如果$X=V{U}^T$最小化了${\Sigma }^2$，则$X^{{}^{\prime }}=V^{{}^{\prime }}{U}^T$也是如此，这里
$$V^{{}^{\prime }}=[v_1,v_2,-v_3]\text{\hspace{1em}(20)}$$
如果$X$是一个反射，则$X^{{}^{\prime }}$是一个旋转，反之依然。因此，如果$SVD$分解给出了一个解$X$且$det(X)=-1$，我们只需要令$X^{{}^{\prime }}=V^{{}^{\prime }}{U}^T$，这是我们需要的旋转。

我们顺便注意到，当且仅当矩阵$H$的三个奇异值中的两个是相等时， { q i } \{q_i\} {qi​}是共线的。

退化：带噪声的情况

如果 { q i } \{q_i\} {qi​}或者 { q i ′ } \{q_i^{'}\} {qi′​}是共面的，那么很容易证明前面的讨论仍然是有效的，除了${\Sigma }^2$不再为0。因此，如果$SVD$算法给出了一个反射$X=V{U}^T$，我们只需要令$X^{{}^{\prime }}=V^{{}^{\prime }}{U}^T$，这是我们需要的旋转。一种特殊的情况是当$N=3$并且 { q i } \{q_i\} {qi​}或者 { q i ′ } \{q_i^{'}\} {qi′​}是共面的点集。

我们无法处理的情况是$SVD$算法给出了一个$det(X)=-1$的$X$，并且$H$的奇异值均不为0时。这意味着 { q i } \{q_i\} {qi​}和 { q i ′ } \{q_i^{'}\} {qi′​}都是共面的点集，但是没有一个旋转可以使得${\Sigma }^2$比反射计算的值更小。这种情况只会发生在噪声$N_i$非常大时。在这种情况下，最小二乘解可能无论如何都是无效的。一种更好的方法是使用类似RANSAC的方法来去除外点。

算法总结

使用前述的方法，我们可以得到
$$X=V{U}^T$$

1）如果$det(X)=+1$，则$X$就是我们期望的旋转的解
2）如果$det(X)=-1$，则$X$是一个反射，对于这种情况，我们有以下两种更处理方法

• 如果矩阵$H$的任何一个奇异值为0，则期望的旋转矩阵为
  $$X=V^{{}^{\prime }}{U}^T$$
  这里$V^{{}^{\prime }}$是通过改变矩阵$V$第三列的符号获得的。

• 如果矩阵$H$的任何一个奇异值都不为0，则最小二乘方法可能是无效的，我们需要使用类似RANSAC的技术。

计算时间需求

在VAX 11/780上进行计算机模拟，以在时间需求方面比较三种算法(SVD，四元数，迭代)。在每次模拟中，都会生成一组3D点 { p i } \{p_i\} {pi​}。他们随机分布在一个中心为(0，0，0)的尺寸为$6\times 6\times 6$的立方体中。然后将点 { p i } \{p_i\} {pi​}平移(80，60，70)，接着绕着通过原点并且方向余弦为(0.6，0.7，0.39)的旋转轴旋转$75^{°}$，最后在结果点的每一个坐标上添加均值为0，标准差为0.5的高斯随机噪声来计算得到 { p i ′ } \{p_i^{'}\} {pi′​}。然后使用三种算法来估计$\hat{R}$和$\hat{T}$，对应的CPU使用时间如表I所示。对于迭代算法，迭代次数在括号中给出。

我们注意到，SVD和四元数算法的计算时间需求是可比的，而迭代算法需要更长的时间。但是，在迭代算法中，解的计算精度为7位。如果我们可以接受百分之十的精度，那么迭代次数可以减少2-3倍。此外，收敛速率可以通过超松弛来增加。

感谢大佬ZJJ提供的md文件，互相学习交流，附上大佬的CSDN:https://blog.csdn.net/weixin_39061796?type=blog