3D-3D:ICP

• 1. ICP算法
• 2. SVD方法
• 3.非線性優化方法

1. ICP算法

2D-2D問題是通過已知的2D圖像坐標點對來求解圖像間的變換關系（即 $R$ 和 $t$）。3D-2D問題是通過已知的3D空間點坐標和對應的2D像素坐標來求解圖像的位姿。 所以很顯然，3D-3D問題是根據已知的3D空間點對來求解圖像圖像間的變換關系。

假設我們有一組配對好的 3D 點

現在想要找到一個歐式變換$R$，$t$，使得

這個問題可以用迭代最近點（Iterative Closest Point, ICP）求解。可以看到3D-3D 位姿估計問題中，并沒有出現相機模型，也就是說，僅考慮兩組 3D 點之間的變換時，和相機并沒有關系。因此，在激光 SLAM 中也會碰到 ICP，不過由于激光數據特征不夠豐富，我們無從知道兩個點集之間的匹配關系，只能認為距離最近的兩個點為同一個，所以這個方法稱為迭代最近點。而在視覺中，特征點為我們提供了較好的匹配關系，所以整個問題就變得更簡單了。在 RGB-D SLAM 中，可以用這種方式估計相機位姿。下文我們用 ICP 指代匹配好的兩組點間運動估計問題。

和 PnP 類似， ICP 的求解也分為兩種方式：利用線性代數的求解（主要是 SVD），以及利用非線性優化方式的求解（類似于 Bundle Adjustment）。下面分別來介紹它們。

2. SVD方法

根據前面描述的 ICP 問題，我們定義第 $i$ 對點的誤差項：$$e_i=p_i?(R{p}_i^{’}+t)$$ 構建最小二乘問題，求使誤差平方和達到極小的 $R$ 和 $t$ ：

接下來，來推導它的求解方法。

首先,定義兩組點的質心:

隨后,在誤差函數中,我們作如下的處理:

注意到交叉項部分中，$(p_i?p?R(p_i^{\prime }?p^{\prime }))$在求和之后是為零的，因此優化目標函數可以簡化為:

仔細觀察左右兩項，我們發現左邊只和旋轉矩陣 R 相關，而右邊既有 R 也有 t，但只和質心相關。只要我們獲得了 R，令第二項為零就能得到 t。于是，ICP 可以分為以下三個步驟求解:

1. 計算兩組點的質心位置 $p$和$p^{\prime }$ ，然后計算每個點的去質心坐標： $$q_i=p_i?p$$ $$q_i^{\prime }=p_i^{\prime }?p^{\prime }$$
2. 根據以下優化問題計算旋轉矩陣:$$R^{?}=argmin\frac{1}{2}\sum _{i=1}^n\mid \mid q_i?R{q}_i^{\prime }\mid {\mid }^2$$
3. 根據第二步的$R$，計算 $t$：$$t^{?}=p?R{p}^{\prime }$$

我們看到,只要求出了兩組點之間的旋轉,平移量是非常容易得到的。所以我們重點關注 R 的計算。展開關于 R 的誤差項，得:

注意到第一項和 R 無關,第二項由于 R T R = I,亦與 R 無關。因此,實際上優化目標函數變為:

接下來,我們介紹怎樣通過 SVD 解出上述問題中最優的 R，為了解 R，先定義矩陣:

$W$是一個 3 × 3 的矩陣,對 $W$ 進行 SVD 分解，得:

其中,$\Sigma$ 為奇異值組成的對角矩陣,對角線元素從大到小排列,而 $U$ 和$V$ 為正交矩陣。當 $W$ 滿秩時,$R$ 為:$$R=U{V}^T$$

解得 $R$ 后，便可求出 $t$.

3.非線性優化方法

求解 ICP 的另一種方式是使用非線性優化,以迭代的方式去找最優值。該方法和前面講述的 PnP 非常相似。以李代數表達位姿時,目標函數可以寫成:

單個誤差項關于位姿導數已經在前面推導過了,使用李代數擾動模型即可:

于是，在非線性優化中只需不斷迭代，我們就能找到極小值。而且，可以證明 ， ICP問題存在唯一解或無窮多解的情況。在唯一解的情況下，只要我們能找到極小值解，那么這個極小值就是全局最優值——因此不會遇到局部極小而非全局最小的情況。這也意味著ICP 求解可以任意選定初始值。這是已經匹配點時求解 ICP 的一大好處。

需要說明的是，我們這里講的 ICP,是指已經由圖像特征給定了匹配的情況下，進行位姿估計的問題。在匹配已知的情況下，這個最小二乘問題實際上具有解析解 ，所以并沒有必要進行迭代優化。ICP 的研究者們往往更加關心匹配未知的情況。不過，在RGB-D SLAM 中，由于一個像素的深度數據可能測量不到，所以我們可以混合著使用 PnP和 ICP 優化：對于深度已知的特征點，用建模它們的 3D-3D 誤差；對于深度未知的特征點，則建模 3D-2D 的重投影誤差。于是，可以將所有的誤差放在同一個問題中考慮，使得求解更加方便。

總結

以上是生活随笔為你收集整理的3D-3D:ICP的全部內容，希望文章能夠幫你解決所遇到的問題。

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