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

说明书

本申请提供一种基于光度误差的改进ICP方法，方法包括：采集物体点云图像，所述点云图像包括临时物体与目标物体；对所述点云图像进行处理，提取点云信息；对点云信息进行滤波；将目标点云信息构造成KD树，实现源点云信息中一点的匹配；所述目标点云信息为滤波后的点云信息；对于源点云信息中的一点，采用最近邻搜索算法实现点云配对；根据匹配数据来采用最小二乘法求解刚性变换矩阵和ICP几何误差；依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度，结合ICP几何误差和光度误差的点云配准，通过误差函数来表征点云匹配程度。

技术领域

本申请涉及机器人视觉SLAM中的三维建图领域技术领域，特别涉及一种基于光度误差的改进ICP方法。

背景技术

随着计算机技术和图像处理器的发展，机器人技术的研究有了迅速的发展。其中地图构建作为机器人执行导航定位的前置步骤，是机器人技术的重要研究方向之一。地图构建算法通过传感器采集到的空间环境信息，在没有先验信息的情况下，完成空间物体和环境在计算机系统上的三维地图重建。视觉传感器获取环境信息的能力更高，其所获取的图像能够实现更智能化的功能。

在机器人系统中，针对非结构动态环境的环境感知和建模，要求建图算法具有一定实时性。实时三维重建算法中传感器采集数据与数据分析同时进行，通过视觉里程计完成不同帧图像位姿估计完成图像拼接和模型重建。实时三维重建算法拥有更快的相应速度和能在运行过程中完成自动化三维重建，更加满足机器人系统对建图算法的需求。在建图算法设计点云配准算法，ICP(迭代最近点算法)算法是其中的经典算法，可以通过原始点云数据进行检索匹配点云，但是针对非结构化环境，由于光照、天气等原因，传统的算法生成的是无序点云，不存在数据结构，检索速度慢，并且在非结构化环境下，点云位置差距较大，容易出现空间点对误匹配，使系统终止于局部最优解，造成运行效率低下，检索效率低。

发明内容

本申请提供了一种基于光度误差的改进ICP方法，可用于解决现有技术中点云之间匹配效率低下的技术问题。

本申请提供一种基于光度误差的改进ICP方法，所述方法包括：

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

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

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

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

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

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

步骤7、依据跟踪误差来评估源点云信息以及目标点云信息的匹配程度，结合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的像素坐标；