[发明专利]一种基于光度误差的改进ICP方法

权利要求书

1.一种基于光度误差的改进ICP方法，其特征在于，所述方法包括：

步骤1、采集物体点云图像，所述点云图像包括临时物体与目标物体；

步骤2、对所述点云图像进行处理，提取点云信息；

步骤3、对点云信息进行滤波；

步骤4、将目标点云信息构造成KD树，实现源点云信息中一点的匹配；所述目标点云信息为滤波后的点云信息；

步骤5、对于源点云信息中的一点，采用最近邻搜索算法实现点云配对；

步骤6、根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差；

步骤7、依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度，结合ICP几何误差和光度误差的点云配准，通过误差函数来表征点云匹配程度。

2.根据权利要求1所述的基于光度误差的改进ICP方法，其特征在于，对所述点云图像进行处理，提取点云信息，包括：

步骤2-1、建立像素坐标系O_pu_pv_p、相机坐标系O_cx_cy_cz_c、；根据像素坐标系和相机坐标系的转换关系，求得世界坐标系一点P在像平面的坐标：

其中，u,v为点P在像平面的横坐标与纵坐标，c_x、c_y代表相机光心在O_pu_pv_p平面的横坐标、纵坐标；为相机内参矩阵；

步骤2-2、根据相机径向畸变参数修正像素坐标，修正坐标p′_c的像素坐标；

步骤2-3采用相机内参和像素对应深度信息，提取像素信息在世界坐标系的三维坐标。

3.根据权利要求1所述的基于光度误差的改进ICP方法，其特征在于，对点云信息进行滤波，包括：

步骤3-1、利用局部加权平均的方式对点云信息进行滤波，具体为：

其中，c(p_i)代表p_i对应的RGB颜色矢量，和分别为以σ_c和σ_s为标准差的高斯核函数，代表空间距离权重和图像特征权重；*,*为表示两个矢量的内积；

步骤3-2、对利用局部加权平均的方式对点云信息进行滤波后的结果，滤除超出区域图像特征权重阈值随机噪声和测量噪声。

4.根据权利要求1所述的基于光度误差的改进ICP方法，其特征在于，将目标点云数据构造成KD树，实现源点云信息中一点的匹配，包括：

步骤4-1、对二维平面切割首先从x轴开始切分，假定6个二维数据点{(2,3)，(5,4)，(9,6)，(4,7)，(8,1)，(7,2)}，将位于二维空间内x轴上各点云数据的数值排序，分别为2、4、5、7、8、9；

选择中位数5、7中的任意一个作为切分点；假设选择中位数为7作为切分点，则二维平面划分成以x＝7为边界的左右两个子平面，将点(7,2)存入二叉树结构中作为根节点；

在y轴对分割出的两个子平面进行切分；

步骤4-2、依据上述操作循环切割切分三维空间，选择中位点云为切分点；完成三维空间KD树构建。