三維點云配準過程詳解：算法原理及推導(dǎo)

本文將對三維點云配準過程中的算法原理及必要推導(dǎo)流程進行詳解。

01點云配準過程

就是求一個兩個點云之間的旋轉(zhuǎn)平移矩陣（rigid transform or euclidean transform 剛性變換或歐式變換），將源點云(source cloud)變換到目標點云(target cloud)相同的坐標系下。

可以表示為以下的方程:

其中就是target cloud與source cloud中的一對對應(yīng)點。

而我們要求的就是其中的R與T旋轉(zhuǎn)平移矩陣。

這里，我們并不知道兩個點集中點的對應(yīng)關(guān)系。這也就是配準的核心問題。

02

配準分為粗配準與精配準兩步

粗配準就是再兩個點云還差得十萬八千里、完全不清楚兩個點云的相對位置關(guān)系的情況下，找到一個這兩個點云近似的旋轉(zhuǎn)平移矩陣（不一定很精確，但是已經(jīng)大概是對的了）。

精配準就是在已知一個旋轉(zhuǎn)平移的初值的情況下（這個初值大概已經(jīng)是正確的了），進一步計算得到更加精確的旋轉(zhuǎn)平移矩陣。

這里從精配準開始講起。

精配準的模式基本上已經(jīng)固定為使用ICP算法及其各種變種。ICP算法由Besl and McKay 1992, Method for registration of 3-D shapes文章提出。

文中提到的算法不僅僅考慮了點集與點集之間的配準，還有點集到模型、模型到模型的配準等。

簡要介紹一下點集到點集ICP配準的算法：

1) ICP算法核心是最小化一個目標函數(shù)：

（這里的表述與原文略微有些不同，原文是用四元數(shù)加上一個偏移向量來表達旋轉(zhuǎn)平移變換。）

就是一對對應(yīng)點，總共有對對應(yīng)點。這個目標函數(shù)實際上就是所有對應(yīng)點之間的歐式距離的平方和。

2) 尋找對應(yīng)點

可是，我們現(xiàn)在并不知道有哪些對應(yīng)點。因此，我們在有初值的情況下，假設(shè)用初始的旋轉(zhuǎn)平移矩陣對source cloud進行變換，得到的一個變換后的點云。

然后將這個變換后的點云與target cloud進行比較，只要兩個點云中存在距離小于一定閾值（這就是題主所說的ICP中的一個參數(shù)），我們就認為這兩個點就是對應(yīng)點。這也是"最鄰近點"這個說法的來源。

3) R、T優(yōu)化

有了對應(yīng)點之后，我們就可以用對應(yīng)點對旋轉(zhuǎn)R與平移T進行估計。這里R和T中只有6個自由度，而我們的對應(yīng)點數(shù)量是龐大的（存在多余觀測值）。因此，我們可以采用最小二乘等方法求解最優(yōu)的旋轉(zhuǎn)平移矩陣。一個數(shù)值優(yōu)化問題，這里就不詳細講了。

4) 迭代

我們優(yōu)化得到了一個新的R與T，導(dǎo)致了一些點轉(zhuǎn)換后的位置發(fā)生變化，一些最鄰近點對也相應(yīng)的發(fā)生了變化。

因此，我們又回到了步驟2)中的尋找最鄰近點方法。2)3)步驟不停迭代進行，直到滿足一些迭代終止條件，如R、T的變化量小于一定值，或者上述目標函數(shù)的變化小于一定值，或者鄰近點對不再變化等。（這里也是題主所說的ICP算法中的一個參數(shù)）

算法大致流程就是上面這樣。這里的優(yōu)化過程是一個貪心的策略。首先固定R跟T利用最鄰近算法找到最優(yōu)的點對，然后固定最優(yōu)的點對來優(yōu)化R和T，依次反復(fù)迭代進行。

這兩個步驟都使得目標函數(shù)值下降，所以ICP算法總是收斂的，這也就是原文中收斂性的證明過程。這種優(yōu)化思想與K均值聚類的優(yōu)化思想非常相似，固定類中心優(yōu)化每個點的類別，固定每個點的類別優(yōu)化類中心。

關(guān)于參數(shù)的選擇:

ICP算法的參數(shù)主要有兩個。一個是ICP的鄰近距離，另外一個是迭代的終止條件。這些參數(shù)的選擇，與實際的工程應(yīng)用相關(guān)。比如說你的儀器精度是5mm，那么小于5mm是可以認為是對應(yīng)點，而最終的迭代終止條件也就是匹配點之間平均距離小于5mm。

而且這些參數(shù)可以由算法逐步迭代減小，最初使用較大的對應(yīng)點距離參數(shù)，然后逐步減小到一個較小的值。（實際過程中這樣操作會比較合適。）需要手動調(diào)整一些參數(shù)。（這跟機器學習調(diào)參比起來，簡直不是事~）

03

粗配準

前面介紹到了，ICP算法的基本原理。它需要一個旋轉(zhuǎn)平移矩陣的初值。這個初值如果不太正確，那么由于它的greedy優(yōu)化的策略，會使其目標函數(shù)下降到某一個局部最優(yōu)點（當然也是一個錯誤的旋轉(zhuǎn)平移矩陣）。因此，我們需要找到一個比較準確的初值，這也就是粗配準需要做的。

粗配準目前來說還是一個難點。針對于不同的數(shù)據(jù)，有許多不同的方法被提出。

我們先介紹配準的評價標準，再在這個標準下提出一些搜索策略。

評價標準：比較通用的一個是LCP(Largetst Common Pointset)。給定兩個點集P,Q，找到一個變換T(P)，使得變換后的P與Q的重疊度最大。在變換后的P內(nèi)任意一點，如果在容差范圍內(nèi)有另外一個Q的點，則認為該點是重合點。重合點占所有點數(shù)量的比例就是重疊度。

解決上述LCP問題，最簡單粗暴的方法就是遍歷。假設(shè)點集P,Q的大小分別為m,n。而找到一個剛體變換需要3對對應(yīng)點。

那么brute force 搜索的需要的復(fù)雜度。對于動輒幾百萬個點的點云，這種時間復(fù)雜度是不可接受的。

因此，許多搜索策略被提出。比較容易想到的是RANSAC之類的搜索方法。而對于不同的場景特點，可以利用需配準點云的特定信息加快搜索。（例如知道點云是由特定形狀的面構(gòu)成的）這里先介紹一個適用于各種點云，不需要先驗信息的搜索策略，稱為4PC(4 Point Congruent)。

搜索策略：4PC搜索策略是在P,Q中找到四個共面的對應(yīng)點。

如上圖所示（來自4PC原文），這四個共面的點相交于e。這里有兩個比例在剛體變化下是不變的。(實際上在仿射變換下也是不變的)

而4PC將對于三個點的搜索轉(zhuǎn)換為對e,e'的搜索，從而將復(fù)雜度降低到了。

這四個點的距離越遠，計算得到的轉(zhuǎn)換越穩(wěn)健。但是這里的四個點的搜索依賴于兩個點云的重疊度。

具體的算法可以參考4-Points Congruent Sets for Robust Pairwise Surface Registration的原文。

4PC算法通用性較好，但是對于重疊度較小、或是噪聲較大的數(shù)據(jù)也會出現(xiàn)配準錯誤或是運行時間過長的問題。針對于不同的場景很多其他的搜索策略也被提出。

舉例說明，對于室外地面站LiDAR獲取的點云配準問題。這種情形下，由于掃描儀內(nèi)有自動安平裝置，Z軸都是豎直方向（重力方向），剛體變換只存在三維平移與平面（XoY面上的）旋轉(zhuǎn)。我們就在場景中搜索豎直的特征線并且得到它們與地面的交點。

再將這些交點構(gòu)建出三角形，以三角形的全等關(guān)系來得到匹配。

找出其中一致性最好的三角形集合，作為匹配的集合，進行粗配準。

這種方法適用于豎直線較多的場景，比如城區(qū)的建筑物的邊線、林區(qū)樹木的樹干等。設(shè)計的方法還是很巧妙的。當然如果場景內(nèi)這種特征較少，就比較難以配準。