[发明专利]一种融合直接法与特征法的RGB-D同时定位与地图创建方法

权利要求书

1.一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，包括以下步骤：

步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将第一帧作为关键帧，初始化相机位姿之后，在关键帧中提取关键点，利用其在深度图像中的深度值构造一系列3D点，从所有3D点中创建一个初始化地图；

步骤S2，对步骤S1中的RGB彩色数据，通过最小化光度误差来进行相机的初始位姿估计以及像素点的对应关系计算；

步骤S3，利用最小化局部地图点的重投影误差再次优化相机位姿，对局部地图进行跟踪，并判断是否生成新的关键帧，若跟踪失败，则触发新的跟踪周期；

步骤S4，在局部建图线程中，对当前关键帧图像中的ORB特征进行提取与匹配，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；

步骤S5，当处理完所有关键帧之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化；

步骤S6，筛选候选关键帧，对冗余的关键帧进行剔除；

步骤S7，在闭环检测线程中，通过检索图像识别数据库执行闭环检测，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；

步骤S8，采用位姿图优化的方法对相机运动的累积误差进行校正；

步骤S9，将步骤S7中周期内的所有关键帧位姿与所有地图点位置作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹；

步骤S10，根据RGB-D图像和相机位姿信息，在创建点云地图的过程中，通过基于八叉树的建图框架Octomap来转化为3D占用网格地图。

2.根据权利要求1所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2相机的初始位姿估计包括以下几个步骤：

步骤S2.1，跟踪线程通过对每一帧图像处理来对当前帧的位姿进行估计，如果上一帧跟踪成功，利用一个恒速模型即假定摄像头处于匀速运动状态来对当前相机的位姿进行预测；

步骤S2.2，根据步骤S2.1中估计的位姿，将参考帧中跟踪的3D地图点投影到当前帧中，当前帧与参考帧的相对位姿变换则可以通过最小化光度误差计算得到，该误差主要建立在同一个3D点对应的图像块之间，即

其中：是参考帧的像素位置，ρ表示二范数，是图像块区域，T表示变换矩阵；

其中是参考帧坐标系中第i个被跟踪的3D地图点的位置，表示参考帧的像素灰度值。

3.根据权利要求1或2所述的一种融合直接法与特征法的RGB-D同时定位与地图创建方法，其特征在于，所述步骤S2求取像素点对应关系的过程为：通过初始位姿估计，得到经过优化且与上一帧像素点相关联的地图点集，从该地图点集中，得到当前帧与局部关键帧可观测到的Covisibility地图点，将局部地图点在当前帧中进行投影，从而通过最小化光度误差的方法获取更多图像像素点和局部地图点的对应关系，最小化光度误差的方法包括以下步骤：为寻找与像素点p₁对应着的同一像素点p₂，首先根据当前相机的位姿估计值，来寻找p₂的位置。但若相机的位姿不够好，p₂的外观与p₁会有明显差别，从而假设一个空间点在各个视角下，成像的灰度是不变的。基于灰度值不变的假设，通过非线性优化的方法最小化p的两个像素点的亮度误差来优化相机位姿，寻找与p₁点更相似的p₂点，其中优化变量为相机位姿。