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

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

A binocular 3D dense mapping method based on ORB-SLAM2

The invention discloses a binocular dense mapping method based on ORB_SLAM2, which relates to the field of robot synchronous positioning and map creation. The method is mainly composed of tracking thread, local map thread, closed loop detection thread and dense mapping thread. The dense mapping thread includes the following steps: 1) depth range estimation, which limits the computation of stereo matching; 2) stereo matching, which estimates the inverse depth of each pixel; 3) intra-frame smoothing, exterior point elimination, which increases the density of the inverse depth map while preserving the discontinuity of the depth boundary; 4) inverse depth fusion, which is the last step. The inverse depth is used for inter frame optimization; 5) intra frame smoothing and external point elimination. This method only uses CPU to realize dense map building in indoor and outdoor environment.

【技术实现步骤摘要】
一种基于ORB-SLAM2的双目三维稠密建图方法
本专利技术属于机器人同步定位与地图创建领域，涉及一种基于ORB-SLAM2的双目三维稠密建图方法。
技术介绍
同步定位与地图创建(SimultaneousLocalizationandMapping,SLAM)是指移动机器人在没有环境先验信息的情况下，通过自身搭载的传感器，于运动过程中建立所处环境的地图模型，同时估计自身的运动。SLAM同时包含定位与建图两个问题。被认为是实现机器人自主性的关键问题之一，对机器人的导航、控制、任务规划等领域有重要的研究意义，同时它也是二十一世纪机器人领域十大进展最快的研究问题，被誉为机器人研究的“圣杯”。针对现有的SLAM系统，大都研究的是定位问题，包括通过特征点的定位、直接法的定位，以及后端优化等，对建图模块不是很重视，建立的稀疏特征点地图也主要是服务于定位问题。但是在具体应用中，地图的用途不仅仅用于辅助定位，其明显还带有许多其他的需求。如：对机器人进行路径规划(即导航)、避障等。需要告知机器人地图中哪些地方可以通行，而哪些地方不能通行。这就超出了稀疏特征点地图的能力范围，需要建立一种稠密的地图。因此，本专利技术提出了一种基于ORB-SLAM2的双目稠密建图方法。
技术实现思路
本专利技术的目的在于提供一种不依赖GPU并行计算，只利用CPU恢复环境三维稠密地图的方法。为实现上述目的，本专利技术提供如下技术方案：一种基于ORB-SLAM2的双目三维稠密建图方法，算法主要由以下四个线程组成。1.跟踪线程：对左、右图像提取ORB特征并进行匹配，三角化建立特征关联的地图点深度和三维坐标。后续图像通过跟踪运动模型、参考关键帧和重定位估计相机位姿。通过最小重投影误差优化当前帧位姿，随后根据预设条件判断是否生成新的关键帧。2.局部地图线程：处理跟踪线程创建的关键帧，更新地图点与关键帧之间的对应关系，剔除地图中新添加的但被观测量少的地图点，随后对共视程度高的关键帧通过三角化恢复地图点，检查关键帧与相邻关键帧的重复地图点。当关键帧队列的所有关键帧处理完毕，对当前关键帧、相邻关键帧和观测到的地图点进行局部捆集调整，通过最小重投影误差优化关键帧位姿和地图点精度。3.闭环检测线程：处理局部地图线程插入的关键帧，主要包含三个过程，分别是闭环检测、计算相似变换矩阵和闭环矫正。闭环检测通过计算词带相似得分选取候选关键帧。随后对每个候选关键帧计算相似变换矩阵，通过随机采样一致性选取最优的关键帧，而后通过本质图(EssentialGraph)优化关键帧位姿，最后执行全局捆集调整得到全局一致性环境地图和相机运行轨迹。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所示，令Pi为地图点在世界坐标系下的3D坐标的齐次表示，Tk,w∈SE(3)为k时刻相机坐标系与世界坐标系的位姿变换，为地图点在k时刻相机坐标系下的3D坐标的齐次表示。场景深度搜索范围(ρmin,ρmax)定义为pi＝[xiyizi1]T其中n为在k时刻关键帧中能观测到的地图点个数。步骤2：立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度。通过步骤1计算的场景深度范围限制立体匹配中匹配代价量(costvolume)的层数，只在逆深度(ρmin,ρmax)对应的视差范围内搜索，减少计算量。同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度。步骤3：帧内平滑、外点剔除：假设立体匹配获得的视差服从方差为1的高斯分布，即d:N(d0,1)其中d0为立体匹配计算得到的视差值，f为相机焦距，b为基线，z为像素深度值，ρ为逆深度。变换后逆深度分布为对立体匹配阶段得到的逆深度图进行填充和剔除孤立外点。具体步骤如下：1)对每一个存在逆深度分布的像素点，计算它与周围8个逆深度分布满足卡方分布小于5.99的个数，如公式(1)所示，当个数小于2时，将其逆深度剔除，当个数大于2时，将满足卡方分布要求的逆深度采用公式(2)进行逆深度融合，融合后该像素点的逆深度为ρp，方差为融合前的逆深度最小方差。其中，a、b为以当前像素为中心，周围8个像素。n为满足卡方分布的个数。2)对于每个不存在逆深度分布的像素点，检测其周围8个像素点之间的逆深度分布是否满足卡方分布，当满足卡方分布的个数大于2时，采用公式(2)进行逆深度融合，同理方差为融合前的逆深度最小方差。步骤4：逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息。具体步骤如下：1)将当前关本文档来自技高网...

【技术保护点】
1.一种基于ORB‑SLAM2的双目三维稠密建图方法，其特征在于，包含以下几个步骤：步骤(1)场景深度范围估计：将双目中的左相机图像作为关键帧输入图像，对任意时刻关键帧观测到的每一个地图点，将其投影到关键帧图像中，计算地图点在关键帧坐标系下的深度值，选取最大最小深度设置场景逆深度搜索范围，即(ρmin,ρmax)；步骤(2)立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度，通过所述步骤(1)中计算的场景深度范围限制立体匹配中匹配代价量(cost volume)的层数，只在逆深度(ρmin,ρmax)对应的视差范围内搜索，减少计算量；同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度；步骤(3)帧内平滑、外点剔除：对所述步骤(2)中得到的逆深度图进行填充和剔除；步骤(4)逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息；步骤(5)帧内平滑、外点剔除：对逆深度融合后获得的逆深度图进行逆深度点的填充与剔除。

【技术特征摘要】
1.一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于，包含以下几个步骤：步骤(1)场景深度范围估计：将双目中的左相机图像作为关键帧输入图像，对任意时刻关键帧观测到的每一个地图点，将其投影到关键帧图像中，计算地图点在关键帧坐标系下的深度值，选取最大最小深度设置场景逆深度搜索范围，即(ρmin,ρmax)；步骤(2)立体匹配：采用基于水平树结构的可变权重代价聚合立体匹配算法计算像素深度，通过所述步骤(1)中计算的场景深度范围限制立体匹配中匹配代价量(costvolume)的层数，只在逆深度(ρmin,ρmax)对应的视差范围内搜索，减少计算量；同时删除立体匹配中视差后处理步骤，在左右一致性匹配中只保留视差相同的像素点逆深度；步骤(3)帧内平滑、外点剔除：对所述步骤(2)中得到的逆深度图进行填充和剔除；步骤(4)逆深度融合：在跟踪线程已计算关键帧位姿的基础上，通过后续6幅关键帧逆深度图优化当前关键帧深度信息；步骤(5)帧内平滑、外点剔除：对逆深度融合后获得的逆深度图进行逆深度点的填充与剔除。2.根据权利要求1所述一种基于ORB-SLAM2的双目三维稠密建图方法，其特征在于：所述步骤(1)中场景深度范围估计，包含如下步骤：令Pi为地图点在世界坐标系下的3D坐标的齐次表示，Tk,w∈SE(3)为k时刻相机坐标系与世界坐标系的位姿变换，为地图点在k时刻相机坐标系下的3D坐标的齐次表示，场景深度搜索范围(ρmin,ρmax)定义为pi＝[xiyizi1]T其中n为在k时刻关键帧中能观测到的地图点个数。3.根据权利要求1所述一种...

【专利技术属性】
技术研发人员：白瑞林，彭建建，李新，
申请(专利权)人：无锡信捷电气股份有限公司，
类型：发明
国别省市：江苏,32