基于改進SIFT-ICP算法的Kinect植株點云配準方法

沈 躍 潘成凱 劉 慧 高 彬

(江蘇大學電氣信息工程學院， 鎮江 212013)

引言

近年來，隨著農業智能化和信息化不斷發展，植株三維點云模型的研究逐步成為國內外農業研究領域的重點和熱點[1-2]。利用三維重建技術構建精確的植株三維形態模型，對于植株特征的獲取具有重要意義。文獻[3-5]利用激光傳感器對植株進行重構，該方法不受環境和光照影響，但存在數據獲取過程復雜、數據量大、價格昂貴等缺點。文獻[6-8]利用雙目視覺對目標三維重建，但是由于傳感器的自身限制，攝像頭很容易受到環境光線變化干擾，嚴重影響重構精度。

微軟公司的Kinect傳感器能夠實時同步采集目標彩色圖像和深度圖像[9]。它利用飛行時間(Time of flight, ToF)主動光源技術，尤其是Kinect V2.0受光線干擾更小、彩色圖像更清晰，在現代農業研究中得到了廣泛的應用[10-11]。

文獻[12]對傳統ICP算法進行了改進，應用盒子結構劃分點云數據構建三角形，根據相似性原理選擇相似性最大的三角形做初始對應點對，進行點云配準，雖然時間方面有所改善，但是精度較低。文獻[13]基于KD-TREE對ICP算法進行改進，利用四元素法求解變換矩陣，通過重復迭代完成配準，精度有所改善，但是時間卻得不到保證。文獻[14]采用NARF-ICP方法對2片蘋果樹點云進行配準，相對傳統ICP算法在精度方面有所改進，但是耗時卻大大增加。為了在保證精度的前提下提高算法的快速性，本文以仿真樹作為研究對象，提出一種基于Kinect傳感器的植株三維點云的改進SIFT-ICP配準方法。Kinect 2.0相機從目標場景中提取植株彩色點云，通過對不同角度下的植株點云進行快速初始配準，提供給最近點迭代算法(Iterative closest point, ICP)一個較優的初始位置，而后使用改進的ICP算法進行精確配準，以期實現不同角度下植株點云的快速精確配準。

1 試驗材料和方法

1.1 植株圖像采集

試驗地點為室內，試驗對象為仿真綠色植株，樹高1.8 m，冠層直徑1.4 m(圖1)，傳感器距離目標植株1.5 m。在日光燈照射下，光強約100 lx,使用Kinect 2.0傳感器通過微軟公司提供的軟件開發包Kinect for Windows SDK以及點云庫(Point cloud library，PCL)[15]和OpenCV獲取植株的彩色、深度圖像(圖1)并合成彩色點云。

1.2 植株三維點云數據預處理

通過Kinect獲取到的目標深度圖像和彩色圖像構成了海量點云集合，點云中包含了大量的無效點和冗余背景信息以及一些不穩定的噪聲點，因此需要對原始點云進行冗余背景去除、無效點去除以及濾波和去噪等預處理，最終得到植株目標的點云數據，減小點云的數量，減輕點云后期處理的壓力。

基于目標的三維坐標信息，根據目標的深度信息d(x,y)(mm)去除植株背景信息。通過設定目標植株的三維坐標取值范圍，將取值范圍外云全部視為無用點并去除。根據試驗條件，目標植株距離傳感器有效距離設定為1 000～2 500 mm，根據

(1)

式中D(x,y)——處理后的深度信息

進行預處理提取目標植株，得到只包含目標植株所在深度區間的點云。

在目標植株周圍還會存在一些偏離較遠的離群點，這些點通常由外界干擾以及采集設備測量誤差等原因造成。由試驗獲取點云的數據分析得出，離群點距離植株一般較遠，且稀疏、離散，因此本文采用圓內數據統計的方法去除。通過統計半徑r的圓內點數量，將小于閾值k的點刪除，保留滿足條件的點。根據試驗效果，r設定為1 cm，k設定為10。

1.3 植株點云初始配準

ICP算法[16]被廣泛用于點云及圖像配準中，當兩片點云位置接近且重疊區域較大時可以很好地完成配準，而當兩片點云差異較大或者重疊區域太少時配準效果很不穩定。因此，本文提出了一種改進的點云初始配準方法，使得兩片點云通過初始配準后獲得較接近的空間位置，然后再利用ICP算法對點云進一步精確配準。

首先，基于植株點云的深度信息對點云進行尺度不變特征轉換(Scale-invariant feature transforms, SIFT)關鍵點搜索[17],得到較為均勻且具有標志性的點云關鍵點并對關鍵點進行自適應法線估計。然后對關鍵點進行快速點特征直方圖(Fast point feature histograms, FPFH)特征提取[18]，并且根據關鍵點特征值和特征描述子利用采樣一致性初始配準(Sample consensus-initial alignment，SAC-IA)算法完成初配，并求得轉換參數。最后根據轉換參數將2片點云轉換到同一空間坐標系中用作精確配準的輸入，最終進行ICP精確配準。由于SIFT算法耗時較長，為了縮短這個過程，本文采用Nanoflann方法[19]加速關鍵點搜索和特征提取。

1.3.1SIFT關鍵點估計

關鍵點的檢測不僅可以最大程度地表達點云中的特征，還可以大幅度地減少后期配準點云的數量，提高配準的快速性和準確性，因此選取一個合適的關鍵點檢測方法至關重要。SIFT關鍵點檢測算法能夠從圖像中提取出對尺度和旋轉保持不變的特征點。尺度空間極值點檢測采用層疊式濾波的方法，應用帶有不同σ的高斯函數對圖像進行處理，圖像尺度空間定義為

F(x,y,σ)=(G(x,y,kσ)-G(x,y,σ))I(x,y)=
L(x,y,kσ)-L(x,y,σ)

(2)

式中 (x,y)——像素點位置G——高斯函數

I——原圖像L——卷積函數

σ——尺度空間因子k——常數

檢測到的極值點作為候選點，對位置、尺度、彎曲度等做擬合，剔除那些低對比度和定位差的邊緣點，同時對尺度空間函數F(x,y,σ)運用Taylor展開得到點云匹配關鍵點。

1.3.2自適應法線估計和特征提取

求得點云匹配關鍵點之后需對關鍵點進行法線估計。由于點云數據沒有幾何表面，可以利用某一點鄰域范圍內的其它點云來近似估計該點的表面法向量。假設有點云數據P=(p1,p2,…)，對于其中某一點pi(xi,yi,zi)，取其鄰域內h個相鄰點，協方差矩陣為

(3)

矩陣C的特征值和特征向量為

CVj=λjVj(j=0,1,2)

(4)

式中λj——第j個特征值

Vj——第j個特征向量

計算出的最小特征值對應的特征向量即為點pi處最小二乘擬合曲面的法向量。

本文采用自適應法線估計，對法線估計過程鄰域值m在5～25之間進行自適應取值。對于點云稀疏部分，法線估計時鄰近點取值m減少；點云稠密部分，法線估計時的鄰近點取值m增加。該自適應過程可以對點云稠密部分獲取更多的有意義點，增加了法線的準確性，而稀疏部分的點云一般是邊緣或者空隙，因此適當減少周圍點的取值可以減少非邊緣點對邊緣法線估計的影響。

利用FPFH算法對帶有法線的關鍵點進行特征提取。FPFH算法是RUSU提出的快速點特征描述子提取算法。該算法基于點特征直方圖[20](Point feature histogram, PFH)進行改進，算法復雜度低，特征提取效果好。本文采用經過降維的33維元素的特征向量，保證了較好的描述性，同時大幅度減少了計算時間。

1.3.3SAC-IA算法初始配準

通過采樣一致性初始配準(SAC-IA)算法,以關鍵點及關鍵點的特征描述子為依據構成候選對應點對，對候選對應關系進行大量采樣并通過以下步驟對其進行排名：①從點集P中選擇w個關鍵點，同時確定它們的配對距離大于設定的最小值dmin。②在點集Q中找到滿足直方圖和關鍵點直方圖相似的點存入一個列表中,從這些點中隨機選擇一些代表關鍵點的對應關系。③計算通過采樣點定義的剛體變換和其對應變換，計算點云的度量誤差來評價轉換的質量。重復上述3個步驟直到取得最佳誤差，并使用暴力配準部分數據。最后使用Levenberg-marquardt算法進行非線性局部優化完成初始配準。

1.4 基于Nanoflann加速的點云精確配準

經過初始配準后，2幅點云圖像之間有了較好的初始位置，而ICP算法運算結果在很大程度上依賴于2幅點云圖像的初始位置，因此經過初始配準后的點云再經過ICP算法的精確配準后會在很大程度上提高配準精度。因此，本文對初始配準后的點云進行進一步的精確配準以提高配準精度。此外，ICP算法主要是通過多次迭代實現點云配準，配準過程中的最近鄰搜索耗費了大量的時間，因此，需要一種更加快速便捷的近鄰搜索算法。本文使用了更為快捷的Nanoflann來進一步加速ICP算法的最近鄰搜索過程，提高ICP的快速性。Nanoflann方法從編程角度和存儲空間的合理利用方面加速搜索過程，其次，它還可以在最近鄰搜索前直接給定點云的最小包圍盒，避免了過程中的重復計算，對數據類型適用性更廣泛。圖2展示了Nanoflann和原始ICP算法中近鄰搜索算法Flann的快速性對比。

圖2 Nanoflann和Flann算法快速性對比Fig.2 Comparison of fastness between Nanoflann and Flann algorithms

由圖2可以看出，Nanoflann算法比Flann算法的快速性提高了50%以上。

利用得到的最終轉換矩陣可以將2片具有不低于30%公共部分的點云進行配準。

1.5 改進SIFT-ICP算法

本文提出的改進SIFT-ICP算法步驟如下：

(1)對2片預處理后的點云圖像分別進行SIFT關鍵點搜索。

(2)使用自適應法線估計求取2片點云中關鍵點的法線。

(3)根據關鍵點和關鍵點法線求取關鍵點的快速點特征直方圖。

(4)通過采樣一致性初始配準算法，根據關鍵點及快速點特征直方圖估計對應點對，進行初始配準，得到初始配準變換矩陣T0，至此完成初始配準。

(5)對經過初始配準后的點云Atemp以均勻分布的方式篩選點集，得到Pi，在點云Ai+1中對Pi中所有點進行Nanoflann最近鄰搜索，得到對應點集Pi+1，計算Pi和Pi+1的重心并對其進行點集中心化，生成新點集。

(6)由新點集得到正定矩陣和最大特征向量，利用四元素法得到變換矩陣Ttemp和轉換后的點集Ptemp。計算Pi和Ptemp之間距離平方和fi，若相鄰平方和之差小于閾值λ，則停止迭代，否則更新精確配準變換矩陣T1和點云Atemp，并回到步驟(5)。

改進SIFT-ICP算法程序流程圖如圖3所示。

圖3 改進SIFT-ICP點云配準流程圖Fig.3 Flow chart of improved SIFT-ICP point cloud registration

2 試驗與結果分析

2.1 點云獲取與處理

通過PCL將Kinect相機獲取的植株深度圖像和彩色圖像進行融合，得到彩色點云圖像。圖4展示了2個不同角度拍攝的植株彩色原始點云圖像。

圖4 植株彩色原始點云圖像Fig.4 Original color point cloud images of plant

由圖4可以看出，除了植株外，還有大量冗余數據的白色背景墻和一些噪聲點、離群點。對原始點云圖像進行閾值分割和圓內數據統計后得到僅含植株信息的點云圖像。圖5為預處理后的植株點云圖像。

圖5 預處理后植株點云圖Fig.5 Point cloud images after preprocessing

圖6 點云配準過程Fig.6 Processes of point cloud registration

由圖5可以看出，2幅點云圖像在視角方向有很大差異，因此先對2幅點云進行SIFT關鍵點搜索并計算關鍵點的法線，得出關鍵點的直方圖，最后通過SAC-IA方法完成初始配準，完成初始配準之后再使用ICP算法進行精確配準。圖6分別從側面視角和正面視角展示了點云的配準過程，圖6a、6b為配準前的2幅點云圖像，圖6c為關鍵點搜索結果，圖6d為對關鍵點進行法線估計的結果，圖6e展示了部分關鍵點FPFH描述子統計結果，x為統計區域的索引，y為落在該區域的點數，圖6f和圖6g為初始配準之后的2幅點云圖像，圖6h、6i是精確配準后的2幅點云圖像，由精確配準后的點云圖像可以看出，2幅點云圖像得到了高精度配準。

2.2 點云匹配算法誤差分析

通過計算ICP配準過程中對應點間平均歐氏距離來對2幅點云圖像配準誤差進行評估。當2片點云之間的對應點距離較小時，表明配準誤差較小，配準效果較好。若2片點云完全配準，在對應點對均正確的情況下，平均歐氏距離應為零。本文對傳統ICP算法、PCA-ICP算法、SIFT-ICP算法及本文改進SIFT-ICP算法進行比較，結果如表1所示。從表1可以看出，初始配準算法的使用可以大幅度提高配準精確度，后3種配準方法相較傳統的ICP算法，配準誤差從4.23 cm分別下降到2.53、1.41、0.48 cm。相對于傳統方法配準精度分別提升了40.2%、66.7%和88.7%。PCA-ICP算法、SIFT-ICP算法相較于傳統ICP算法時間都明顯增加，分別由56.2 s增加到189.5 s和138.6 s，然而本文算法在時間上優化效果明顯，只需26.6 s，詳見表1。算法配準效果對比見圖7。

表1 點云配準誤差分析Tab.1 Analysis of point cloud registration error

圖7 算法配準結果對比Fig.7 Comparison of algorithm registration results

為驗證本文算法的穩定性，對仿真植株每間隔30°獲取一次點云圖像，對間隔角在30°的點云進行兩兩配準，試驗結果如圖8所示。可以看出配準誤差控制在7 mm以內，配準時間控制在30 s以內，因此本文算法在快速性和穩定性方面均得到了保證。

圖8 算法快速性和穩定性結果分析Fig.8 Fastness and stability results analysis of algorithm

考慮到太陽光對傳感器的影響，在天氣晴朗條件下于11：00獲取植株彩色點云圖像進行補充試驗。由于傳感器是利用結構光原理來獲取深度信息，因此在太陽光直射的情況下，植株點云數量明顯減少，出現邊緣缺失和點云丟失等情況，平均每幅點云圖像的點云數量為23 800。雖然獲取的植株點云不佳，但經預處理后，其背景及周圍噪聲點均可以得到有效去除，對多組相鄰點云進行配準，平均誤差為6.2 mm，并且因為點云數量的減少，經多組試驗測試，平均算法耗時縮短5 s，達到21.6 s。試驗效果如圖9所示。

3 結論

(1) 提出了改進后的SIFT-ICP算法，通過對法線計算和ICP算法過程近鄰搜索算法的改進，大幅度提高了配準精度和速度。

(2) 初始配準后的點云樹葉部分配準較好，但植株樹干部分誤差較明顯，原因在于樹干部分的關鍵點全部位于點云的邊緣且檢測出的關鍵點較少，而傳感器精度及處理后其邊緣部分的變化會導致直方圖的差異，因而在z軸方向存在較大誤差，但在精確配準過程中得到糾正。

圖9 室外植株點云獲取及配準Fig.9 Access and registration of outdoor plant point cloud

(3) 提出了Nanoflann加速的ICP精確配準算法，通過對ICP算法最近鄰搜索算法的改進，使近鄰搜索過程快速性提高了50%以上，大幅度提高了精確配準效率。

1 袁曉敏，趙春江，溫維亮, 等.番茄植株三維形態精確重構研究[J/OL].農業機械學報, 2012, 43(12): 204-210.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20121237&flag=1.DOI:10.6041/j.issn.1000-1298.2012.12.037.

YUAN Xiaomin, ZHAO Chunjiang, WEN Weiliang, et al. Detailed modeling of 3-D configuration of tomato plant[J/OL].Transactions of the Chinese Society for Agricultural Machinery, 2012, 43(12): 204-210. (in Chinese)

2 劉剛, 司永勝, 馮娟.農林作物三維重建方法研究進展[J/OL].農業機械學報, 2014, 45(6): 39-46. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20140607&flag=1.DOI:10.6041/j.issn.1000-1298.2014.06.007.

LIU Gang, SI Yongsheng, FENG Juan. 3D reconstruction of agriculture and forestry crops[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(6): 39-46. (in Chinese)

3 馬曉丹, 郭彩玲, 張雪, 等.基于三維點云顏色特征的蘋果樹冠層光照分布計算方法[J/OL].農業機械學報, 2015, 46(6): 263-268. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150638&flag=1. DOI:10.6041/j.issn.1000-1298.2015.06.038.

MA Xiaodan, GUO Cailing, ZHANG Xue, et al. Calculation of light distribution of apple tree canopy based on color characteristics of 3D point cloud[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(6): 263-268. (in Chinese)

4 LIN Y, HYYPPA J. Multiecho-recording mobile laser scanning for enhancing individual tree crown reconstruction[J]. Geoscience and Remote Sensing, 2012, 50(11): 4323-4332.

5 HUI L, HEPING Z. Evaluation of a laser scanning sensor in detection of complex-shaped targets for vari-able-rate sprayer development[J]. American Society of Agricultural and Biological Engineers, 2016, 59(5): 1181-1192.

6 王珊, 徐曉.基于雙目單視面的三維重建[J]. 光學學報, 2017, 37(5): 0515004.

WANG Shan, XU Xiao. The 3D reconstruction based on horopter[J]. Acta Optic Sin, 2017, 37(5): 0515004. (in Chinese)

7 蔡健榮, 孫海波, 李永平, 等.基于雙目立體視覺的果樹三維信息獲取與重構[J/OL]. 農業機械學報, 2012, 43(3): 152-156. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20120328&flag=1. DOI：10.6041/j.issn.1000-1298.2012.03.028.

CAI Jianrong, SUN Haibo, LI Yongping, et al. Fruit trees 3-D information perception and reconstraction based on binocular stereo vision[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2012, 43(3): 152-156. (in Chinese)

8 呂耀文, 康凱.基于雙目視覺的三維重建和拼接技術研究[J]. 光電子技術, 2016, 36(4): 237-241.

LV Yaowen, KANG Kai. Research on three-dimensional reconstruction and stitching based on binocular stereo vision[J]. Optoelectronic Technology, 2016, 36(4): 237-241. (in Chinese)

9 KHOSHELHAM K, ELBERINK S O. Accuracy and resolution of Kinect depth data for indoor mapping application[J]. Sensors, 2012, 12(2): 1437-1454.

10 鄭立華, 麥春燕, 廖巍, 等. 基于Kinect相機的蘋果樹三維點云配準[J/OL]. 農業機械學報, 2016, 47(5): 9-14.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160502&flag=1.DOI:10.6041/j.issn.1000-1298.2016.05.002.

ZHENG Lihua, MAI Chunyan, LIAO Wei, et al. 3D point cloud registration for apple tree based on Kinect[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(5): 9-14. (in Chinese)

11 AZZARI G, GOULDEN M L, RUSU R B. Rapid characterization of vegetation structure with a Microsoft Kinect sensor[J]. Sensor, 2013, 13:2384-2398.

12 楊小青, 楊秋翔, 楊劍,等. 應用改進ICP算法的點云配準[J]. 計算機工程與設計, 2015, 36(9):2457-2461.

YANG Xiaoqing, YANG Qiuxiang, YANG Jian, et al. Point cloud registration based on improved ICP algorithm[J]. Computer Engineering and Design, 2015, 36(9):2457-2461. (in Chinese)

13 郭俊輝. 基于KDTree改進的ICP算法在點云配準中的應用研究[J]. 微型機與應用, 2015, 34(14):81-83.

GUO Junhui. The applied research of improved ICP algorithm based in KDTree in point cloud registration[J]. Technique and Method, 2015, 34(14):81-83. (in Chinese)

14 何東健, 邵小寧, 王丹, 等. Kinect獲取植物三維點云數據的去噪方法[J/OL]. 農業機械學報, 2016,47(1):331-336. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160145&flag=1. DOI：10.6041/j.issn.1000-1298.2016.01.045.

HE Dongjian, SHAO Xiaoning, WANG Dan, et al. Denoising method of 3-D point cloud data of plants obtained by Kinect[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(1): 331-336. (in Chinese)

15 RUSU R B, COUSINS S. 3D is here: Point cloud library (PCL)[C]∥IEEE International Conference on Robotics and Automation(ICRA), Shanghai, China, 2011.

16 SHARP G C, LEE S W, WEHE D K. ICP registration using invariant feature[J]. Pattern Analysis and Machine Intelligence, 2002, 24(1): 90-102.

17 DAVID G L. Distinctive image feature from scale-invariant keypoints[J]. International Journal of Computer Vision, 2004, 60(2):91-110.

18 RUSU R B, BLODOW N, BEETZ M. Fast point feature histograms(FPFH) for 3D registration[C]∥Robotics and Automation, 2009. ICRA, 2009: 3212-3217.

19 常樂樂. SICP配準的三維人臉建模研究[D]. 合肥：合肥工業大學,2016.

CHANG Lele. 3D face modeling based on registering of SICP[D]. Hefei: Hefei University of Technology, 2016. (in Chinese)

20 RUSU R B, BLODOW N, MARTON Z C, et al. Aligning point cloud views using persistent feature histo-grams[C]∥IEEE International Conference on Intelligent Robots and Systems, 2008: 3384-3391.