三維點(diǎn)云配準(zhǔn)算法原理及推導(dǎo)

??作者|劉緣

1點(diǎn)云配準(zhǔn)過(guò)程 就是求一個(gè)兩個(gè)點(diǎn)云之間的旋轉(zhuǎn)平移矩陣（rigid transform or euclidean transform 剛性變換或歐式變換），將源點(diǎn)云(source cloud)變換到目標(biāo)點(diǎn)云(target cloud)相同的坐標(biāo)系下。 可以表示為以下的方程: 其中就是target cloud與source cloud中的一對(duì)對(duì)應(yīng)點(diǎn)。 而我們要求的就是其中的R與T旋轉(zhuǎn)平移矩陣。 這里，我們并不知道兩個(gè)點(diǎn)集中點(diǎn)的對(duì)應(yīng)關(guān)系。這也就是配準(zhǔn)的核心問(wèn)題。

2

配準(zhǔn)分為粗配準(zhǔn)與精配準(zhǔn)兩步

粗配準(zhǔn)就是再兩個(gè)點(diǎn)云還差得十萬(wàn)八千里、完全不清楚兩個(gè)點(diǎn)云的相對(duì)位置關(guān)系的情況下，找到一個(gè)這兩個(gè)點(diǎn)云近似的旋轉(zhuǎn)平移矩陣（不一定很精確，但是已經(jīng)大概是對(duì)的了）。 精配準(zhǔn)就是在已知一個(gè)旋轉(zhuǎn)平移的初值的情況下（這個(gè)初值大概已經(jīng)是正確的了），進(jìn)一步計(jì)算得到更加精確的旋轉(zhuǎn)平移矩陣。 這里從精配準(zhǔn)開(kāi)始講起。 精配準(zhǔn)的模式基本上已經(jīng)固定為使用ICP算法及其各種變種。ICP算法由Besl and McKay 1992, Method for registration of 3-D shapes文章提出。 文中提到的算法不僅僅考慮了點(diǎn)集與點(diǎn)集之間的配準(zhǔn)，還有點(diǎn)集到模型、模型到模型的配準(zhǔn)等。 簡(jiǎn)要介紹一下點(diǎn)集到點(diǎn)集ICP配準(zhǔn)的算法：

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

（這里的表述與原文略微有些不同，原文是用四元數(shù)加上一個(gè)偏移向量來(lái)表達(dá)旋轉(zhuǎn)平移變換。）就是一對(duì)對(duì)應(yīng)點(diǎn)，總共有對(duì)對(duì)應(yīng)點(diǎn)。這個(gè)目標(biāo)函數(shù)實(shí)際上就是所有對(duì)應(yīng)點(diǎn)之間的歐式距離的平方和。

2) 尋找對(duì)應(yīng)點(diǎn) 可是，我們現(xiàn)在并不知道有哪些對(duì)應(yīng)點(diǎn)。因此，我們?cè)谟谐踔档那闆r下，假設(shè)用初始的旋轉(zhuǎn)平移矩陣對(duì)source cloud進(jìn)行變換，得到的一個(gè)變換后的點(diǎn)云。 然后將這個(gè)變換后的點(diǎn)云與target cloud進(jìn)行比較，只要兩個(gè)點(diǎn)云中存在距離小于一定閾值（這就是題主所說(shuō)的ICP中的一個(gè)參數(shù)），我們就認(rèn)為這兩個(gè)點(diǎn)就是對(duì)應(yīng)點(diǎn)。這也是"最鄰近點(diǎn)"這個(gè)說(shuō)法的來(lái)源。
3) R、T優(yōu)化 有了對(duì)應(yīng)點(diǎn)之后，我們就可以用對(duì)應(yīng)點(diǎn)對(duì)旋轉(zhuǎn)R與平移T進(jìn)行估計(jì)。這里R和T中只有6個(gè)自由度，而我們的對(duì)應(yīng)點(diǎn)數(shù)量是龐大的（存在多余觀測(cè)值）。因此，我們可以采用最小二乘等方法求解最優(yōu)的旋轉(zhuǎn)平移矩陣。一個(gè)數(shù)值優(yōu)化問(wèn)題，這里就不詳細(xì)講了。

4) 迭代 我們優(yōu)化得到了一個(gè)新的R與T，導(dǎo)致了一些點(diǎn)轉(zhuǎn)換后的位置發(fā)生變化，一些最鄰近點(diǎn)對(duì)也相應(yīng)的發(fā)生了變化。 因此，我們又回到了步驟2)中的尋找最鄰近點(diǎn)方法。2)3)步驟不停迭代進(jìn)行，直到滿足一些迭代終止條件，如R、T的變化量小于一定值，或者上述目標(biāo)函數(shù)的變化小于一定值，或者鄰近點(diǎn)對(duì)不再變化等。（這里也是題主所說(shuō)的ICP算法中的一個(gè)參數(shù)） 算法大致流程就是上面這樣。這里的優(yōu)化過(guò)程是一個(gè)貪心的策略。首先固定R跟T利用最鄰近算法找到最優(yōu)的點(diǎn)對(duì)，然后固定最優(yōu)的點(diǎn)對(duì)來(lái)優(yōu)化R和T，依次反復(fù)迭代進(jìn)行。 這兩個(gè)步驟都使得目標(biāo)函數(shù)值下降，所以ICP算法總是收斂的，這也就是原文中收斂性的證明過(guò)程。這種優(yōu)化思想與K均值聚類的優(yōu)化思想非常相似，固定類中心優(yōu)化每個(gè)點(diǎn)的類別，固定每個(gè)點(diǎn)的類別優(yōu)化類中心。 關(guān)于參數(shù)的選擇: ICP算法的參數(shù)主要有兩個(gè)。一個(gè)是ICP的鄰近距離，另外一個(gè)是迭代的終止條件。這些參數(shù)的選擇，與實(shí)際的工程應(yīng)用相關(guān)。比如說(shuō)你的儀器精度是5mm，那么小于5mm是可以認(rèn)為是對(duì)應(yīng)點(diǎn)，而最終的迭代終止條件也就是匹配點(diǎn)之間平均距離小于5mm。 而且這些參數(shù)可以由算法逐步迭代減小，最初使用較大的對(duì)應(yīng)點(diǎn)距離參數(shù)，然后逐步減小到一個(gè)較小的值。（問(wèn)過(guò)師兄才知道實(shí)際過(guò)程這樣操作會(huì)比較合適。）需要手動(dòng)調(diào)整一些參數(shù)。（這跟機(jī)器學(xué)習(xí)調(diào)參比起來(lái)，簡(jiǎn)直不是事~）

3

粗配準(zhǔn)

前面介紹到了，ICP算法的基本原理。它需要一個(gè)旋轉(zhuǎn)平移矩陣的初值。這個(gè)初值如果不太正確，那么由于它的greedy優(yōu)化的策略，會(huì)使其目標(biāo)函數(shù)下降到某一個(gè)局部最優(yōu)點(diǎn)（當(dāng)然也是一個(gè)錯(cuò)誤的旋轉(zhuǎn)平移矩陣）。因此，我們需要找到一個(gè)比較準(zhǔn)確的初值，這也就是粗配準(zhǔn)需要做的。 粗配準(zhǔn)目前來(lái)說(shuō)還是一個(gè)難點(diǎn)。針對(duì)于不同的數(shù)據(jù)，有許多不同的方法被提出。

我們先介紹配準(zhǔn)的評(píng)價(jià)標(biāo)準(zhǔn)，再在這個(gè)標(biāo)準(zhǔn)下提出一些搜索策略。 評(píng)價(jià)標(biāo)準(zhǔn)：比較通用的一個(gè)是LCP(Largetst Common Pointset)。給定兩個(gè)點(diǎn)集P,Q，找到一個(gè)變換T(P)，使得變換后的P與Q的重疊度最大。在變換后的P內(nèi)任意一點(diǎn)，如果在容差范圍內(nèi)有另外一個(gè)Q的點(diǎn)，則認(rèn)為該點(diǎn)是重合點(diǎn)。重合點(diǎn)占所有點(diǎn)數(shù)量的比例就是重疊度。 解決上述LCP問(wèn)題，最簡(jiǎn)單粗暴的方法就是遍歷。假設(shè)點(diǎn)集P,Q的大小分別為m,n。而找到一個(gè)剛體變換需要3對(duì)對(duì)應(yīng)點(diǎn)。 那么brute force 搜索的需要的復(fù)雜度。對(duì)于動(dòng)輒幾百萬(wàn)個(gè)點(diǎn)的點(diǎn)云，這種時(shí)間復(fù)雜度是不可接受的。 因此，許多搜索策略被提出。比較容易想到的是RANSAC之類的搜索方法。而對(duì)于不同的場(chǎng)景特點(diǎn)，可以利用需配準(zhǔn)點(diǎn)云的特定信息加快搜索。（例如知道點(diǎn)云是由特定形狀的面構(gòu)成的）這里先介紹一個(gè)適用于各種點(diǎn)云，不需要先驗(yàn)信息的搜索策略，稱為4PC(4 Point Congruent)。 搜索策略：4PC搜索策略是在P,Q中找到四個(gè)共面的對(duì)應(yīng)點(diǎn)。

如上圖所示（來(lái)自4PC原文），這四個(gè)共面的點(diǎn)相交于e。這里有兩個(gè)比例在剛體變化下是不變的。(實(shí)際上在仿射變換下也是不變的) 而4PC將對(duì)于三個(gè)點(diǎn)的搜索轉(zhuǎn)換為對(duì)e,e'的搜索，從而將復(fù)雜度降低到了。 這四個(gè)點(diǎn)的距離越遠(yuǎn)，計(jì)算得到的轉(zhuǎn)換越穩(wěn)健。但是這里的四個(gè)點(diǎn)的搜索依賴于兩個(gè)點(diǎn)云的重疊度。 具體的算法可以參考4-Points Congruent Sets for Robust Pairwise Surface Registration的原文。 4PC算法通用性較好，但是對(duì)于重疊度較小、或是噪聲較大的數(shù)據(jù)也會(huì)出現(xiàn)配準(zhǔn)錯(cuò)誤或是運(yùn)行時(shí)間過(guò)長(zhǎng)的問(wèn)題。針對(duì)于不同的場(chǎng)景很多其他的搜索策略也被提出。 這里安利一下我?guī)熜值恼撐陌蓗Automatic registration of large-scale urban scene point clouds based on semantic feature points 我們課題組主要是研究室外地面站LiDAR獲取的點(diǎn)云配準(zhǔn)問(wèn)題。這種情形下，由于掃描儀內(nèi)有自動(dòng)安平裝置，Z軸都是豎直方向（重力方向），剛體變換只存在三維平移與平面（XoY面上的）旋轉(zhuǎn)。我們就在場(chǎng)景中搜索豎直的特征線并且得到它們與地面的交點(diǎn)。

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

找出其中一致性最好的三角形集合，作為匹配的集合，進(jìn)行粗配準(zhǔn)。 這種方法適用于豎直線較多的場(chǎng)景，比如城區(qū)的建筑物的邊線、林區(qū)樹(shù)木的樹(shù)干等。設(shè)計(jì)的方法還是很巧妙的。當(dāng)然如果場(chǎng)景內(nèi)這種特征較少，就比較難以配準(zhǔn)。

編輯：黃飛