基于Point-to-Plane ICP的點云與影像數(shù)據(jù)自動配準(zhǔn)?

張 星 張雙星

（華中科技大學(xué)自動化學(xué)院 武漢 430074）

基于Point-to-Plane ICP的點云與影像數(shù)據(jù)自動配準(zhǔn)?

張 星 張雙星

（華中科技大學(xué)自動化學(xué)院 武漢 430074）

針對三維激光點云與二維影像數(shù)據(jù)的融合問題，采用了一種基于Point-to-Plane ICP的配準(zhǔn)方法；該方法僅采用一塊普通的平面黑白棋盤格作為標(biāo)定板，能同時完成單目相機(jī)的標(biāo)定與三維激光掃描儀和相機(jī)的聯(lián)合標(biāo)定，進(jìn)而實現(xiàn)三維點云數(shù)據(jù)與二維影像數(shù)據(jù)的配準(zhǔn)；與以往基于控制點或者邊緣對應(yīng)的配準(zhǔn)方法不同，該方法使用RANSAC算法自動提取場景中的標(biāo)定平面，通過優(yōu)化點到平面的距離來求取兩組數(shù)據(jù)的變換。實驗結(jié)果表明，該配準(zhǔn)方法減少了人工的干預(yù)，并獲得了很高的精度。

三維點云；二維影像；自動配準(zhǔn)；Point-to-Plane ICP

1 引言

隨著三維激光掃描技術(shù)的快速發(fā)展，人們能夠很便捷地通過三維激光掃描儀獲得目標(biāo)場景表面的信息，獲取的表面信息是高密度的離散數(shù)據(jù)，被稱為“點云”，相應(yīng)的三維點云建模技術(shù)被廣泛應(yīng)用于逆向工程、數(shù)字城市可視化、虛擬現(xiàn)實等諸多領(lǐng)域，然而三維激光掃描儀僅能獲取目標(biāo)場景表面精確的三維點云坐標(biāo)信息，缺乏顏色紋理信息并且分辨率較低，嚴(yán)重制約了點云建模技術(shù)在真實感重建、目標(biāo)識別等領(lǐng)域的應(yīng)用。與之相比，現(xiàn)代工業(yè)相機(jī)能夠獲取目標(biāo)場景高分辨率高質(zhì)量的影像信息，由此可見激光掃描儀提供的三維點云坐標(biāo)信息與相機(jī)提供的影像色彩信息具有很強(qiáng)的互補性，因此兩種信息的配準(zhǔn)融合成為當(dāng)前的研究熱點。

目前針對點云數(shù)據(jù)與影像數(shù)據(jù)的配準(zhǔn)一般分為三步：單目相機(jī)的標(biāo)定；激光掃描儀與相機(jī)的聯(lián)合標(biāo)定；點云與影像數(shù)據(jù)的融合。單目相機(jī)的標(biāo)定方法已經(jīng)非常成熟，如基于直接線性變換的標(biāo)定法［1］、基于徑向排列約束的兩步標(biāo)定法［2］以及基于多姿態(tài)平面棋盤格標(biāo)定法［3］。在激光掃描儀與相機(jī)的聯(lián)合標(biāo)定方面，國內(nèi)外學(xué)者也進(jìn)行了大量的研究，總的來說可以分為三種類型：基于特征控制點共線約束的標(biāo)定，如采用手工選取對應(yīng)控制點［4］和檢測人工標(biāo)志點的聯(lián)合標(biāo)定方法［5］；基于點到線距離約束的標(biāo)定，如Kiho Kwak［6］使用邊緣明顯的V形相交平面作為標(biāo)定板，通過優(yōu)化三維點到邊緣線的距離來標(biāo)定參數(shù)，Na Li［7］則巧妙的使用了一個可折疊的M形標(biāo)定板；基于點到面約束的標(biāo)定，如Qilong Zhang［8］使用了一個平面標(biāo)定板，將點云投影在標(biāo)定板上的約束轉(zhuǎn)換成一個線性問題求解。然而由于三維激光掃描角分辨率較低且易受環(huán)境和激光傳感器精度影響，不論手工還是自動選取控制點往往都不精確，而基于點到線距離約束的標(biāo)定方法主要針對二維激光掃描儀與相機(jī)的標(biāo)定。

本文提出了一種基于Point-to-Plane ICP的配準(zhǔn)方法，能夠很好的解決三維激光掃描儀與單目相機(jī)的標(biāo)定，該方法使用和Qilong Zhang［8］相同的平面棋盤格標(biāo)定板，將平面標(biāo)定板放置在三維激光掃描儀與相機(jī)均可見的位置，獲取不同位置的平面標(biāo)定板的姿態(tài)，通過RANSAC算法自動提取平面標(biāo)定板對應(yīng)的點云數(shù)據(jù)，并采用Point-to-Plane ICP方法計算點云數(shù)據(jù)到相機(jī)坐標(biāo)系下標(biāo)定平面的剛性變換，最終完成三維點云與二維影像數(shù)據(jù)的配準(zhǔn)。

2 點云與影像數(shù)據(jù)配準(zhǔn)原理

點云與影像數(shù)據(jù)配準(zhǔn)的目的是確定激光掃描儀掃描的空間物體表面某點的三維幾何位置與其在相機(jī)拍攝的圖像中的對應(yīng)點之間的關(guān)系。其基本思想是先通過相機(jī)標(biāo)定獲得關(guān)于相機(jī)的空間三維幾何位置在圖像中的對應(yīng)點的關(guān)系，再通過聯(lián)合標(biāo)定確定關(guān)于激光掃描儀的空間三維幾何位置到相機(jī)坐標(biāo)系之間的關(guān)系。

圖1 點云與影像配準(zhǔn)坐標(biāo)變換圖

在配準(zhǔn)過程中主要涉及三個坐標(biāo)系統(tǒng)：激光掃描儀坐標(biāo)系（LCS）、相機(jī)坐標(biāo)系（CCS）、圖像坐標(biāo)系（ICS）?？紤]激光掃描儀坐標(biāo)系下任意一點PL=[XL，YL，ZL]T，在相機(jī)坐標(biāo)系中的坐標(biāo)表示為PC=[XC，YC，ZC]T，PL到 PC的剛性變換可以用旋轉(zhuǎn)矩陣RLC和平移矩陣tLC表示：

根據(jù)等式（1）可以將激光掃描儀坐標(biāo)系下所有點云的坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系下。在符合小孔成像模型的相機(jī)下，相機(jī)坐標(biāo)系中任意一點PC可以使用下面的等式投影到圖像坐標(biāo)系中：

其中 pI=[u，v，1]T和 pC=[XC，YC，ZC，1]T分別表示圖像坐標(biāo)系中像素點和相機(jī)坐標(biāo)系中的三維點的齊次坐標(biāo)，s表示縮放因子，矩陣A是相機(jī)標(biāo)定的內(nèi)參數(shù)矩陣，具有下面的形式：

其中 fx和 fy是相機(jī)的焦距，cx和cy表示相機(jī)主點，r是畸變參數(shù)。

結(jié)合式（1）和式（2）就能得到激光掃描儀坐標(biāo)系中任意一點與圖像坐標(biāo)系中像素點的關(guān)系：

點云與影像數(shù)據(jù)的配準(zhǔn)就是要找出上式中空間坐標(biāo)間的參數(shù)矩陣 A、RL，C、tL，C。

本文將點云與影像數(shù)據(jù)配準(zhǔn)分為下面幾個步驟，主要包括數(shù)據(jù)獲取、數(shù)據(jù)預(yù)處理、坐標(biāo)標(biāo)定三個階段，文章的第二節(jié)會具體介紹每個階段的流程。

圖2 點云與影像配準(zhǔn)流程圖

3 點云與影像數(shù)據(jù)配準(zhǔn)的實現(xiàn)

3.1 點云與影像數(shù)據(jù)的獲取

三維點云數(shù)據(jù)和二維影像數(shù)據(jù)分別由一臺三維激光掃描儀和CCD面陣相機(jī)獲取。本文數(shù)據(jù)獲取階段采用與Qilong Zhang［8］文中相同的平面棋盤格標(biāo)定板，Qilong Zhang［8］在實施過程中只使用了一塊平面標(biāo)定板，依次擺放在不同位置若干次，同時保證標(biāo)定板對兩個設(shè)備可見，利用二維激光掃描儀和相機(jī)采集數(shù)據(jù)，最終得到若干對“點云—影像”數(shù)據(jù)集，與之不同的是本文采用的是三維激光掃描儀，與Qilong Zhang［8］采用的二維激光掃描儀相比，三維激光掃描儀掃描場景周期很長，同時考慮到相機(jī)采集一個場景的影像可以在極短的時間內(nèi)完成，兩個設(shè)備采集數(shù)據(jù)花費的時間及其懸殊。如果采用 Qilong Zhang［8］的實施方案，在數(shù)據(jù)獲取階段將花費大量的時間。

考慮到將一個平面標(biāo)定板依次放置在不同位置若干次，使用激光掃描儀和相機(jī)分同等次數(shù)來采集若干對“點云—影像”數(shù)據(jù)集，等價于將若干個平面標(biāo)定板同時擺放在場景中的不同位置，使用激光掃描儀掃描一次就能獲取場景中所有平面標(biāo)定板的點云數(shù)據(jù)，同時只需要相機(jī)拍攝一次就能獲取包含所有標(biāo)定板的影像數(shù)據(jù)，如圖3所示，實驗過程中要保證所有平面標(biāo)定板具有同等規(guī)格，并且建議激光掃描儀與相機(jī)擺放的位置盡量靠近，為提高配準(zhǔn)精度，建議擺放5個以上的標(biāo)定板，如果相機(jī)視角有限，可以分多組來實施。采用這種方案巧妙的減少了采集數(shù)據(jù)的次數(shù)，能夠很明顯縮減獲取數(shù)據(jù)的時間。

圖3 點云與影像獲取示意圖

3.2 點云與影像數(shù)據(jù)預(yù)處理

在獲取點云數(shù)據(jù)時，由于設(shè)備精度、環(huán)境因素、被測場景表面性質(zhì)變化等的影響，點云數(shù)據(jù)中將不可避免地出現(xiàn)一些噪聲點，這些噪聲點尤其是離群點對后續(xù)激光掃描儀與相機(jī)的聯(lián)合標(biāo)定精度有很大的影響，本文采用 Radu Bogdan Rusu［9］等首次使用的基于高斯分布統(tǒng)計的離群點濾波方法對第一步采集的點云數(shù)據(jù)進(jìn)行預(yù)處理。該方法的基本思想是：對每個點，計算它到它的所有臨近點的平均距離，假設(shè)得到的結(jié)果是一個高斯分布，其形狀由均值和標(biāo)準(zhǔn)差決定，平均距離在閾值之外的點，就被定義為離群點并可從數(shù)據(jù)集中去除掉。這里的閾值由均值和標(biāo)準(zhǔn)差定義，見式（5）。

對于影像數(shù)據(jù)，只需要進(jìn)行簡單的亮度均衡化處理就能使圖像中的平面標(biāo)定板黑白格更加清晰可見，具體方法是將影像從RGB顏色空間轉(zhuǎn)換到Y(jié)CbCr顏色空間，對亮度通道進(jìn)行均衡化運算后再轉(zhuǎn)回到RGB顏色空間，這個過程可以很容易通過OpenCV庫實現(xiàn)。

3.3 相機(jī)標(biāo)定與聯(lián)合標(biāo)定

對于單目相機(jī)的標(biāo)定，其基本原理如圖4所示，本文采用非常成熟的基于多姿態(tài)平面棋盤格標(biāo)定法［3］，Matlab 工具箱［10］和 OpenCV 庫［11］中都有其相應(yīng)的實現(xiàn)。

圖4 相機(jī)標(biāo)定原理圖

相機(jī)標(biāo)定可以得到相機(jī)的外參數(shù)矩陣RC，i和tC，i，分別是3×3的正交旋轉(zhuǎn)矩陣和3×1的平移矩陣，其中i=1…m對應(yīng)第i個標(biāo)定板的外參數(shù)矩陣，m為標(biāo)定板的個數(shù)，同時還能得到內(nèi)參數(shù)矩陣A。

圖5 世界坐標(biāo)系

如圖5所示，使用基于多姿態(tài)平面棋盤格標(biāo)定法標(biāo)定相機(jī)時世界坐標(biāo)系規(guī)定在標(biāo)定平面上，其中標(biāo)定板平面在世界坐標(biāo)系中即為Z=0的平面。在世界坐標(biāo)系中，第i個標(biāo)定板平面的單位法向量nW，i=[0，0，1]T，則在相機(jī)坐標(biāo)系對應(yīng)標(biāo)定板平面的單位法向量可由下式計算得到：

其中 RC，i，3表示旋轉(zhuǎn)矩陣 RC，i的第三列。此外還可以得到相機(jī)坐標(biāo)系原點到標(biāo)定平面的距離：

接下來需要在點云數(shù)據(jù)中提取相應(yīng)的平面標(biāo)定板點云，由于室內(nèi)場景存在很多平面目標(biāo)，以往的研究者大多采用從整個場景點云數(shù)據(jù)中通過可視化的手工定向選擇的方式來選取對應(yīng)標(biāo)定板的點云區(qū)域數(shù)據(jù)，這種操作費時費力，且因為點云噪聲點的影響，提取的平面點云精度不夠高，為了解決這些問題，本文提出一種全自動的標(biāo)定板點云提取方案。首先使用RANSAC［11］算法自動提取場景中所有的平面點云，記作Planeall，可以用平面的單位法向量nL，i和激光掃描儀坐標(biāo)系原點到平面的距離dL，i表示。在使用RANSAC算法時設(shè)置的閾值為所使用激光掃描儀傳感器的測距精度（本文實驗中為1.5cm），使用RANSAC算法會自動保留屬于平面的“內(nèi)點”，進(jìn)一步提高配準(zhǔn)的精度。另外，考慮到平面標(biāo)定板放置的位置對激光掃描儀可見度很高，因此落在標(biāo)定板上的點云數(shù)量較大，通過遍歷Planeall將點云數(shù)量小于kmin的平面過濾掉，其中kmin的值需要根據(jù)實際設(shè)置的掃描儀分辨率來設(shè)定，同理可以通過設(shè)置kmax排除實驗平臺、室內(nèi)墻體等較大的平面點云。

鑒于數(shù)據(jù)獲取階段采用的是同時放置多個標(biāo)定板的方式，通過上述操作，一個點云場景中仍然存在多個符合條件的平面點云區(qū)域。由于實驗過程中激光掃描儀與相機(jī)的位置比較靠近，這里不妨假設(shè)同一個標(biāo)定板在兩個坐標(biāo)系中的法向量(nC，i，nL，i)和到原點的距離 (dC，i，dL，i)如果分別滿足一定的閾值εn和εd則為所對應(yīng)的標(biāo)定板。提取點云場景中對應(yīng)標(biāo)定板平面點云區(qū)域的操作可以用下面的數(shù)學(xué)模型表示。

最后進(jìn)行激光掃描儀和相機(jī)的聯(lián)合標(biāo)定，即求取激光掃描儀坐標(biāo)系到相機(jī)坐標(biāo)系的剛性變換，ICP配準(zhǔn)算法［12］常被用來求兩個坐標(biāo)系之間的剛性變換，但是對于激光掃描儀中平面標(biāo)定板點云中的每一點很難找到其在相機(jī)坐標(biāo)系中的對應(yīng)點，因此無法實施Point-to-Point ICP算法，而采用Point-to-Plane可以解決這個問題，并且實踐證明，Point-to-Plane ICP具有更高的精度和收斂速度［13］。

圖6 （a）Point-to-Point（b）Point-to-Plane

如圖6所示兩種ICP方案的示意圖，Point-to-Point ICP中源曲面上一點 p在目標(biāo)曲面上的對應(yīng)點為距離最近的點q，與此不同的是Point-to-Plane ICP方案中源曲面上一點 p在目標(biāo)曲面上的對應(yīng)點為過法向量與目標(biāo)表面的交點q，在本文中，源表面和目標(biāo)表面均為平面，使得尋找對應(yīng)點的操作非常簡單。采用Point-to-Plane ICP方案對兩個坐標(biāo)系進(jìn)行配準(zhǔn)的目標(biāo)函數(shù)為

當(dāng)目標(biāo)表面為平面時，上式的空間幾何解釋為：將激光掃描儀坐標(biāo)系下標(biāo)定板平面點云到對應(yīng)相機(jī)坐標(biāo)系中平面標(biāo)定板的距離作為最小化的優(yōu)化目標(biāo)函數(shù)，可以通過非線性優(yōu)化方法［14］求得該函數(shù)的最優(yōu)解 RL，C，tL，C，根據(jù)式（4）可以得到三維點云數(shù)據(jù)與二維影像的配準(zhǔn)轉(zhuǎn)換關(guān)系。

最后考慮到激光點云的分辨率遠(yuǎn)遠(yuǎn)小于影像數(shù)據(jù)的分辨率，影像中大量的像素點并沒有與之對應(yīng)的三維點，可以使用移動最小二乘法（MLS）［15］對三維點云進(jìn)行上采樣。

4 實驗與結(jié)果分析

圖7為自行設(shè)計的點云和影像采集系統(tǒng)，其中三維激光掃描儀為實驗室自制，相機(jī)為BASLER piA2400-17gc彩色千兆網(wǎng)工業(yè)相機(jī)，其分辨率為2456×2058。激光掃描儀與相機(jī)的位置在實驗過程中要保持相對靜止，為了突出實驗效果，實驗過程中以每組一個標(biāo)定板為例，共采集了15對“點云—影像”對，其中一對數(shù)據(jù)如圖8所示。

圖7 激光點云與影像采集平臺

圖8 點云與影像數(shù)據(jù)

本文實驗的軟件環(huán)境為：Windows10 64位操作系統(tǒng)，VS2013集成開發(fā)環(huán)境，圖像處理和相機(jī)標(biāo)定基于OpenCV2.4.9，點云數(shù)據(jù)處理基于PCL1.7.2。

相機(jī)標(biāo)定的結(jié)果為鏡頭焦距為

相機(jī)主點坐標(biāo)為

鏡頭畸變系數(shù)為

通過RANSAC算法和輔助信息自動提取的平面點云如圖9所示。

圖9 自動提取的標(biāo)定板點云

通過Point-to-Plane ICP算法聯(lián)合標(biāo)定的結(jié)果為

根據(jù)標(biāo)定結(jié)果，將三維點云數(shù)據(jù)與二維影像數(shù)據(jù)進(jìn)行配準(zhǔn)融合，將配準(zhǔn)結(jié)果寫入ASCII編碼文件，該文件中每一行為6個域組成，分別對應(yīng)X、Y、Z、R、G、B，使用CloudCompare軟件顯示配準(zhǔn)后的結(jié)果如圖10所示，可見三維點云數(shù)據(jù)與二維影像數(shù)據(jù)配準(zhǔn)的相當(dāng)準(zhǔn)確，滿足后續(xù)研究的需求。

圖10 點云與影像配準(zhǔn)結(jié)果圖

5 結(jié)語

本文針對三維激光點云與二維影像數(shù)據(jù)的融合問題，采用了一種基于Point-to-Plane ICP的配準(zhǔn)方法。該方法僅使用多個普通的平面黑白棋盤格同時完成單目相機(jī)的標(biāo)定與三維激光掃描儀和相機(jī)的聯(lián)合標(biāo)定。使用RANSAC算法和輔助信息自動提取場景中的標(biāo)定平面，通過優(yōu)化點到平面的距離來實現(xiàn)兩組數(shù)據(jù)的融合，進(jìn)而實現(xiàn)三維點云數(shù)據(jù)與二維影像數(shù)據(jù)的配準(zhǔn)。實驗結(jié)果表明，該配準(zhǔn)方法操作簡單且大大減少了人工和時間成本，并獲得了很高的精度。

［1］Abdel-Aziz Y，Karara H M.Direct linear transformation into object space coordinates in close-range photogramme?try，in proc.symp.close-range photogrammetry［J］.Urba?na-Champaign，1971：1-18.

［2］ Tsai R.A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses［J］.IEEE Journal on Robotics and Automation，1987，3（4）：323-344.

［3］Zhang Z.A flexible new technique for camera calibration［J］.IEEE Transactions on pattern analysis and machine intelligence，2000，22（11）：1330-1334.

［4］Naikal N，Kua J，Zakhor A.Image augmented laser scan matching for indoor localization［R］.CALIFORNIA UNIV BERKELEY，2009.

［5］趙松.三維激光掃描儀與數(shù)碼相機(jī)聯(lián)合標(biāo)定方法研究［D］.解放軍信息工程大學(xué)，2012.ZHAO Song.Study on the Joint Calibration Method of 3D Laser Scanner and Digital Camera［D］.Library of Informa?tion Engineering，2012.

［6］Kwak K，Huber D F，Badino H，et al.Extrinsic calibration of a single line scanning lidar and a camera［C］//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.IEEE，2011：3283-3289.

［7］Li N，Hu Z，Zhao B.Flexible extrinsic calibration of a cam?era and a two-dimensional laser rangefinder with a folding pattern［J］.Applied optics，2016，55（9）：2270-2280.

［8］Zhang Q，Pless R.Extrinsic calibration of a camera and la?ser range finder（improves camera calibration）［C］//Intel?ligent Robots and Systems，2004.（IROS 2004）.Proceed?ings.2004 IEEE/RSJ International Conference on.IEEE，2004，3：2301-2306.

［9］Rusu R B，Marton Z C，Blodow N，et al.Towards 3D pointcloud based object maps for household environments［J］.Robotics and Autonomous Systems，2008，56（11）：927-941.

［10］Bouguet，J.Y.“Camera Calibration Toolbox for Matlab.”Computational Vision at the California Institute of Tech?nology.

［11］Bradski，G.，and A.Kaehler.Learning OpenCV：Com?puter Vision with the OpenCV Library.Sebastopol，CA：O'Reilly，2008.

［11］Fischler M A，Bolles R C.Random sample consensus：a paradigm for model fitting with applications to image analysis and automated cartography［J］.Communications of the ACM，1981，24（6）：381-395.

［12］Besl P J，McKay N D.Method for registration of 3-D shapes［C］//Robotics-DL tentative.International Society for Optics and Photonics，1992：586-606.

［13］Rusinkiewicz S，Levoy M.Efficient variants of the ICP al?gorithm［C］//3-D Digital Imaging and Modeling，2001.Proceedings.Third International Conference on.IEEE，2001：145-152.

［14］ Levenberg K.A method for the solution of certain non-linear problems in least squares［J］.Quarterly of ap?plied mathematics，1944，2（2）：164-168.

［15］Alexa M，Behr J，Cohen-Or D，et al.Computing and ren?dering point set surfaces［J］.IEEE Transactions on visu?alization and computer graphics，2003，9（1）：3-15.

Automatic Registration of Point Cloud and Image Data Based on Point-to-Plane ICP

ZHANG XingZHANG Shuangxing
（School of Automation，Huazhong University of Science and Technology，Wuhan 430074）

A Point-to-Plane ICP based registration method is used for the fusion of 3D laser point cloud and 2D image data.The method uses only an ordinary flat black and white checkerboard as the calibration board.The calibration of the monocular cam?era combined with the calibration of the 3D laser scanner and the camera can be completed at the same time，so as to realize the reg?istration of the 3D point cloud data and the 2D image data.Unlike the traditional registration methods based on the correspondence of control points or edges，RANSAC algorithm is adopted to automatically extracts the calibration plane in the scene and achieves the fusion of the two sets of data by optimizing the distance from the point to the plane.The experimental results show that the pro?posed method can reduce the manual operation and obtain high precision.

3D point cloud，2D iamge data，automatic registration，Point-to-Plane ICP

Class Number TP391

TP391

10.3969/j.issn.1672-9722.2017.12.039

2017年6月7日，

2017年7月25日

張星，男，碩士研究生，研究方向：三維點云數(shù)據(jù)處理，機(jī)器視覺與圖像處理。張雙星，男，碩士研究生，研究方向：三維點云數(shù)據(jù)處理及可視化。