Least-Squares Fitting of Two 3-D Point Sets

這是一篇非常老的論文，最關鍵的點其實是作者關于閉式解退化情況的描述

摘要

兩個點集$\{p_i\}$和$\{p_i^{{}^{\prime }}\}$;$i=1,2,...,N$由$p_i^{{}^{\prime }}=R{p}_i+T+N_i$所關聯，這里$R$是旋轉矩陣，$T$是平移向量，$N_i$是噪聲向量。給定$\{p_i\}$和$\{p_i^{{}^{\prime }}\}$，我們提出了一種算法來找到$R$和$T$的最小二乘解，這個方法基于$3\times 3$矩陣的奇異值分解(SVD)。在計算時間方面，將此算法與兩種較早提出的算法進行了比較。

引言

在許多計算機視覺的應用，尤其是使用3-D點對的對應關系[1]來估計剛體的運動參數以及剛體相對于參考坐標系的位姿[2]中，我們遇到了下面的數學問題。我們給定兩個3-D點集$\{p_i\}$和$\{p_i^{{}^{\prime }}\}$;$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^{{}^{\prime }}\}$和$\{p_i^{{}^{\prime \prime }}\equiv \hat{R}{p}_i+\hat{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^{{}^{\prime }}\}$計算$p$和$p^{{}^{\prime }}$，之后計算$\{q_i\}$和$\{q_i^{{}^{\prime }}\}$

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^{{}^{\prime }}\}$和$\{\hat{R}{q}_i\}$是全等的，因此${\Sigma }^2=0$。從幾何層面來考慮，很容易看出存在三種可能性。

1）$\{q_i\}$不共面，那么旋轉的解是唯一的。進一步，沒有反射$X$可以使得${\Sigma }^2=0$。因此，$SVD$算法給出了一個期望的解。
2）$\{q_i\}$是共面但是不共線的，這里存在一個唯一的旋轉和一個唯一的反射可以使得${\Sigma }^2=0$。因此，$SVD$算法可能給出另外一個解。我們將看到，這種情況很容易解決。
3）$\{q_i\}$是共線的，這里有無數種旋轉和反射可以使得${\Sigma }^2=0$.

現在我們回到共面的情況，通過檢查$3\times 3$矩陣$H$的元素，可以很容易發現當且僅當矩陣H的三個奇異值之中的一個為0時點集$\{q_i\}$是共面的。令三個奇異值為${\lambda }_1>{\lambda }_2>{\lambda }_3=0$。然后
$$H={\lambda }_1{u}_i{v}_1^T+{\lambda }_2{u}_2{v}_2^T+0?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\}$或者$\{q_i^{{}^{\prime }}\}$是共面的，那么很容易證明前面的討論仍然是有效的，除了${\Sigma }^2$不再為0。因此，如果$SVD$算法給出了一個反射$X=V{U}^T$，我們只需要令$X^{{}^{\prime }}=V^{{}^{\prime }}{U}^T$，這是我們需要的旋轉。一種特殊的情況是當$N=3$并且$\{q_i\}$或者$\{q_i^{{}^{\prime }}\}$是共面的點集。

我們無法處理的情況是$SVD$算法給出了一個$det(X)=?1$的$X$，并且$H$的奇異值均不為0時。這意味著$\{q_i\}$和$\{q_i^{{}^{\prime }}\}$都是共面的點集，但是沒有一個旋轉可以使得${\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\}$。他們隨機分布在一個中心為(0，0，0)的尺寸為$6\times 6\times 6$的立方體中。然后將點$\{p_i\}$平移(80，60，70)，接著繞著通過原點并且方向余弦為(0.6，0.7，0.39)的旋轉軸旋轉$75^{°}$，最后在結果點的每一個坐標上添加均值為0，標準差為0.5的高斯隨機噪聲來計算得到$\{p_i^{{}^{\prime }}\}$。然后使用三種算法來估計$\hat{R}$和$\hat{T}$，對應的CPU使用時間如表I所示。對于迭代算法，迭代次數在括號中給出。

我們注意到，SVD和四元數算法的計算時間需求是可比的，而迭代算法需要更長的時間。但是，在迭代算法中，解的計算精度為7位。如果我們可以接受百分之十的精度，那么迭代次數可以減少2-3倍。此外，收斂速率可以通過超松弛來增加。

參考文獻

總結

以上是生活随笔為你收集整理的雷达系列论文翻译（一）：Least-Squares Fitting of Two 3-D Point Sets的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。