[发明专利]一种基于曼哈顿世界假设及因子图的相机位姿优化方法

说明书

本发明公开了一种基于曼哈顿世界假设及因子图的相机位姿优化方法，可以在弱纹理环境下的同步定位与制图（SLAM，Simultaneously positioning and mapping）系统中增加定位的准确性和鲁棒性，其思路是计算特征的重投影误差以及结构化特征的结构误差项，构建因子图模型并进行位姿优化。该算法首先计算特征的重投影误差，然后根据曼哈顿世界假设构造结构化特征的结构误差项，最后在SLAM后端中对上述误差项构建因子图模型并进行位姿优化。本发明通过将结构化特征的重投影误差和结构误差加入到后端优化中，可以提高弱纹理场景下SLAM系统位姿解算的准确度和鲁棒性，以便满足机器人更加广阔的应用场景。

技术领域

本发明属于图像图形领域，涉及一种同时定位与建图技术，具体涉及一种基于曼哈顿世界假设及因子图的相机位姿优化方法。

背景技术

近年来，智能机器人已成为学术界和工业界的一个重要的研究方向。要使机器人更加智能、更加自主，机器人必须提高对未知环境的认知能力。同时定位与地图构建技术(SLAM，Simultaneously positioning and mapping)是构建三维场景地图以及机器自身定位的关键技术，已经成功应用到无人车、无人机、快递机器人、智能清洁机器人等产品上。

目前一些基于点特征的SLAM系统在大部分场景下已经能够稳定运行，然而在一些弱纹理场景下(白墙，地面)，由于纹理特征不够丰富，基于特征点匹配的SLAM，容易出现跟踪失败的情况，导致SLAM系统定位失败。因此，考虑基于点线面特征的可以保证SLAM系统稳定跟踪，并提高定位精度，同时现实世界中的线和平面存在着大量的平行和垂直关系(曼哈顿假设)，可以通过这些结构性条件约束SLAM系统的优化过程，从而增加系统的定位精度和鲁棒性，扩展机器人的应用场景。

目前一些基于结构特征的SLAM系统正逐渐被重视起来，虽然基于点特征的SLAM系统在大部分场景下已经能够稳定运行，但在一些弱纹理场景下(白墙，地面)，由于纹理特征不够丰富容易出现跟踪失败的情况，从而导致SLAM系统定位失败。由于结构化场景中墙面、地面和书桌等存在较多的互相垂直或平行的平面，在弱纹理环境下考虑加入这些结构化特征，虽然有一些研究已经将结构化特征加入到SLAM的后端优化中，但是大多数研究都忽略了结构化特征之间存在的约束，导致其构建的图优化模型过于单一，如果提取到的特征存在结构性误差和漂移，若把误差加入到后端优化，将严重影响系统性能。根据曼哈顿世界假设，将结构化特征之间存在的平行和垂直关系加入到后端优化的过程中，对SLAM系统的定位求解加以约束，提高SLAM系统在弱纹理环境下定位的准确性和鲁棒性。

发明内容

为解决上述问题，本发明公开了一种基于曼哈顿世界假设及因子图的相机位姿优化方法，通过将结构化特征的重投影误差和结构误差加入到后端优化中，可以提高弱纹理场景下SLAM系统位姿解算的准确度和鲁棒性，以便满足机器人更加广阔的应用场景。

为达到上述目的，本发明的技术方案如下：

一种基于曼哈顿世界假设及因子图的相机位姿优化方法，用于弱纹理大场景的SLAM稳健定位，包括如下步骤：

(1)提取点、线、面特征；

(2)计算点、线、面特征的重投影误差；

(3)根据曼哈顿世界假设计算线特征和面特征的结构性误差；

(4)利用重投影误差和结构性误差构建因子图模型并进行位姿优化。

其中步骤(1)中，提取点、线、面特征，具体步骤为：

(1.1)根据灰度图提取图像的ORB特征点；

(1.2)根据深度图对点云进行分割后得到平面特征，提取平面边缘得到线特征。

步骤(2)中，计算点、线、面特征的重投影误差，具体步骤为：