ICP算法可以使用SVD或者非線性優化的方法進行求解。因爲PCL源碼中對ICP的求解就是使用SVD,所以這裏對其進行一些探究。
1.公式推導
1.首先對第i i i 個點構建誤差項:
e i = p i − ( R p i ′ + t )
e_{i}=p_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)
e i = p i − ( R p i ′ + t )
2.構建最小二乘問題:求得使得目標函數最小的未知量R , t R,t R , t
min R , t J = 1 2 ∑ i = 1 n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2
\min _{\boldsymbol{R}, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)\right)\right\|_{2}^{2}
R , t min J = 2 1 i = 1 ∑ n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2
接下來定義一些需要用的變量:
首先定義兩組點雲的質心:
p = 1 n ∑ i = 1 n ( p i ) , p ′ = 1 n ∑ i = 1 n ( p i ′ )
\boldsymbol{p}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}\right), \quad \boldsymbol{p}^{\prime}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}^{\prime}\right)
p = n 1 i = 1 ∑ n ( p i ) , p ′ = n 1 i = 1 ∑ n ( p i ′ )
4.在誤差函數中添加質心,進行配平
1 2 ∑ i = 1 n ∥ p i − ( R p i ′ + t ) ∥ 2 = 1 2 ∑ i = 1 n ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 = 1 2 ∑ i = 1 n ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 = 1 2 ∑ i = 1 n ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 + 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) )
\begin{aligned}\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)\right\|^{2} &=\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}-\boldsymbol{t}-\boldsymbol{p}+\boldsymbol{R} \boldsymbol{p}^{\prime}+\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}\right\|^{2} \\&=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right)+\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right\|^{2} \\&=\frac{1}{2} \sum_{i=1}^{n}\left(\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2}+\right.\\&\left.2\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right)^{T}\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right)\end{aligned}
2 1 i = 1 ∑ n ∥ p i − ( R p i ′ + t ) ∥ 2 = 2 1 i = 1 ∑ n ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 = 2 1 i = 1 ∑ n ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 = 2 1 i = 1 ∑ n ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 + 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) )
注意到交叉項部分中,( p i − p − R ( p i ′ − p ′ ) ) \left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right) ( p i − p − R ( p i ′ − p ′ ) ) 在求和之後是爲0的,因此目標優化函數可以進行化簡,變成:
min R , t J = 1 2 ∑ i = 1 n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2
\min _{\boldsymbol{R}, \boldsymbol{t}} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2}
R , t min J = 2 1 i = 1 ∑ n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2
5.仔細觀察左右兩項,我們發現左邊只和旋轉矩陣 R R R 相關,而右邊既有 $R 也 有 也有 也 有 $t,但 只和質心相關。只要我們獲得了 R R R ,令第二項爲零就能得到 t t t 。於是,ICP 可以分爲以下 三個步驟求解:
5.1 計算兩組點的質心位置 p , p ′ p, p′ p , p ′ ,然後計算每個點的去質心座標:
q i = p i − p , q i ′ = p i ′ − p ′
\boldsymbol{q}_{i}=\boldsymbol{p}_{i}-\boldsymbol{p}, \quad \boldsymbol{q}_{i}^{\prime}=\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}
q i = p i − p , q i ′ = p i ′ − p ′
5.2 根據以下優化問題計算旋轉矩陣:
R ∗ = arg min R 1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2
\boldsymbol{R}^{*}=\arg \min _{\boldsymbol{R}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}
R ∗ = arg R min 2 1 i = 1 ∑ n ∥ q i − R q i ′ ∥ 2
5.3 根據第二步的 R R R ,計算$ t$:
t ∗ = p − R p ′
\boldsymbol{t}^{*}=\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}
t ∗ = p − R p ′
6.根據以上推導,我們重點需要求出旋轉量R R R ,求得之後代入自然可以獲得t t t 。對公式(6)進行展開:
1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 = 1 2 ∑ i = 1 n q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′
\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{n} \boldsymbol{q}_{i}^{T} \boldsymbol{q}_{i}+\boldsymbol{q}_{i}^{\prime T} \boldsymbol{R}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}-2 \boldsymbol{q}_{i}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}
2 1 i = 1 ∑ n ∥ q i − R q i ′ ∥ 2 = 2 1 i = 1 ∑ n q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′
注意到第一項和 R R R 無關,第二項由於 R T R = I R^TR = I R T R = I ,亦與 R R R 無關。因此,實際上優化 目標函數變爲:
∑ i = 1 n − q i T R q i ′ = ∑ i = 1 n − tr ( R q i ′ q i T ) = − tr ( R ∑ i = 1 n q i ′ q i T )
\sum_{i=1}^{n}-\boldsymbol{q}_{i}^{T} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}=\sum_{i=1}^{n}-\operatorname{tr}\left(\boldsymbol{R} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{T}\right)=-\operatorname{tr}\left(\boldsymbol{R} \sum_{i=1}^{n} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{T}\right)
i = 1 ∑ n − q i T R q i ′ = i = 1 ∑ n − t r ( R q i ′ q i T ) = − t r ( R i = 1 ∑ n q i ′ q i T )
其實就是3 ∗ 1 ∗ 1 ∗ 3 3*1 * 1*3 3 ∗ 1 ∗ 1 ∗ 3 的矩陣,自然只有對角線上的數據表示兩個點之間的有效數據。
7.對這個優化函數的求解,則使用SVD來完成。定義矩陣:
W = ∑ i = 1 n q i q i ′ T
\boldsymbol{W}=\sum_{i=1}^{n} \boldsymbol{q}_{i} \boldsymbol{q}_{i}^{\prime T}
W = i = 1 ∑ n q i q i ′ T
W W W 是一個 3 × 3 的矩陣,對 W W W 進行 SVD 分解,得:
W = U Σ V T
\boldsymbol{W}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{T}
W = U Σ V T
其中,Σ Σ Σ 爲奇異值組成的對角矩陣,對角線元素從大到小排列,而 U U U 和 V V V 爲正交矩 陣。當 W W W 滿秩時,R R R 爲:
R = U V T
\boldsymbol{R}=\boldsymbol{U} \boldsymbol{V}^{T}
R = U V T
求得結果後,根據公式(7)即可獲得t t t 的結果。
2.PCL源碼閱讀
整個源碼從align 函數開始,調用關係如上圖所示。
需要注意的是,PCL中根據算法的分類,對所有的代碼進行歸類。比如pcl屬於Registration類,那麼所有的匹配算法都統一放在Registration文件夾下。
其中h文件是定義好的函數接口,一些簡單的函數直接在裏面通過內聯函數的形式進行了完善。
impl(implement)文件夾下的hpp文件則是對h文件中定義的複雜文件的實現。
src文件夾則是一個空的.cpp文件,只包含了對應的頭文件。個人理解主要是爲了編譯生成可調用的庫文件。