迭代最近點演算法（ICP）演算法是Lidar SLAM中常用的點雲配準方法，可以求解兩組點雲之間的相對位姿。

本文對最基本的ICP演算法進行了介紹和簡單實現，並集成為一個簡化版的Odometry。

1 原理

1.1 問題：給定兩組點雲

egin{gathered} X = left{ {{x_1},{x_2}, cdots ,{x_m}} ight} hfill \ P = left{ {{p_1},{p_2}, cdots ,{p_n}} ight} hfill \ end{gathered} (1)

求解兩組點雲之間的先對位姿 R,t 。問題的特殊性在於：1）兩組點雲之間的匹配關係是未知的；2）兩組點雲中點的數量不相同。

下面，首先給出已知匹配，且點個數相同的求解方方法。之後再給出未知匹配，且點數量不同的情況的求解方法：ICP。

1.2 已知匹配的解法

此時，兩組點雲是已經匹配好的（通過下標一一對應），且數量相等（ m=n ）。可通過最小化下式求解 R,t

Eleft( {R,t} ight) = sumlimits_{i = 1}^n {{{left| {{x_i} - left( {R{p_i} + t} ight)} ight|}^2}} (2)

這個式子可以獲得一個解析解，具體步驟如下。

step 1. 去掉兩組點雲的質心

兩組點雲的質心分別為 [{mu _x} = frac{1}{n}sumlimits_{i = 1}^n {{x_i}} , {mu _p} = frac{1}{n}sumlimits_{i = 1}^n {{p_i}} ] 。

兩組點雲去掉質心後的坐標為 [X = left{ {{x_i} - {mu _x}} ight} = left{ {{{x}_i}} ight},P = left{ {{p_i} - {mu _p}} ight} = left{ {{{p}_i}} ight}]

step 2. 計算

W = sumlimits_{i = 1}^n {{{x}_i}} {left( {p} ight)^T} (3)

step 3. 計算旋轉矩陣R

對 W 進行SVD分解，有 [W = USigma {V^T}] 。當 W 滿秩的時候，有唯一解

R=UV^T

step 4. 計算平移t

平移為

t=mu_x -Rmu_p

1.3 ICP求解方法

實際情況下，點雲之間的匹配關係是不知道，且點的數量不相等的。為此，ICP演算法採用迭代求解的方法。具體的迭代如下步驟。

step 1. 從兩組點雲中選擇匹配的點對，形成匹配兩組點雲；

step 2. 利用1.2節中的方法求解 R,t ；step 3. 如果相對於上一次迭代誤差 E(R,t) 的減少量 e<e_{th} || 超過最大迭代次數 ||E(R,t)<E_{th}結束迭代。否則以本次迭代為初值繼續迭代。

第2、3步都比較簡單，關鍵的是第一步，如何構建匹配點。最基本的做法是對於 P 中的一個點 p_i 選擇 X 中與其歐式距離最近的點最為匹配點（可以用KD-Tree加速）。如此，對於 P 中的每一個點，都可以在 X 中找到一個對應點。這樣就形成了兩組已知匹配的點雲，並且點數相等。

還有一個問題，有一些匹配時錯誤的，這些錯誤的匹配的表現是匹配點之間的距離過大。一般會用一個N倍平均距離的閾值 D_{th}=Ncdot D_{avg} 去掉那些錯誤匹配。這裡的平均閾值可以使用上一次迭代的值。

2 實現

全部實現，請見

ydsf16/lidar_slam?

github.com圖標

我們利用TUM提供的RGB-D數據進行測試，只使用了其中的Depth數據生成點雲。通過ICP計算相鄰兩幀之間的位姿變換，串接形成一個Odometry。利用rgbd_dataset_freiburg1_xyz這組數據形成的軌跡如下。

3 其他變種

ICP演算法在199x年就被提出來了，後面又出了很多的變種。主要的不同在於以下幾點。

參考資料

[1] PCL 庫實現

[2] ais.informatik.uni-freiburg.de

[3]Besl P J , Mckay N D . A Method for Registration of 3-D Shapes[J]. Proceedings of SPIE - The International Society for Optical Engineering, 1992, 14(3):239-256.


推薦閱讀：
相關文章