4.0ORBSLAM3之局部建图线程概述

超爱吃小蛋糕的66

已于 2023-06-30 15:02:36 修改

阅读量714

点赞数 1

分类专栏： ORB-SLAM3算法代码及原理解析文章标签：性能优化自动驾驶人工智能计算机视觉

于 2023-06-30 14:57:14 首次发布

本文链接：https://blog.csdn.net/guanjing_dream/article/details/131476812

版权

ORB-SLAM3算法代码及原理解析专栏收录该内容

25 篇文章 34 订阅

订阅专栏

1.简介

局部建图线程是ORBSLAM3的核心线程之一，在初始化SLAM系统时被创建和启动，主要作用是为跟踪线程(跟踪局部地图)以及回环检测线程(回环检测)服务，并进行局部地图优化以及时消除轨迹的累计误差。局部建图线程主要维护一个由共视图Covisibility Graph、本质图Essential Graph和生成树Spanning Tree组成的局部地图，它包含了当前帧的共视关键帧以及关键帧之间的连接关系和关键帧对应的3D空间点(这些点都在世界坐标系下)。

跟踪线程负责向局部建图线程传递关键帧，当有新的关键帧传来时，根据当前局部建图线程的状态(是否在处理关键帧，是否在进行BA优化等)决定要不要接受该关键帧，如果拒绝接收跟踪线层会取消创建关键帧。当新的关键帧插入到局部地图中时，会对局部地图中的关键帧以及关键帧之间的连接关系进行更新，同时对当前局部地图中关键帧的位姿和地图点进行BA优化，最后对冗余的关键帧及其对应的地图点进行删除。

此外，在IMU模式下局部建图线程需要完成IMU的初始化(主要对偏置信息进行优化)和视觉惯导联合初始化。当IMU未完成初始化时局部建图线程优化模块只进行纯视觉BA，当IMU完成初始化后进行视觉-惯导联合BA优化

本章对局部地图中维护的几种关键帧连接关系进行了介绍，并对局部建图线程的关键模块进行分析，主要包括以下内容：

处理列表中新的关键帧ProcessNewKeyFrame
根据地图点的观测情况剔除质量不好的地图点MapPointCulling
当前关键帧与相邻关键帧通过三角化产生新的地图点CreateNewMapPoints
检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点SearchInNeighbors
IMU初始化与联合BA优化
检测并剔除当前帧相邻的关键帧中冗余的关键帧KeyFrameCulling
尺度和重力方向优化

局部建图线程涉及的更多的是对关键帧、地图点在逻辑上的一些处理即代码设计，因此除了比较繁杂的部分对每个关键模块只作简单概述，代码中的逻辑还是比较清晰的。
在这里插入图片描述

2.局部地图中维护的几种图

1.Covisibility Graph
对于某个区域的3D地图点，投影在图像上就是特征点，就是该图像观测到了这个地图点。如果这个3D地图点被两个图像观测到，就称这两张图像存在共视关系。
在共视图Covisibility Graph中，每个节点代表一个关键帧，共视关键帧之间通过权重θ进行连接，权重θ即两个关键帧共视地图点的数量。在ORB-SLAM中只有当共视地图点大于15时才会进行连接(15是作者实验出的经验值)。
在这里插入图片描述

2.Essential Graph
本质图Essential Graph主要为回环检测服务，Essential Graph包含了一个生成树，一个高covisibility（θmin=100）的covisibility graph边缘子集，以及闭环回路的边缘，这样的组合共同构建了一个强大的相机网络。
在这里插入图片描述

3.生成树Spanning Tree：
在这里插入图片描述

每个关键帧都会存在很多和共视关键帧，在进行连接时将某个关键帧指向和其共视关系最好、权重最大的关键帧，根据这种规则从初始关键帧开始创建，就可以的到一个生成树。
如图，5和3、4存在共视关系，但是5和4之间共视关系最好，所以将箭头由5指向3.
在这里插入图片描述

3.关于共视图和滑动窗口法的一点思考

基于共视图原理的局部地图和滑窗法的主要区别在与对老数据的处理方式不同。

局部地图根据共视关系确定优化变量(位姿，地图点)；其次，通过在优化中固定一些变量(如orb-slam 固定了和局部地图存在共视但不和当前关键帧直接相连的关键帧的位姿)，提供先前信息的约束。
滑动窗口(以VINS为例)通过相对固定的窗口大小对待优化参数的数量进行限制，平衡计算量；通过边缘化得到的先验在优化中保留一些旧的状态的信息。

基于局部的优化不会出现线性化点不一致的问题在于局部地图优化不会进行边缘化,因为共视图可以链接到比较久远的环境信息,当共视图中的某个关键帧与当前帧共视信息比较少时说明该帧和当前帧已经相距很远了,这时优化时直接丢弃该关键帧的信息也不会对当前帧的位姿优化造成影响.

4.关键模块介绍

4.1 处理缓存队列中的关键帧

该函数主要包括以下几个部分：

计算该关键帧特征点的Bow信息
更新当前关键帧新增地图点的属性
更新共视图中关键帧间的连接关系
将该关键帧插入到地图中

详细见博客《处理缓存队列中的新关键帧》

4.2 根据地图点的观测情况剔除质量不好的地图点

需要删除的地图点类型：

跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%，(mnFound/mnVisible） < 25%

mnFound ：该地图点有对应特征点的帧数
mnVisible：地图点应该被看到的次数，表示该地图点在视野范围内
(mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低

从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数却不超过cnThObs帧，那么删除该点

    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        // Step 2.1：已经是坏点的MapPoints直接从检查链表中删除
        if(pMP->isBad())
            lit = mlpRecentAddedMapPoints.erase(lit);
        else if(pMP->GetFoundRatio()<0.25f)
        {
            // Step 2.2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%，删除
            // (mnFound/mnVisible） < 25%
            // mnFound ：该地图点有对应特征点的帧数
            // mnVisible：地图点应该被看到的次数，表示该地图点在视野范围内
            // (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            // Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧
            // 但是观测到该点的关键帧数却不超过cnThObs帧，那么删除该点
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        // Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点
        // 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
        {
            lit++;
            borrar--;
        }
    }

4.3 与相邻关键帧产生新的额地图点

void CreateNewMapPoints();

在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
之前在处理局部建图线程中的新关键帧时，最后会更新该关键帧共视图的连接关系，其中一项操作就是按照共视权重对关键帧的共视关键帧进行排序，因此这里可以比较简单的获取共视程度最高的nn帧关键帧。
忽略与当前帧平移距离较小的关键帧(恢复出的3D点不稳定)
通过bow词袋向量加速特征匹配，然后根据对极约束原理校验匹配是否合理
如下图所示，在初步确定匹配点对后，会计算当前帧特征点在参考帧上投影点到极线l2的距离是否小于一定阈值，理想情况下P点在参考帧上的投影点会和p2重合即距离为0,真实情况下在相对关系确定时这个距离越小说明匹配就越合拍
对每对匹配通过三角化生成3D点
对于生成3D点的途径主要有2种，根据视差角的情况进行选择。当两帧之间的视差角比较大时选择通过三角化恢复3D点，具体原理在《初始化模块》已经介绍的比较多了;当双目相机本身左右目视差角比较大时，则选择基于内参信息通过反投影来恢复。
分别检验3D点在当前关键帧和参考关键帧上的重投影误差并进行卡方检验，对于双目相机只需要计算x方向上的误差
忽略深度值较远的点，并进行尺度一致性的检验
地图上的一些远点精度比较低，之前在逆深度 部分讲过，真实世界中对于100m和50m处不同的深度点，表现在图像上可能就几个像素的差距，这种比例是严重失衡的，因此在一些算法中采用逆深度作为优化的对象。

4.4融合重复的地图点

void SearchInNeighbors();

1.要融合的对象： 当前关键帧对应的地图点与其相邻关键帧中的地图点。

2.融合原则：

如果地图点能匹配关键帧的特征点，并且该特征点有对应的地图点，那么选择观测数目多那个地图点作为两帧共有的地图点
如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点

3.核心匹配函数：
该函数同样使用了特征匹配加速策略，根据特征点网格注册信息，对于某个Mappoint根据位姿投影到共视关键帧中得到投影点，加速策略就是只在投影点附近的网格中寻找Mappoint的匹配点，缩小了匹配范围。

int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th, const bool bRight);

4.5 删除冗余的关键帧

删除原则：
该关键帧中90%以上的地图点能被其他关键帧（至少3个）观测到，判定为冗余关键帧。

4.6 IMU的初始化以及与视觉的BA联合优化

详情见博客《3.3视觉与IMU联合初始化》