论文分享-ORBSLAM3中的多地图系统

一只废佐_

于 2023-08-15 10:01:42 发布

阅读量324

点赞数

分类专栏： ORBSLAM2系列文章标签：计算机视觉算法

本文链接：https://blog.csdn.net/zz_0000000/article/details/132267061

版权

ORBSLAM2系列专栏收录该内容

10 篇文章 0 订阅

订阅专栏

ORBSLAM3与ORBSLAM2之间的不同：

添加了IMU相关，可以有单目或双目与IMU将结合的模式，并由于IMU的加入使得单目下视觉惯性模式有了绝对尺度
多地图系统，添加了Atlas地图集，维护当前活跃地图和以前的非活跃地图，实现地图之间的数据关联

Atlas地图集

ORBSLAM3中基于DBoW2词袋建立了关键帧的数据库，实现各个子地图之间的重定位、回环检测等功能。Atlas其实就是由 map 类组成的集合，代码中使用 set 容器存储：

//Atlas类中部分内容
std::set<Map*> mspMaps;                                     // 地图集（包含当前Active map）
std::set<Map*> mspBadMaps;                             // 坏地图集
Map* mpCurrentMap;                                              // Active map  当前活跃地图

// 当前Active map最大关键帧ID + 1 ，就是下一个地图中初始帧的ID
unsigned long int mnLastInitKFidMap;            

KeyFrameDatabase* mpKeyFrameDB;             // 整个地图集共用同一个DBoW数据库
ORBVocabulary* mpORBVocabulary;

Map地图

ORBSLAM3只有一个活跃地图和多个非活跃地图：

Active Map（活跃地图）：当前正在跟踪的图像帧，被选为关键帧后，再LocalMaping线程中与其他共视关键帧一起优化（就是ORBSLAM2中唯一的地图）
Non-active Map（非活跃地图）：跟踪丢失后，保留下来的所有地图

地图创建

程序开始时，会先建立ID为0的地图，并设定为活跃地图，之后的每次地图的创建都会将当前的地图设定为非活跃地图。代码中指定 map 类中的 mIsInUse 指定该地图是否为活跃地图：

//在system的构造函数中创建了地图集Atlas
mpAtlas = new Atlas(0);

//Atlas构造函数
//创建第一个地图，指定了地图第一个关键帧ID：mnLastInitKFidMap
Atlas::Atlas(int initKFid) : mnLastInitKFidMap(initKFid), mHasViewer(false)
{
        mpCurrentMap = static_cast<Map *>(NULL);
        CreateNewMap();
}

//新地图的创建，会先将当前地图设定为非活跃，再重新new一个新的地图
void Atlas::CreateNewMap()
{
        unique_lock<mutex> lock(mMutexAtlas);                            // 锁住地图集
        if (mpCurrentMap)														                   // 如果当前地图有效，先设置为非活跃
        {
                // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是上一个地图最大关键帧id的基础上增加1
                if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
                    mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1;      

                mpCurrentMap->SetStoredMap();                                   // 设置非活跃，其实就是把mIsInUse标记为false
        }
        mpCurrentMap = new Map(mnLastInitKFidMap);  		   //新建地图
        mpCurrentMap->SetCurrentMap();             							   //设置为活跃地图
        mspMaps.insert(mpCurrentMap);               					        //插入地图集
}

地图存储

在整个SLAM过程结束后，会将地图集中的地图进行保存。ORBSLAM3将 Atlas 地图集中关键帧数量最多的地图保存下来

//在main函数的最后，会有地图保存的函数，下面以EuRoC数据集为例
void System::SaveKeyFrameTrajectoryEuRoC(const string &filename)
{
        vector<Map*> vpMaps = mpAtlas->GetAllMaps();
        Map* pBiggerMap;
        int numMaxKFs = 0;
        for(Map* pMap :vpMaps)
        {
                //选择了关键帧数量最多的地图
                if(pMap && pMap->GetAllKeyFrames().size() > numMaxKFs)
                {
                        numMaxKFs = pMap->GetAllKeyFrames().size();
                        pBiggerMap = pMap;
                }
        }
        vector<KeyFrame*> vpKFs = pBiggerMap->GetAllKeyFrames();
        sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

        ofstream f;
        f.open(filename.c_str());
        f << fixed;

        for(size_t i=0; i<vpKFs.size(); i++)
        {
                //保存每个关键帧的位姿
                ...
        }
        f.close();
}

ORBSLAM3中应用

多地图系统最主要的功能就是在跟踪丢失的情况下，能够新建地图，之后根据回环的检测，将地图融合起来。

Tracking线程

多地图系统最主要的优点在于能够在跟踪丢失时候，进行子地图的创建，所以Tracking线程中，我们就只需要关注一下再什么地方重建了地图：

跟踪线程中，时间戳出现异常（比如说出现跳变）
一阶段跟踪丢失（跟踪上一帧）：如果当前活跃地图中关键帧小于10个，认为信息太少，直接重置，否则，存储起来并新建一个地图
二阶段跟踪丢失（跟踪局部地图）：如果关键帧超过5个，认为地图仍有一定价值，存储起来并新建一个地图

跟踪线程中创建地图的函数 CreateMapInAtlas ，内部实际就是调用了 CreateNewMap ，但是重新新建子地图之后，程序会重新初始化（也就是说，新建子地图的世界坐标系是以后续初始化帧为基准）

LocalMapping线程

此线程主要都是做关键帧的增加删除等操作，对于多地图系统中，没有过多的参与（因为LocalMapping线程都是运行在活跃地图中）

Loop&Map Merging线程

前面的部分重点都是维护一个 Atlas 类，而闭环&地图融合部分才是多地图系统的重点部分，主要包括：共同区域检测、地图融合和闭环检测。其中闭环检测与ORBSLAM2基本一致，而其他的部分都有相应的改进。

NewDetectCommonRegions(共同区域检测)：

同时进行闭环和地图融合的检测（闭环：检测到与同一个地图中匹配，地图融合：检测到与不同子地图中匹配）
改进了校验方式，可以减少延时：
- ORBSLAM2中，要先检测时间连续性，也就是需要连续成功匹配三个关键帧，才认为检测到闭环。这种方法牺牲时间提升准确率，也就是响应速度缓慢。
- ORBSLAM3中，先进行几何一致性，当前关键帧的5个共视关键帧中只要有3个匹配成功即认为检测到共同区域，如果不够3个，再检测后面进来关键帧的时间连续性。这种方法以略高的计算成本为代价，提高了召回率和地图精度。
闭环和地图融合是有可能同时检测出来的，但是由于地图融合是在多地图之间运行，优先级比较高，所以同时出现的时候，执行地图融合

MergeLocal(地图融合)：分为纯视觉模式和惯性模式（下面只讲解了纯视觉模式）

为了提高实时性，地图融合分为了两个阶段运行，在两个阶段中间，才开启了LocalMapping线程，并且在地图融合的整个过程中Tracking线程也一直再原来的活跃地图中工作
多了一个融合生成树的操作

CorrectLoop(闭环矫正)：

与ORBSLAM2的过程一致

NewDetectCommonRegions函数

检测共同区域主要是为了找到当前关键帧的闭环或者融合候选关键帧，并求解它们的位姿变换

总共分为三个部分：寻找初始候选关键帧、求解位姿变换和校验候选帧

寻找初始候选关键帧
在ORBSLAM2中使用了3个阈值进行筛选，而ORBSLAM3中进行了简化，只使用了一个相对阈值，并且只提取前3个最佳的候选关键帧。

DetectNBestCandidates 函数的简要流程：

1. 通过词袋找到与当前关键帧具有公共单词的所有关键帧，其中不包括当前关键帧的共视关键帧（找离自己比较远的关键帧，无论哪个地图）
2. 找到公共单词数最多的单词数，使用其0.8倍作为阈值 minCommonWords ，并找到单词数大于这个阈值的关键帧
3. 将上述关键帧对应的共视关键帧作为组，求解上述每个关键帧组的总得分，按照关键帧组的总分进行排序，选取总分最高的几个关键帧组中单个得分最高的关键帧，将其按照是否与当前关键帧在同一地图分类，分为闭环候选关键帧和融合候选关键帧，每种不得多于3个

求解位姿变换
得到了候选关键帧 $K_m$ 后，需要求解 $K_m$ 到当前关键帧 $K_c$ 的相对位姿变换 $T_{cm}$ 。在单目和单目惯性模式中，就是相似变换 $S im (3)$ 。

$S im (3)$ 就是通过两个坐标系中不共线的3对匹配点，确定两个坐标系之间的变换关系。ORBSLAM系列中，通过内点数量进行多次迭代，找到最优解

DetectCommonRegionsFromBoW 函数的简要流程：（注，该函数还包括了校验候选关键帧中的共视几何校验,，放到后面讲解）

1. 设定候选关键帧的局部窗口（为进行求解Sim3变换设定参数）
- 1-1. 选取候选关键帧的前 nNumCovisibles 个共视关系最好的关键帧（1.0版本是10个）
- 1-2. 使用 SearchByBoW 函数找到 候选关键帧窗口 中与 当前关键帧 之间能够匹配的地图点 vpMatchedPoints
2. 将上面参数放入 Sim3Solver 函数求解候选关键帧，得到初始相对位姿 $T_{cm}$
- 2-1. Sim3Solver 函数中，与ORBSLAM2不同的是数据关联方式不同（表示怀疑）
- - 2-1-1. ORBSLAM2中使用1对1的方式，就是当前关键帧与候选关键帧之间进行位姿解算
- - 2-1-2. ORBSLAM3中使用1对N的方式，就是当前关键帧与候选关键帧窗口之间进行位姿解算（传入参数就不同）
3. 反复投影，优化相对位姿 $T_{cm}$
- 3-1. 使用 $T_{cm}$ 和 $T_{mw}$ ，得到初步计算的当前关键帧位姿 $T_{cw}$ （注意这里的世界坐标系，如果是不同的地图，此时 $T_{mw}$ 是候选关键帧所在的世界坐标系坐标，经过位姿传播之后，得到的当前关键帧位姿 $T_{cw}$ 实际上是当前关键帧在候选关键帧世界坐标系中的坐标），使用 SearchByProjection 函数按照8为搜索半径，将当前关键帧与候选关键帧窗口内地图点进行匹配，如果匹配点数量大于 nProjMatches 也就是50，才进行下一步
- 3-2. 使用 OptimizeSim3 函数只优化相对位姿 $T_{cm}$ ，并计算内点数量，内点数量大于 nSim3Inliers 也就是20,才进行下一步
- 3-3. 再次使用 SearchByProjection函数，按照更小的5为半径搜索，依然对当前关键帧与候选关键帧窗口内地图点进行匹配，如果匹配点大于 nProjOptMatches 也就是80，才进行下一步

校验候选帧
此时相对位姿 $T_{cm}$ 已经是比较准的，但是这个相对位姿只是对于当前关键帧和候选关键帧比较准，并不清楚这个相对位姿对于各自的窗口是否准，因此还要校验

1. 共视几何校验（看看当前关键帧的共视关键帧是否能够再相对位姿变换中得到一定数量的匹配点）
- 1-1. 找到当前关键帧的共视关键帧（10个），依次与候选关键窗口内地图点进行搜索匹配，DetectCommonRegionsFromLastKF 函数中其实依然是SearchByProjection函数，只不过使用了更加严格的3半径搜索，并内点数量要大于 nProjMatches 也就是30，才算这个共视关键帧校验成功
- 1-2. 如果共视关键帧中有3个能够校验成功，那么这个相对位姿 $T_{cm}$ 就是可以使用的，直接进行地图融合
- 1-3. 如果共视关键帧中少于3个，进行时序几何校验。如果是0个，直接认为没有共同区域
2. 时序几何校验（看看后面来的关键帧是否也能在相对位姿变换中得到一定数量的匹配点）
- 2-1. 将后续来的关键帧与候选关键帧窗口内地图点进行搜索匹配，FindMatchesByProjection 函数中依然是 SearchByProjection 函数，使用了3为半径搜索，内点数量要大于30，才进行下一步
- 2-2. 使用 OptimizeSim3 函数只优化相对位姿 $T_{cm}$ ，并计算内点数量，内点数量大于50,才进行下一步
- 2-3. 依然使用 FindMatchesByProjection 函数，使用3为半径搜索，内点数量要大于100,才算时序校验成功
3. 共同区域检测的最终条件
- 3-1. 满足共视几何校验和时序几何校验的总关键帧数大于3，则认为校验成功，直接进行地图融合
- 3-2. 若两次新进来的关键帧时序几何校验都失败，就认为最终验证失败，之后重新进行共同区域检测

MergeLocal函数

当检测到共同区域后，此时已经得到的量：当前关键帧 $K_c$ 、融合关键帧 $K_m$ 和这两个之间的相对位姿 $T_{cm}$
在地图融合的过程中，需要注意的：

在地图的融合过程中，其实与回环进行融合并没有太多的区别，需要注意的就是，当前关键帧和融合关键帧不在同一个世界坐标系（因为世界坐标系都是以该地图的第一帧为主），所以在进行位姿传播到其他关键帧的时候需要留意（但是代码中做法差不多）

融合的过程中实际上是以新换旧，就是将当前关键帧所在地图（新），融到 融合关键帧所在地图中（旧），因此在进行位姿传播的时候只需要得到其他关键帧与当前关键帧之间的位姿关系 $T_{ic}$ ，再直接乘相对位姿 $T_{cm}$ ，就将坐标系转换为旧坐标系中

3510856

为了保证地图融合的时候没有关键帧的进入，同时减少延时，ORBSLAM3中将整个地图融合优化分为两个阶段：
- 一阶段：局部BA优化，优化所有的当前关键帧共视窗口里的关键帧和地图点，固定所有融合帧共视窗口里的帧（因为是以新换旧）
- 二阶段：本质图优化，优化融合后非活跃地图中剩下的所有关键帧和地图点，固定融合关键帧和当前关键帧各自的共视窗口中的关键帧
- 原因在于，一阶段优化的是局部窗口，而二阶段优化其他的（经过了位姿矫正），认为这些没有那么紧急，所以在一阶段优化结束，就会打开LocalMapping线程，而在二阶段优化开始后，再重新关闭LocalMapping线程

MergeLocal 地图融合的简要流程：

1. 停止全局BA和LocalMapping线程，防止新的关键帧插入
2. 建立当前关键帧和融合关键帧的各自局部窗口，要保证各自窗口中有25个关键帧，一级共视关键帧不够，就从二级共视关键帧拿，再加上各自关键帧的地图点，构成当前局部窗口和融合局部窗口
3. 位姿传播，根据当前关键帧的相对位姿 $T_{cm}$ ，将当前局部窗口的所有关键帧和地图点进行矫正：
- 3-1. 关键帧矫正：就是上面说的，直接相乘，就实现了两个坐标系的转换
- 3-2. 地图点矫正：地图点都是世界坐标系的坐标，因此需要先将地图点转换为关键帧所在坐标系（参考关键帧），然后经过矫正后的关键帧坐标，转换为另一个世界坐标系坐标：g2oCorrectedSwi.map(g2oNonCorrectedSiw.map(P3Dw))
4. 两个地图以新换旧，将当前局部窗口内的关键帧和地图点所在的地图设置为融合关键帧地图（旧），并在当前地图中删除局部窗口的关键帧和地图点，激活融合关键帧地图（旧） （注意：此时当前关键帧的局部窗口都已经在融合关键帧地图中了）
5. 融合两个局部窗口中关键帧的生成树（用于本质图优化）–
6. 融合两个局部窗口中重复的地图点，并重新更新地图中的连接关系
7. 一阶段优化，也叫Welding BA，实际上使用的就是局部BA优化函数 LocalBundleAdjustment ，优化之后，打开LocalMapping线程
8. 对当前帧地图中其他的关键帧和地图点进行位姿传播（注意，此时当前帧地图已经是非活跃的了，融合帧地图才是活跃的）
9. 二阶段优化，再优化的过程依然关闭了LocalMapping线程，并将这些优化的关键帧都放到活跃地图中（融合帧地图）
10. 如果需要的化，进行全局BA优化

一只废佐_

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
论文分享-ORBSLAM3中的多地图系统

此线程主要都是做关键帧的增加删除等操作，对于多地图系统中，没有过多的参与（因为LocalMapping线程都是运行在活跃地图中）已经是比较准的，但是这个相对位姿只是对于当前关键帧和候选关键帧比较准，并不清楚这个相对位姿对于各自的窗口是否准，因此还要校验。Atlas其实就是由。程序开始时，会先建立ID为0的地图，并设定为活跃地图，之后的每次地图的创建都会将当前的地图设定为非活跃地图。当前关键帧的5个共视关键帧中只要有3个匹配成功即认为检测到共同区域，如果不够3个，再检测后面进来关键帧的时间连续性。
复制链接

扫一扫