CN117253003A - 一种融合直接法与点面特征法的室内rgb-d slam方法 - Google Patents

Abstract

本发明提供一种融合直接法与点面特征法的室内RGB‑D SLAM方法，涉及移动机器人视觉SLAM领域，主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对图像帧进行跟踪，通过融合最小化光度图像误差与最小深度图像误差来进行相机的初始位姿估计，ORB点特征与平面特征提取与匹配，根据“曼哈顿世界”模型，将平面特征根据平行平面与垂直平面进行匹配，使用重投影误差获取相机位姿，再利用最小化局部地图特征重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位。新的关键帧选取方法，通过当前图像帧与关键帧旋转角度只差判断是否插入新的关键帧；在局部建图中对冗余关键帧进行剔除。

Description

一种融合直接法与点面特征法的室内RGB-D SLAM方法

技术领域

本发明涉及移动机器人视觉SLAM领域，具体为一种融合直接法与点面特征法的室内RGB-D SLAM方法。

背景技术

同时定位与地图构建(SLAM)是机器人领域的一个关键挑战和主要研究重点。未知环境中的自主移动机器人在执行各种任务时需要对环境地图进行构建，例如路径规划，避障和探索等。移动机器人必须知道其位置才能创建地图。另一方面，移动机器人需要通过地图来获取位置信息。因此，移动机器人必须对环境地图进行创建，同时在地图中定位。

在稀疏特征环境定位建图领域中，常见的方法主要是基于激光雷达的定位建图方法，如单线2D激光雷达、二十四线3D激光雷达，以及使用视觉的定位建图方法,如利用单目、双目相机或深度相机进行基于优化的同时定位与建图。随着定位建图算法的快速发展，目前已经出现多种基于多类视觉特征结合定位建图的方法，但普遍存在稀疏纹理环境下由于环境特征提取困难造成的定位过程鲁棒性较差、定位结果不精确的问题。

因此，我们设计了一种融合直接法与点面特征法的室内RGB-D SLAM方法。

发明内容

(一)解决的技术问题

针对现有技术的不足，本发明提供了一种融合直接法与点面特征法的室内RGB-DSLAM方法，通过融合直接法与点面特征法方式实现稀疏纹理环境下的视觉快速定位，不依赖丰富的环境纹理特征，在环境纹理特征缺少甚至缺失的时候，仍具有较高的精度和鲁棒性。

(二)技术方案

为实现上述目的，本发明提供如下技术方案：

一种融合直接法与点面特征法的室内RGB-D SLAM方法，其特征在于，包括以下：

步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将初始化成功的第一帧作为关键帧，并将第一关键帧作设定初始位姿为单位旋转，平移为0m，在关键帧中提取点特征与面特征，利用其在深度图像中的深度值构造一系列3D点与3D平面，创建一个初始化地图；

步骤S2，对步骤S1中的RGB彩色数据与深度数据，通过联合最小化光度误差与最小深度误差来进行相机的位姿预估计；

步骤S3，对当前图像帧中的ORBB(Oriented FAST and Rotated BRIEF)特征与面特征进行提取与匹配，然后使用BA只对位姿进行优化，筛选出当前图像帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点地图平面；

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

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

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

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

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

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

进一步的，所述步骤S2相机的位姿预估计包括：

在跟踪线程通过前一彩色图像帧与深度图像帧提取的ORB特征点，得到融合最小光度误差与最小深度误差来对当前帧的位姿进行预估计。当前帧与前一帧的相对位姿T_cr为

其中：为最小光度误差/>为最小深度误差，/>当前帧某一点的灰度值，/>上一帧提取的关键点的灰度值，为当前帧某一点深度值，/>为前一帧某一特征点深度值。/>为Huber核。最后利用L-M(Levenberg-Marquadt)算法求解上式，L-M(Levenberg-Marquadt)

算法步骤如下：

(1)给定初始点x₀，允许误差ε＞0，设置k＝0；

(2)f′(x^(k))＜ε，导数接近0即在极小点附近，得到x，算法结束，否则继续；

(3)计算x^(k+1)，

进一步，所述步骤S3对当前图像帧中的ORB特征与面特征进行提取与匹配，然后使用BA只对位姿进行优化，筛选出当前图像帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点地图平面，包括以下几个步骤：

使用ORB特征点进行匹配，利用匹配的特征点来构建最小重投影误差为三维空间点，K为相机内参，T为相机位姿。

u_i＝[u_i,v_i]^T为其投影的像素坐标点。

使用AHC算法(层次聚类算法)提取当前帧的面特征，并以π＝(n_x,n_y,n_z,d)输出，n＝(n_x,n_y,n_z)为面特征的法向量，d是相机坐标原点到平面的垂直距离。平面参数π＝(n_x,n_y,n_z,d)是过参化表示，过参化会使信息矩阵秩亏。所以将平面转为表示平面法向量在OXY平面上的投影与OX轴之间的夹角，ψ＝arctan(n_y/n_x)为平面法向量与OZ轴之间的夹角。

面特征通过几何角度约束与几何距离约束匹配。几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值10°，则满足平面间夹角约束。|θ₁-θ₂|≥10°

几何距离约束：计算观测平面与路标平面间的距离，若距离小于设定距离阈值10cm，则满足平面间距离约束。|d₁-d₂|≥10cm

根据面特征匹配结果构建误差函数其中π_w为地图平面特征，π_m为当前帧地图平面特征，T_cw为地图坐标到当前帧的位姿变换。

此外，室内环境中存在明显的结构特征，一些论文中也利用了曼哈顿世界假设，认为所有法向量只分布在三个互相垂直的主轴上。但这一假设太强，这里做了简化。本发明搜索了地图中的平行或垂直的平面，并以此添加约束关系。平行或垂直面的搜索是小于一定的角度差异阈值即可。这里在添加约束时，应该为地图中的平面特征之间构建，但这样的话无法对前端中的相机位姿求解带来帮助，因此我们将这约束关系也转化到相机与对应的平面之间。

垂直平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差大于设定角度阈值85°，则满足垂直平面间夹角约束。|θ₁-θ₂|≥85°

根据垂直平面匹配构建误差函数e_so(n_w,T_cw)＝R_⊥n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换，R_⊥为90度旋转矩阵。

平行平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值5°，则满足平行平面间夹角约束。|θ₁-θ₂|≤5°

根据平行平面匹配构建误差函数e_sp(n_w,T_cw)＝n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换。

点特征和平面特征的匹配关系均可优化当前帧位姿。局部地图中包含关键帧、点路标、平面路标，使用所有点和平面对当前位姿进行优化。使用点面特征跟踪构建的联合位姿估计函数为：

进一步，所述步骤S4当处理完所有图像之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化，具体包括步骤：

通过最小化重投影误差进行局部BA来进一步优化关键帧和局部地图点的位姿，最小化重投影误差，即将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差进行求和，构建最小二乘问题，然后寻找最好的相机位姿，利用非线性优化的方法使其最小化。

进一步，所述步骤S5筛选候选关键帧，对冗余的关键帧进行剔除，具体包括步骤：

确定当前帧是否应作为关键帧生成，选取关键帧的规则如下：

(1)当前帧至少跟踪到30个地图点；

(2)当前帧跟踪到的地图点少于参考关键帧的90％；

(3)距上一个关键帧插入之后已跟踪超过20帧图像。

(4)当前帧跟踪到的垂直与平行平面个数大于n时，

其中：R为当前帧到上一个关键帧的旋转变化矩阵，n_i为当前帧垂直平面特征法向量，n_j为当前帧平行平面特征法向量，n′_i为上一个关键帧垂直平面特征法向量，n′_j为上一个关键帧平行平面特征法向量，旋转角度为当旋转角度大于阈值时选入当关键帧。

确定是否为冗余关键帧应该删除规则如下：

如果一个关键帧观测到的地图点有90％以上被至少其他3个关键帧观测到，那么个键帧被定为冗余关键帧，对其进行删除处理。

进一步，所述步骤S6在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环。

进一步，所述步骤S7，采用位姿图优化的方法对相机运动的累积误差进行校正，即利用位姿图中的节点表示相机位姿，边表示两个位姿节点之间的相对运动估计，所有的位姿顶点和位姿-位姿边构成了一个图优化，本质上是一个最小二乘法问题，优化变量为各个顶点的位姿，边来自于为位姿观测约束，最后利用g2o(General Graphic Optimization，G2O)进行求解，从而校正累计误差。

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

进一步，所述步骤S9具体为：

在八叉树建图过程中根节点的占据概率由子节点的概率计算得出，具体为：计算根节点的方法有两种：求平均值法和求最大值法/>L(n_i)表示某个节点，表示某节点的概率对数值。在点云地图创建完成后，把输出的pcd格式点云转换成octomap格式的点云，进而生成Octomap地图。

(三)有益效果

与现有技术相比，本发明提供了一种融合直接法与点面特征法的室内RGB-D SLAM方法，具备以下有益效果：

1、主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对图像帧进行跟踪，通过融合最小化光度图像误差与最小深度图像误差来进行相机的初始位姿估计，ORB点特征与平面特征提取与匹配，利用最小化局部地图特征重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位。新的关键帧选取方法，利用“曼哈顿世界”模型，通过当前图像帧与关键帧旋转角度只差判断是否插入新的关键帧；在局部建图中对冗余关键帧进行剔除。在闭环线程中执行对关键帧的闭环检测和优化，从而保证SLAM全局一致性。另外，根据RGB-D图像和相机位姿信息，通过基于Octomap的建图框架，构建完整准确的3D稠密环境地图。

附图说明

图1为本发明实施例的一种融合直接法与点面特征法的室内RGB-D SLAM方法的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

实施例

本发明提供一种融合直接法与点面特征法的室内RGB-D SLAM方法。本发明所述的融合直接法与点面特征法的室内RGB-D SLAM方法，为了实现稀疏纹理环境下的视觉快速定位，在环境纹理特征缺少甚至缺失的时候，仍具有较高的精度和鲁棒性。该算法主要包含3个并行线程：跟踪线程、局部建图和闭环线程。在跟踪线程中对图像帧进行跟踪，通过融合最小化光度图像误差与最小深度图像误差来进行相机的初始位姿估计，ORB点特征与平面特征提取与匹配，根据“曼哈顿世界”模型，将平面特征根据平行平面与垂直平面进行匹配，使用重投影误差获取相机位姿，再利用最小化局部地图特征重投影误差进一步优化相机位姿，实现快速准确的跟踪与定位。新的关键帧选取方法，通过当前图像帧与关键帧旋转角度只差判断是否插入新的关键帧；在局部建图中对冗余关键帧进行剔除。在闭环线程中执行对关键帧的闭环检测和优化，从而保证SLAM全局一致性。另外，根据RGB-D图像和相机位姿信息，通过基于Octomap的建图框架，构建完整准确的3D稠密环境地图。下面结合附图，对本发明作进一步的详细说明。

如图1所示，本发明的实现流程图包括如下：

步骤1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将初始化成功的第一帧作为关键帧，并将第一关键帧作设定初始位姿为单位旋转，平移为0m，在关键帧中提取点特征与面特征，利用其在深度图像中的深度值构造一系列3D点与3D平面，创建一个初始化地图；

步骤2，对步骤1中的RGB彩色数据与深度数据，通过联合最小化光度误差与最小深度误差来进行相机的位姿预估计；

在跟踪线程通过前一彩色图像帧与深度图像帧提取的ORB特征点，得到融合最小光度误差与最小深度误差来对当前帧的位姿进行预估计。当前帧与前一帧的相对位姿T_cr为

其中：为最小光度误差/>为最小深度误差，/>当前帧某一点的灰度值，/>上一帧提取的关键点的灰度值，/>为当前帧某一点深度值，/>为前一帧某一特征点深度值。/>为Huber核。最后利用L-M(Levenberg-Marquadt)算法求解上式，L-M(Levenberg-Marquadt)

算法步骤如下：

(1)给定初始点x₀，允许误差ε＞0，设置k＝0；

(2)f′(x^(k))＜ε，导数接近0即在极小点附近，得到x，算法结束，否则继续；

(3)计算x^(k+1)，

步骤3，对当前图像帧中的ORB特征与面特征进行提取与匹配，然后使用BA只对位姿进行优化，筛选出当前图像帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点地图平面，包括以下几个步骤：

使用ORB特征点进行匹配，利用匹配的特征点来构建最小重投影误差为三维空间点，K为相机内参，T为相机位姿。

u_i＝[u_i,v_i]^T为其投影的像素坐标点。

使用AHC算法(层次聚类算法)提取当前帧的面特征，并以π＝(n_x,n_y,n_z,d)输出，n＝(n_x,n_y,n_z)为面特征的法向量，d是相机坐标原点到平面的垂直距离。平面参数π＝(n_x,n_y,n_z,d)是过参化表示，过参化会使信息矩阵秩亏。所以将平面转为表示平面法向量在OXY平面上的投影与OX轴之间的夹角，ψ＝arctan(n_y/n_x)为平面法向量与OZ轴之间的夹角。

面特征通过几何角度约束与几何距离约束匹配。几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值10°，则满足平面间夹角约束。|θ₁-θ₂|≥10°

几何距离约束：计算观测平面与路标平面间的距离，若距离小于设定距离阈值10cm，则满足平面间距离约束。|d₁-d₂|≥10cm

根据面特征匹配结果构建误差函数其中π_w为地图平面特征，π_m为当前帧地图平面特征，T_cw为地图坐标到当前帧的位姿变换。

此外，室内环境中存在明显的结构特征，一些论文中也利用了曼哈顿世界假设，认为所有法向量只分布在三个互相垂直的主轴上。但这一假设太强，这里做了简化。本发明搜索了地图中的平行或垂直的平面，并以此添加约束关系。平行或垂直面的搜索是小于一定的角度差异阈值即可。这里在添加约束时，应该为地图中的平面特征之间构建，但这样的话无法对前端中的相机位姿求解带来帮助，因此我们将这约束关系也转化到相机与对应的平面之间。

垂直平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差大于设定角度阈值85°，则满足垂直平面间夹角约束。|θ₁-θ₂|≥85°

根据垂直平面匹配构建误差函数e_so(n_w,T_cw)＝R_⊥n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换，R_⊥为90度旋转矩阵。

平行平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值5°，则满足平行平面间夹角约束。|θ₁-θ₂|≤5°

根据平行平面匹配构建误差函数e_sp(n_w,T_cw)＝n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换。

点特征和平面特征的匹配关系均可优化当前帧位姿。局部地图中包含关键帧、点路标、平面路标，使用所有点和平面对当前位姿进行优化。使用点面特征跟踪构建的联合位姿估计函数为：

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

通过最小化重投影误差进行局部BA来进一步优化关键帧和局部地图点的位姿，最小化重投影误差，即将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差进行求和，构建最小二乘问题，然后寻找最好的相机位姿，利用非线性优化的方法使其最小化。

步骤5，筛选候选关键帧，具体包括步骤：

确定当前帧是否应作为关键帧生成，选取关键帧的规则如下：

(1)当前帧至少跟踪到30个地图点；

(2)当前帧跟踪到的地图点少于参考关键帧的90％；

(3)距上一个关键帧插入之后已跟踪超过20帧图像。

(4)当前帧跟踪到的垂直与平行平面个数大于n时，

其中：R为当前帧到上一个关键帧的旋转变化矩阵，n_i为当前帧垂直平面特征法向量，n_j为当前帧平行平面特征法向量，n′_i为上一个关键帧垂直平面特征法向量，n′_j为上一个关键帧平行平面特征法向量，旋转角度为当旋转角度大于阈值时选入当关键帧。

步骤6，筛选出当前关键帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点；

步骤7，当处理完所有关键帧之后，执行局部BA优化，对周围的关键帧位姿以及地图点位置进行优化，以获得更加准确的定位与建图精度；

步骤8，对冗余的关键帧进行剔除，规则如下：

如果一个关键帧观测到的地图点有90％以上被至少其他3个关键帧观测到，那么个键帧被定为冗余关键帧，对其进行删除处理。

步骤9，在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环，当检测到闭环时，对闭环周期进行判断，若当前关键帧与闭环关键帧不属于同一个周期，则将闭环关键帧的周期与当前关键帧的周期合并为同一周期；

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

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

步骤12，在八叉树建图过程中根节点的占据概率由子节点的概率计算得出，具体为：计算根节点的方法有两种：求平均值法和求最大值法/>L(n_i)表示某个节点，表示某节点的概率对数值。在点云地图创建完成后，把输出的pcd格式点云转换成octomap格式的点云，进而生成Octomap地图。/>

Claims (9)

1.一种融合直接法与点面特征法的室内RGB-D SLAM方法，其特征在于，包括以下：

步骤S1，初始化，并利用RGB-D相机采集室内环境的RGB彩色数据与Depth深度数据，将初始化成功的第一帧作为关键帧，并将第一关键帧作设定初始位姿为单位旋转，平移为0m，在关键帧中提取点特征与面特征，利用其在深度图像中的深度值构造一系列3D点与3D平面，创建一个初始化地图；

步骤S2，对步骤S1中的RGB彩色数据与深度数据，通过联合最小化光度误差与最小深度误差来进行相机的位姿预估计；

步骤S3，对当前图像帧中的ORB(Oriented FAST and Rotated BRIEF)特征与面特征进行提取与匹配，然后使用BA只对位姿进行优化，筛选出当前图像帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点地图平面；

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

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

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

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

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

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

2.根据权利要求1所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S2相机的位姿预估计包括：

在跟踪线程通过前一彩色图像帧与深度图像帧提取的ORB特征点，得到融合最小光度误差与最小深度误差来对当前帧的位姿进行预估计。当前帧与前一帧的相对位姿T_cr为

其中：为最小光度误差/>为最小深度误差，/>当前帧某一点的灰度值，/>上一帧提取的关键点的灰度值，/>为当前帧某一点深度值，/>为前一帧某一特征点深度值。/>为Huber核。最后利用L-M(Levenberg-Marquadt)算法求解上式，L-M(Levenberg-Marquadt)

算法步骤如下：

(1)给定初始点x₀，允许误差ε＞0，设置k＝0；

(2)f′(x^(k))＜ε，导数接近0即在极小点附近，得到x，算法结束，否则继续；

(3)计算x^(k+1)，

3.根据权利要求1所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S3对当前图像帧中的ORB特征与面特征进行提取与匹配，据“曼哈顿世界”模型，将平面特征根据平行平面与垂直平面进行匹配，然后使用BA只对位姿进行优化，筛选出当前图像帧中冗余的地图点，由特征提取与匹配的结果来创建新的地图点地图平面，包括以下几个步骤：

使用ORB特征点进行匹配，利用匹配的特征点来构建最小重投影误差

其中：为三维空间点，K为相机内参，T为相机位姿；

u_i＝[u_i,v_i]^T为其投影的像素坐标点；

使用AHC算法(层次聚类算法)提取当前帧的面特征，并以π＝(n_x,n_y,n_z,d)输出，n＝(n_x,n_y,n_z)为面特征的法向量，d是相机坐标原点到平面的垂直距离；平面参数π＝(n_x,n_y,n_z,d)是过参化表示，过参化会使信息矩阵秩亏；所以将平面转为表示平面法向量在OXY平面上的投影与OX轴之间的夹角，ψ＝arctan(n_y/n_x)为平面法向量与OZ轴之间的夹角；

面特征通过几何角度约束与几何距离约束匹配。几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值10°，则满足平面间夹角约束。|θ₁-θ₂|≥10°

几何距离约束：计算观测平面与路标平面间的距离，若距离小于设定距离阈值10cm，则满足平面间距离约束；|d₁-d₂|≥10cm

根据面特征匹配结果构建误差函数其中π_w为地图平面特征，π_m为当前帧地图平面特征，T_cw为地图坐标到当前帧的位姿变换；

此外，室内环境中存在明显的结构特征，一些论文中也利用了曼哈顿世界假设，认为所有法向量只分布在三个互相垂直的主轴上。但这一假设太强，这里做了简化。本发明搜索了地图中的平行或垂直的平面，并以此添加约束关系。平行或垂直面的搜索是小于一定的角度差异阈值即可；这里在添加约束时，应该为地图中的平面特征之间构建，但这样的话无法对前端中的相机位姿求解带来帮助，因此我们将这约束关系也转化到相机与对应的平面之间；

垂直平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差大于设定角度阈值85°，则满足垂直平面间夹角约束；|θ₁-θ₂|≥85°

根据垂直平面匹配构建误差函数e_so(n_w,T_cw)＝R_⊥n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换，R_⊥为90度旋转矩阵；

平行平面匹配：

几何角度约束：计算观测平面法向量与路标平面法向量间的夹角，若夹角差小于设定角度阈值5°，则满足平行平面间夹角约束；|θ₁-θ₂|≤5°

根据平行平面匹配构建误差函数e_sp(n_w,T_cw)＝n_m-R_cwn_w，其中n_w为地图平面特征法向量，n_m为当前帧地图平面特征法向量，T_cw为地图坐标到当前帧的位姿变换；

点特征和平面特征的匹配关系均可优化当前帧位姿。局部地图中包含关键帧、点路标、平面路标，使用所有点和平面对当前位姿进行优化。使用点面特征跟踪构建的联合位姿估计函数为：

4.根据权利要求3所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S4当处理完所有图像之后，执行局部BA光束平差法优化，对周围的关键帧位姿以及地图点位置进行优化，具体包括步骤：

通过最小化重投影误差进行局部BA来进一步优化关键帧和局部地图点的位姿，最小化重投影误差，即将像素坐标(观测到的投影位置)与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差进行求和，构建最小二乘问题，然后寻找最好的相机位姿，利用非线性优化的方法使其最小化。

5.根据权利要求3所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S5，筛选候选关键帧，对冗余的关键帧进行剔除，具体包括步骤：

确定当前帧是否应作为关键帧生成，选取关键帧的规则如下：

(1)当前帧至少跟踪到30个地图点；

(2)当前帧跟踪到的地图点少于参考关键帧的90％；

(3)距上一个关键帧插入之后已跟踪超过20帧图像。

(4)当前帧跟踪到的垂直与平行平面个数大于n时，

其中：R为当前帧到上一个关键帧的旋转变化矩阵，n_i为当前帧垂直平面特征法向量，n_j为当前帧平行平面特征法向量，n′_i为上一个关键帧垂直平面特征法向量，n′_j为上一个关键帧平行平面特征法向量，旋转角度为当旋转角度大于阈值时选入当关键帧。

确定是否为冗余关键帧应该删除规则如下：

如果一个关键帧观测到的地图点有90％以上被至少其他3个关键帧观测到，那么个键帧被定为冗余关键帧，对其进行删除处理。

6.根据权利要求5所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S6在闭环检测线程中，利用ORB特征在DBoW2上构建的词袋模型来检测闭环。

7.根据权利要求6所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S7，采用位姿图优化的方法对相机运动的累积误差进行校正，即利用位姿图中的节点表示相机位姿，边表示两个位姿节点之间的相对运动估计，所有的位姿顶点和位姿-位姿边构成了一个图优化，本质上是一个最小二乘法问题，优化变量为各个顶点的位姿，边来自于为位姿观测约束，最后利用g2o(General Graphic Optimization，G2O)进行求解，从而校正累计误差。

8.根据权利要求7所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤步骤S8，将步骤S6中周期内的所有关键帧位姿与所有地图点位置与面特征作为优化变量执行全局BA优化，从而得到全局一致的环境地图和相机运动轨迹。

9.根据权利要求8所述的一种融合直接法与点面特征法的室内RGB-DSLAM方法，其特征在于：所述步骤S9具体为：

在八叉树建图过程中根节点的占据概率由子节点的概率计算得出，具体为：计算根节点的方法有两种：求平均值法和求最大值法/>L(n_i)表示某个节点，表示某节点的概率对数值。在点云地图创建完成后，把输出的pcd格式点云转换成octomap格式的点云，进而生成Octomap地图。