技術標籤：opencv如何判斷點集中距離最大的兩點

本文參考自 ^{[1] [2] [3] [4]}而成

問題定義

假設有了兩個配對好的3D點集合：

我們希望尋找一個歐式變換

，使得這兩個點集合能夠能夠通過空間中的姿態變換，匹配在一起。實際上，可以通過構建最小二乘問題尋找最優匹配：

---

變形處理

為了求解

，先求解 ，再求解 。首先，定義兩組點的質心：

令

對 的偏導為 得：

由此可以得到：

將

代回 ，消去 得到：

為簡化符號，計算

、 集合的去質心座標：

由

、 和 ，此時需要求解的最優 為:

為了進一步降低階次，展開二範數部分可得到：

注意到

與 的結果都為標量，即：

由

可得

目前，我們已經得到求解

的模型——只與 、 集合的去質心座標和每一個點的權值相關，在求出 之後， 可由 得到，下一節將展示如何轉換為矩陣表示，並使用SVD進行求解。

---

使用SVD求解旋轉

定義：

其中，

和 均為由點向量的轉置組成的 矩陣， 為由每一個點的權值組成的 矩陣。實際上計算 的求和部分，就是在計算 的跡，即：

由跡的運演算法則

可將

進行變換，如下：

其中

，對 使用SVD分解得到：

帶入

有

再使用一次

，交換矩陣的順序，有 ，此時要求解 即求解：

由於

、 、 都為正交矩陣，三者矩陣乘積 也為正交矩陣。對於一個正交矩陣，其矩陣的行（或列）向量之間點積等於0（向量正交），行（或列）向量與自身的點積等於1（單位向量） ^[5]，即 . 因此，對於 裡的每一元素有：

我們希望找到一個

使得 最大，注意到 是一個對角矩陣，其對角線上排布對角元素 ，因此

可見，當

時有最大值，又因為 為正交矩陣，所以此時 是一個單位矩陣，因此可得

---

處理反射

由

得到的 是一個正交矩陣，但不能確定是一個旋轉矩陣或者是一個反射矩陣 ^{[3] [4]}。例如，當點集 是點集 的一個完美反射時，此時得到的 是一個反射矩陣，因為這一個解滿足 的要求。我們可以簡單地由這兩種矩陣的性質下結論：當 時，得到我們所需的旋轉矩陣，當 時，得到反射矩陣。此外，從幾何視角上看，有一下三種“解的數量”的可能性 ^[4]：

1. 與 不共面，此時可以保證SVD可以給出唯一解。
2. 與 共面不共線，此時存在一個唯一的反射矩陣和唯一的旋轉矩陣可以使得兩組點匹配，可以通過判斷行列式進行判斷。
3. 與 共線，此時存在不唯一的反射矩陣和不唯一的旋轉矩陣滿足，解不唯一。

那麼如何限制SVD得到的解為“旋轉矩陣”呢？由於本人目前水平不夠（懶），由^[3]可知，額外相乘一個對角矩陣修正，即可將得到的解限制為旋轉矩陣：

測試程式碼

以上小節標紅色部分公式，即為求解所需要的關鍵步驟。在這裡我們使用numpy和opencv實現最優配對的求解和驗證，目前假設權值

都為

import numpy as np
import cv2

def solve(X, Y):
    """
    Solve Y = RX + t

    Input:
        X: Nx3 numpy array of N points in camera coordinate
        Y: Nx3 numpy array of N points in world coordinate
    Returns:
        R: 3x3 numpy array describing camera orientation in the world (R_wc)
        t: 1x3 numpy array describing camera translation in the world (t_wc)

    """
    # equation (2)
    cx, cy = X.sum(axis=0) / Y.shape[0], Y.mean(axis=0)

    # equation (6)
    x, y = np.subtract(X, cx), np.subtract(Y, cy)

    # equation (13)
    w = x.transpose() @ y

    # equation (14)
    u, s, vh = np.linalg.svd(w)

    # equation (20)
    ide = np.eye(3)
    ide[2][2] = np.linalg.det(vh.transpose() @ u.transpose())
    R = vh.transpose() @ ide @ u.transpose()

    # compute equation (4)
    t = cy - R @ cx

    return R, np.array([t]).transpose()

opencv中自帶了Rodrigues函式可以製造一個旋轉矩陣，並用於驗證

R, _ = cv2.Rodrigues(np.array([-5, 1, 3], dtype=np.float))
t = np.array ([0.5, 1, 3])
X = np.random.randint(0, 10, [10, 3])
Y = (R @ X.T).T + t

R_est, t_est = solve(X, Y)
# print((R_est @ X.T).T + t_est.T)

print(R_est, 'n')
print(t_est, 'n')

得到

[[ 0.99758986  0.05581199 -0.0412249 ]
 [-0.0595199   0.99369656 -0.09499754]
 [ 0.03566303  0.09722228  0.99462353]] 

[[0.5]
 [1. ]
 [3. ]] 

參考

1. ^《視覺SLAM十四講》
2. ^使用 SVD 方法求解 ICP 問題http://www.liuxiao.org/2019/08/%e4%bd%bf%e7%94%a8-svd-%e6%96%b9%e6%b3%95%e6%b1%82%e8%a7%a3-icp-%e9%97%ae%e9%a2%98/
3. ^^abcLeast-Squares Rigid Motion Using SVDhttps://igl.ethz.ch/projects/ARAP/svd_rot.pdf
4. ^^abcLeast-Squares Fitting of Two 3-D Point Setshttps://ieeexplore.ieee.org/document/4767965
5. ^https://en.wikipedia.org/wiki/Orthogonal_matrixhttps://en.wikipedia.org/wiki/Orthogonal_matrix