Least-Squares Fitting of Two 3-D Point Sets

这是一篇非常老的论文，最关键的点其实是作者关于闭式解退化情况的描述

摘要

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

引言

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

解耦平移和旋转

根据[3]中所述，如果公式(1)的最小二乘解为R^\hat{R}R^和T^\hat{T}T^，则{pi′}\{p_i^{'}\}{pi′}和{pi′′≡R^pi+T^}\{p_i^{''} \equiv \hat{R}p_i +\hat{T}\}{pi′′≡R^pi+T^}有着相同的质心，也就是说
p′=p′′.(3)p^{'} = p^{''}. \tag{3} p′=p′′.(3)
这里
p′=1N∑i=1Npi′.(4)p^{'} = \frac{1}{N} \sum_{i = 1}^{N}{p_i^{'}}. \tag{4} p′=N1i=1∑Npi′.(4)
p′′=1N∑i=1Npi′′=R^p+T^(5)p^{''} = \frac{1}{N} \sum_{i=1}^{N}{p_i^{''}} = \hat{R}p + \hat{T} \tag{5} p′′=N1i=1∑Npi′′=R^p+T^(5)
p=1N∑i=1Npip = \frac{1}{N} \sum_{i=1}^{N}{p_i} p=N1i=1∑Npi
令
qi=pi−p(7)q_i = p_i - p \tag{7} qi=pi−p(7)
qi′=pi′−p′(8)q_i^{'} = p_i^{'} - p^{'} \tag{8} qi′=pi′−p′(8)
我们有
∑i=1N∣∣qi′−Rqi∣∣2(9)\sum_{i = 1}^{N}{|| q_i^{'} - Rq_{i} ||^2} \tag{9} i=1∑N∣∣qi′−Rqi∣∣2(9)

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

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

A.算法

Step 1:

从{pi}\{p_i\}{pi}，{pi′}\{p_i^{'}\}{pi′}计算ppp和p′p^{'}p′，之后计算{qi}\{q_i\}{qi}和{qi′}\{q_i^{'}\}{qi′}

Step 2：

计算3×33 \times 33×3的矩阵
H=∑i=1Nqiqi′T(11)H = \sum_{i=1}^{N}{q_i {q_i^{'}}^T} \tag{11} H=i=1∑Nqiqi′T(11)
这里上标TTT定义为矩阵的转置

Step 3：

对矩阵H进行SVD分解
H=UΛVT(12)H = U \Lambda V^T \tag{12} H=UΛVT(12)

Step 4：

计算
X=VUT(13)X = VU^T \tag{13} X=VUT(13)

Step 5：

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

B.推导

展开公式(9)的右侧
Σ2=∑i=1N(qi′−Rqi)T(qi′−Rqi)=∑i=1N(qi′Tqi′+qiTqi−qi′TRqi−qiTRTqi′)=∑i=1N(qi′Tqi′+qiTqi−2qi′TRqi)\Sigma^2 = \sum_{i=1}^{N}{ (q_i^{'} - Rq_i)^T (q_i^{'} - Rq_i) } \\ = \sum_{i=1}^{N}{( {q_i^{'}}^Tq_i^{'} + q_i^T q_i - {q_i^{'}}^TRq_i - q_i^T R^T q_i^{'} )} \\ = \sum_{i=1}^{N}{( {q_i^{'}}^Tq_i^{'} + q_i^T q_i - 2{q_i^{'}}^TRq_i )} Σ2=i=1∑N(qi′−Rqi)T(qi′−Rqi)=i=1∑N(qi′Tqi′+qiTqi−qi′TRqi−qiTRTqi′)=i=1∑N(qi′Tqi′+qiTqi−2qi′TRqi)
因此，最小化Σ2\Sigma^2Σ2等价于最大化
F=∑i=1Nqi′TRqi=Trace(∑i=1NRqiqi′T)=Trace(RH)(14)F = \sum_{i=1}^{N}{{q_i^{'}}^T R q_i} \\ = Trace(\sum_{i=1}^{N}{ Rq_i {q_i^{'}}^T}) \\ = Trace(RH) \tag{14} F=i=1∑Nqi′TRqi=Trace(i=1∑NRqiqi′T)=Trace(RH)(14)
这里
H=∑i=1Nqiqi′T(15)H = \sum_{i=1}^{N}{q_i {q_i^{'}}^T} \tag{15} H=i=1∑Nqiqi′T(15)

定理：

对于任何正定的矩阵AATAA^TAAT以及任何正交矩阵B
Trace(AAT)≥Trace(BAAT)Trace(AA^T) \geq Trace(BAA^T) Trace(AAT)≥Trace(BAAT)

定理证明：

令aia_iai为矩阵AAA的第iii列，所以
Trace(BAAT)=Trace(ATBA)=∑i=1aiT(Bai)Trace(BAA^T) = Trace(A^TBA) = \sum_{i=1}{a_i^T(Ba_i)} Trace(BAAT)=Trace(ATBA)=i=1∑aiT(Bai)
但是，根据施瓦茨不等式
aiT(Bai)≤(aiTai)(aiTBTBai)=aiTaia_i^T(B a_i) \leq \sqrt{(a_i^Ta_i)(a_i^T B^T B a_i)} = a_i^Ta_i aiT(Bai)≤(aiTai)(aiTBTBai)=aiTai
因此Trace(BAAT)≤∑iaiTai=Trace(AAT)Trace(BAA^T) \leq \sum_{i}{a_i^Ta_i} = Trace(AA^T)Trace(BAAT)≤∑iaiTai=Trace(AAT)

对HHH进行SVDSVDSVD分解
H=UΛVT(16)H = U \Lambda V^T \tag{16} H=UΛVT(16)
这里UUU和VVV为3×33 \times 33×3的正交矩阵，Λ\LambdaΛ为具有非负元素的3×33 \times 33×3的对角矩阵，现在令
X=VUTX = VU^T X=VUT
我们有
XH=VUTUΛVT=VΛVT(17)XH = VU^TU \Lambda V^T = V \Lambda V^T \tag{17} XH=VUTUΛVT=VΛVT(17)
这里XHXHXH为正定对称矩阵，因此，根据定理，对于任何的3×33 \times 33×3正交矩阵B
Trace(XH)≥Trace(BXH)(18)Trace(XH) \geq Trace(BXH) \tag{18} Trace(XH)≥Trace(BXH)(18)
因此，在所有的3×33 \times 33×3正交矩阵中，XXX最大化了公式(14)中的FFF，并且如果det(X)=+1det(X) = +1det(X)=+1，XXX是一个旋转，这正是我们想要的。

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

退化：无噪声的情况

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

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

现在我们回到共面的情况，通过检查3×33 \times 33×3矩阵HHH的元素，可以很容易发现当且仅当矩阵H的三个奇异值之中的一个为0时点集{qi}\{q_i\}{qi}是共面的。令三个奇异值为λ1>λ2>λ3=0\lambda_1 > \lambda_2 > \lambda_3 = 0λ1>λ2>λ3=0。然后
H=λ1uiv1T+λ2u2v2T+0⋅u3v3T.(19)H = \lambda_1 u_i v_1^T + \lambda_2u_2v_2^T + 0 \cdot u_3 v_3^T. \tag{19} H=λ1uiv1T+λ2u2v2T+0⋅u3v3T.(19)
这里uiu_iui和viv_ivi分别是矩阵UUU和VVV对应的列。注意，改变u3u_3u3或者v3v_3v3的符号不会改变HHH。因此，如果X=VUTX = VU^TX=VUT最小化了Σ2\Sigma^2Σ2，则X′=V′UTX^{'} = V^{'}U^TX′=V′UT也是如此，这里
V′=[v1,v2,−v3](20)V^{'} = [v_1,v_2,-v_3] \tag{20} V′=[v1,v2,−v3](20)
如果XXX是一个反射，则X′X^{'}X′是一个旋转，反之依然。因此，如果SVDSVDSVD分解给出了一个解XXX且det(X)=−1det(X) = -1det(X)=−1，我们只需要令X′=V′UTX^{'} = V^{'}U^TX′=V′UT，这是我们需要的旋转。

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

退化：带噪声的情况

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

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

算法总结

使用前述的方法，我们可以得到
X=VUTX = VU^T X=VUT

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

如果矩阵HHH的任何一个奇异值为0，则期望的旋转矩阵为
X=V′UTX = V^{'}U^T X=V′UT
这里V′V^{'}V′是通过改变矩阵VVV第三列的符号获得的。
如果矩阵HHH的任何一个奇异值都不为0，则最小二乘方法可能是无效的，我们需要使用类似RANSAC的技术。

计算时间需求

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

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

参考文献

雷达系列论文翻译（一）：Least-Squares Fitting of Two 3-D Point Sets相关推荐

雷达系列论文翻译（十二）：Cartographer系列（三）
Real-Time Loop Closure in 2D LIDAR SLAM 摘要便携式激光测距仪,又称激光雷达,同时定位和建图(SLAM)是一种有效的获取竣工平面图的方法.实时生成和可视化楼层平 ...
雷达系列论文翻译（六）：LIO-SAM
LIO-SAM: Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping 摘要我们提出了一种通过平滑和建图实现紧耦合的激光雷 ...
雷达系列论文翻译（七）：Lio-mapping
Tightly Coupled 3D Lidar Inertial Odometry and Mapping 摘要自我运动估计是大多数移动机器人应用的基本要求.通过传感器融合,我们可以弥补独立传感器 ...
雷达系列论文翻译（四）：LeGO-LOAM
LeGO-LOAM: Lightweight and Ground-OptimizedLidar Odometry and Mapping on Variable Terrain 摘要我们提出了一种 ...
雷达系列论文翻译（十一）：LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometryvia Smoothing and Mapping
LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometryvia Smoothing and Mapping 摘要我们提出了一个通过平滑和映射实现 ...
雷达系列论文翻译（十）：Scan registration using segmented region growing NDT
Scan registration using segmented region growing NDT 这篇论文可以视为对上一篇论文的详述版本,作者为同一人,这篇中对于每一部分的算法给出了详细的公式 ...
分布式系统领域经典论文翻译集
分布式领域论文译序 sql&nosql年代记 SMAQ:海量数据的存储计算和查询一．google论文系列 1. google系列论文译序 2. The anatomy o ...
php 谷歌翻译api_科研福音，论文翻译神器系列！
参考文献很大程度上反映了一篇论文的水平.对于研究生来说,自己动手写论文前的第一步工作就是阅读大量高水平.前沿的文献,而这些论文大多是英文写就. 人工翻译一般比较耗时且需要扎实的语言功底,对于初学者来说 ...
A Survey of Deep Learning-based Object Detection论文翻译 + 阅读笔记
A Survey of Deep Learning-based Object Detection论文翻译 + 阅读笔记 //2022.1.7 日下午16:00开始阅读双阶段检测器示意图单阶段检测器 ...

雷达系列论文翻译（一）：Least-Squares Fitting of Two 3-D Point Sets