基于改进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.