CN108520554A - 一种基于orb-slam2的双目三维稠密建图方法 - Google Patents

Abstract

本发明公开了一种基于ORB‑SLAM2的双目稠密建图方法，涉及机器人同步定位与地图创建领域，该方法主要由跟踪线程、局部地图线程、闭环检测线程和稠密建图线程组成。其中稠密建图线程包含以下步骤：1)场景深度范围估计，限制立体匹配计算量；2)立体匹配，估计每个像素点的逆深度值；3)帧内平滑、外点剔除，增加逆深度图的稠密度的同时保留深度边界的不连续性；4)逆深度融合，对上一步获得的逆深度进行帧间优化；5)帧内平滑、外点剔除。本方法只利用CPU，实现室内、室外环境的稠密地图创建。

Description

一种基于ORB-SLAM2的双目三维稠密建图方法

技术领域

本发明属于机器人同步定位与地图创建领域，涉及一种基于ORB-SLAM2的双目三维稠密建图方法。

背景技术

同步定位与地图创建(Simultaneous Localization and Mapping,SLAM)是指移动机器人在没有环境先验信息的情况下，通过自身搭载的传感器，于运动过程中建立所处环境的地图模型，同时估计自身的运动。SLAM同时包含定位与建图两个问题。被认为是实现机器人自主性的关键问题之一，对机器人的导航、控制、任务规划等领域有重要的研究意义，同时它也是二十一世纪机器人领域十大进展最快的研究问题，被誉为机器人研究的“圣杯”。

针对现有的SLAM系统，大都研究的是定位问题，包括通过特征点的定位、直接法的定位，以及后端优化等，对建图模块不是很重视，建立的稀疏特征点地图也主要是服务于定位问题。但是在具体应用中，地图的用途不仅仅用于辅助定位，其明显还带有许多其他的需求。如：对机器人进行路径规划(即导航)、避障等。需要告知机器人地图中哪些地方可以通行，而哪些地方不能通行。这就超出了稀疏特征点地图的能力范围，需要建立一种稠密的地图。

因此，本发明提出了一种基于ORB-SLAM2的双目稠密建图方法。

发明内容

本发明的目的在于提供一种不依赖GPU并行计算，只利用CPU恢复环境三维稠密地图的方法。

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

一种基于ORB-SLAM2的双目三维稠密建图方法，算法主要由以下四个线程组成。

1.跟踪线程：对左、右图像提取ORB特征并进行匹配，三角化建立特征关联的地图点深度和三维坐标。后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿。通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧。

2.局部地图线程：处理跟踪线程创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点。当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度。

3.闭环检测线程：处理局部地图线程插入的关键帧，主要包含三个过程，分别是闭环检测、计算相似变换矩阵和闭环矫正。闭环检测通过计算词带相似得分选取候选关键帧。随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(Essential Graph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹。

4.稠密建图线程：对新建立的关键帧执行地图点深度范围搜索，随后在该深度范围内建立匹配代价量，执行立体匹配得到关键帧初始深度图。基于相邻像素深度相近原则，对获得的初始深度图进行相邻像素逆深度融合和填充空缺像素点，通过相邻关键帧深度图融合优化深度信息，进一步执行帧内填充和外点剔除得到最终深度图，最后利用点云拼接得到环境稠密地图。

其中，稠密建图线程具体方法如下所述：

步骤1：场景深度范围估计：将双目中的左相机图像作为关键帧输入图像，对任意时刻关键帧观测到的每一个地图点，将其投影到关键帧图像中，计算地图点在关键帧坐标系下的深度值。选取最大最小深度设置场景逆深度搜索范围，即(ρ_min,ρ_max)。

步骤2：立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度。通过步骤1计算的场景深度范围限制立体匹配中匹配代价量(costvolume)的层数，只在逆深度(ρ_min,ρ_max)对应的视差范围内搜索，减少计算量。同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度。

步骤3：帧内平滑、外点剔除：对步骤2得到的逆深度图进行填充和剔除。

步骤4：逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息。

步骤5：帧内平滑、外点剔除：对逆深度融合后获得的逆深度图进行逆深度点的填充与剔除。

本发明的有益效果：

本发明提出了一种基于ORB-SLAM2的双目三维稠密建图方法，只利用CPU运算，实现室内、室外环境的地图重建。通过双目相机实时采集图像，对ORB-SLAM2跟踪线程创建的关键帧进行场景深度估计，只在该深度范围内构建匹配代价量，大大减少了立体匹配花费时间。基于相邻像素深度相近原则，在获得初始深度图后进行帧内平滑与外点剔除环节，增加了深度图的稠密度和剔除了可能存在的孤立的匹配点。针对立体匹配获得的初始深度估计精度不高，且出现严重的视差分层现象，本发明提出了一种多关键帧逆深度融合方法，对每个候选逆深度假设进行高斯融合，优化当前关键帧深度值。

附图说明

图1是本发明算法框架示意图。

图2是本发明地图点深度计算示意图。

图3是本发明逆深度融合前后稠密地图示意图。

图4是本发明KITTI数据(序列08，14，18)点云图对比示意图。

图5是本发明最终场景点云图与八叉树地图。

具体实施方式

为了更好地理解本发明的技术方案，以下结合附图作进一步描述。如图1所示，一种基于ORB-SLAM2的双目三维稠密建图方法，包含以下步骤：

步骤1：场景深度范围估计：将双目中的左相机图像作为关键帧输入图像，对任意时刻关键帧观测到的每一个地图点，将其投影到关键帧图像中，计算地图点在关键帧坐标系下的深度值。选取最大最小深度设置场景逆深度搜索范围。

如图2所示，令P_i为地图点在世界坐标系下的3D坐标的齐次表示，T_k,w∈SE(3)为k时刻相机坐标系与世界坐标系的位姿变换，为地图点在k时刻相机坐标系下的3D坐标的齐次表示。场景深度搜索范围(ρ_min,ρ_max)定义为

p_i＝[x_i y_i z_i 1]^T

其中n为在k时刻关键帧中能观测到的地图点个数。

步骤2：立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度。通过步骤1计算的场景深度范围限制立体匹配中匹配代价量(cost volume)的层数，只在逆深度(ρ_min,ρ_max)对应的视差范围内搜索，减少计算量。同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度。

步骤3：帧内平滑、外点剔除：假设立体匹配获得的视差服从方差为1的高斯分布，即

d:N(d₀,1)

其中d₀为立体匹配计算得到的视差值，f为相机焦距，b为基线，z为像素深度值，ρ为逆深度。变换后逆深度分布为

对立体匹配阶段得到的逆深度图进行填充和剔除孤立外点。具体步骤如下：

1)对每一个存在逆深度分布的像素点，计算它与周围8个逆深度分布满足卡方分布小于5.99的个数，如公式(1)所示，当个数小于2时，将其逆深度剔除，当个数大于2时，将满足卡方分布要求的逆深度采用公式(2)进行逆深度融合，融合后该像素点的逆深度为ρ_p，方差为融合前的逆深度最小方差。

其中，a、b为以当前像素为中心，周围8个像素。n为满足卡方分布的个数。

2)对于每个不存在逆深度分布的像素点，检测其周围8个像素点之间的逆深度分布是否满足卡方分布，当满足卡方分布的个数大于2时，采用公式(2)进行逆深度融合，同理方差为融合前的逆深度最小方差。

步骤4：逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息。具体步骤如下：

1)将当前关键帧逆深度图对应的地图点投影到相邻关键帧，读取投影点的逆深度ρ₀与逆深度方差

2)将相邻关键帧中逆深度为ρ₀+σ₀、ρ₀与ρ₀-σ₀对应的地图点逆投影到当前关键帧，保留逆投影后的逆深度ρ₁、ρ₂与ρ₃。

3)构造候选融合逆深度假设ρ:N(ρ₂,[max(|ρ₁-ρ₂|,|ρ₃-ρ₂|)]²)。

4)循环上述步骤获得6个候选融合逆深度假设，利用卡方分布小于5.99选取待融合逆深度假设，融合后逆深度ρ_p与方差分别为

其中，p为当前帧像素点，n为待融合逆深度个数。

步骤5：帧内平滑、外点剔除：基于场景中相邻区域的深度相近的假设，对逆深度融合后获得的逆深度图进行帧内平滑和外点剔除，提高地图点的精度和增加点云的稠密度。具体步骤如步骤2的逆深度填充与剔除环节。

为验证本发明提出的逆深度融合在稠密建图线程中的有效性，在包含逆深度融合与剔除逆深度融合两种情况下进行稠密建图，对比地图效果。以EuRoc数据集的V1_01_easy为例，地图效果如图3所示。其中第一行为在不同角度观测下，剔除逆深度融合后建图效果。从图中可以看到在对墙面点云创建中存在大量的不准确三维点致使墙体出现膨胀现象(如椭圆形区域)。且地图中存在大量估计错误的外点(如圆形区域)。与之对比，第二行为采用逆深度融合后的稠密地图，在墙体边缘和对外点处理都有很好的改善。

为评价本发明稠密建图效果，将提出的算法与Stereo LSD-SLAM进行对比。如图4所示，第一行为Stereo LSD-SLAM在KITTI数据集的08序列、14序列和18序列下创建半稠密点云图。第二行为本发明创建的环境地图。从图4可见，本发明对物体轮廓表述更清晰，且对道路场景重建效果突出，如第三行所示，其中椭圆区域为道路表面，其周围离散点云很少，近乎在一个平面。

稠密三维点云地图不能直接用于机器人导航，现将其转化为八叉树地图。如图5为本发明建立的EuRoc数据集中V1_01_easy的点云图和八叉树地图。

Claims (4)

1.一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于，包含以下几个步骤：

步骤(1)场景深度范围估计：将双目中的左相机图像作为关键帧输入图像，对任意时刻关键帧观测到的每一个地图点，将其投影到关键帧图像中，计算地图点在关键帧坐标系下的深度值，选取最大最小深度设置场景逆深度搜索范围，即(ρ_min,ρ_max)；

步骤(2)立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度，通过所述步骤(1)中计算的场景深度范围限制立体匹配中匹配代价量(cost volume)的层数，只在逆深度(ρ_min,ρ_max)对应的视差范围内搜索，减少计算量；同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度；

步骤(3)帧内平滑、外点剔除：对所述步骤(2)中得到的逆深度图进行填充和剔除；

步骤(4)逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息；

步骤(5)帧内平滑、外点剔除：对逆深度融合后获得的逆深度图进行逆深度点的填充与剔除。

2.根据权利要求1所述一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于：所述步骤(1)中场景深度范围估计，包含如下步骤：

令P_i为地图点在世界坐标系下的3D坐标的齐次表示，T_k,w∈SE(3)为k时刻相机坐标系与世界坐标系的位姿变换，为地图点在k时刻相机坐标系下的3D坐标的齐次表示，场景深度搜索范围(ρ_min,ρ_max)定义为

p_i＝[x_i y_i z_i 1]^T

其中n为在k时刻关键帧中能观测到的地图点个数。

3.根据权利要求1所述一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于：所述步骤(3)中，对立体匹配阶段得到的逆深度图进行填充和剔除孤立外点，具体步骤如下：

1)对每一个存在逆深度分布的像素点，计算它与周围8个逆深度分布满足卡方分布小于5.99的个数，如公式(1)所示，当个数小于2时，将其逆深度剔除，当个数大于2时，将满足卡方分布要求的逆深度采用公式(2)进行逆深度融合，融合后该像素点的逆深度为ρ_p，方差为融合前的逆深度最小方差；

其中，a、b为以当前像素为中心，周围8个像素；n为满足卡方分布的个数；

2)对于每个不存在逆深度分布的像素点，检测其周围8个像素点之间的逆深度分布是否满足卡方分布，当满足卡方分布的个数大于2时，采用公式(2)进行逆深度融合，同理方差为融合前的逆深度最小方差。

4.根据权利要求1所述一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于：步骤(4)中逆深度融合，在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息，具体步骤如下：

1)将当前关键帧逆深度图对应的地图点投影到相邻关键帧，读取投影点的逆深度ρ₀与逆深度方差

2)将相邻关键帧中逆深度为ρ₀+σ₀、ρ₀与ρ₀-σ₀对应的地图点逆投影到当前关键帧，保留逆投影后的逆深度ρ₁、ρ₂与ρ₃；

3)构造候选融合逆深度假设ρ:N(ρ₂,[max(|ρ₁-ρ₂|,|ρ₃-ρ₂|)]²)；

4)循环上述步骤获得6个候选融合逆深度假设，利用卡方分布小于5.99选取待融合逆深度假设，融合后逆深度ρ_p与方差分别为

其中，p为当前帧像素点，n为待融合逆深度个数。