ORB-SLAM3多地图管理
ORB-SLAM3多地图管理
-
- 0.引言
- 1.Atlas(地图集)
-
- 1.1.逻辑简析
- 1.2.代码简析
- 2.Tracking thread(追踪线程)
-
- 2.1.逻辑简析
-
- 2.2.1.ORB-SLAM3中LOST条件
- 2.2.2.ORBSLAM-Atlas中LOST条件
- 2.2.代码简析
- 3.Local Mapping thread(局部建图线程)
- 4.Loop and map merging thread(闭环和地图融合线程)
-
- 4.1.场景识别
- 4.2.视觉地图融合
- 5.重定位
- 6.系统线程
- 7.参考
0.引言
只看地图管理部分。ORB-SLAM3的地图管理，是在另一篇论文ORBSLAM-Altas中进行详细讲解的。本文也只看视觉方面，不讨论融合IMU的情况。
系统框图和ORB-SLAM的框图十分相似。区别明显的地方有有：
- 1、系统中有两类子地图：Active map和Non-active map；
- 2、跟踪线程要判断是否需要构建新的地图以供跟踪；
- 3、检测的闭环可以是在同一个子地图中，也可以在不同子地图中。
1.Atlas(地图集)
1.1.逻辑简析
所谓Atlas就是一种多地图的表示形式，多地图由不相连的地图组成，它由active map以及non-active map组成。
- active map被Tracking thread（追踪线程）用作定位，它被连续优化同时会加入新的关键帧。
- Non-active则是之前跟踪丢失后保留下来的所有地图。
每个子地图都是独立的，它们的第一参考帧也都是独立的。所有子地图共用同一个DBoW数据库，使得能够实现重定位回环等操作，闭环分为两种情况，如图所示。
- active map内部形成闭环，此时处理和之前的ORB-SLAM相差不大。
- Active map与Non-active map形成闭环，那么就会将两个存在闭环的地图融合起来，然后系统使用融合后的地图作为新的Active map使用。
这种做法的好处有：
- 在跟踪丢失后系统通过创建新的地图来继续工作；
- 构建多个子地图可以降低误差的累积；
- 所有子地图融合后获得完整地图。这个融合后的地图相比于ORB-SLAM构建的一整个地图，在精度方面会有所提高（有部分原因是剔除了一些精度较低或是跟踪丢失的轨迹）。
1.2.代码简析
地图集的数据结构在atlas类里面，但该类只是抽象了数据结构，底层还是Map类，KeyFrame类等，地图的匹配融合等操作还是在其他线程进行实现。地图集合使用set进行表示。
std::set<Map*> mspMaps;// 地图集（包含当前Active map）
std::set<Map*> mspBadMaps;// 坏地图集
Map* mpCurrentMap;// 活动地图
// 当前Active map最大帧ID + 1 = 下一个子地图的初始化帧ID
unsigned long int mnLastInitKFidMap;
// $所有子地图共用同一个DBoW数据库
// Class references for the map reconstruction from the save file
KeyFrameDatabase* mpKeyFrameDB;
ORBVocabulary* mpORBVocabulary;
2.Tracking thread(追踪线程)
2.1.逻辑简析
与ORB-SLAM2类似，该线程用来处理传感器信息，计算当前帧相对于active map的位姿以及最小化匹配到的地图点的重投影误差。该线程决定何时当前帧被判定为关键帧。在VI模式下，机体的速度以及IMU的bias通过优化惯导残差被估计。当系统追踪丢失后，首先将进行重定位操作，即当前帧在所有的Altas进行重定位；若重定位成功，当前帧恢复追踪状态；否则，经过一段时间（超过5秒），当前的active map会被存储为non-active map，同时重新创建一个新的地图，新的地图作为活动地图继续进行跟踪与建图过程。
跟踪线程需要判断是否跟踪丢失，再决定是否构建新的地图。新地图构建和初始化的过程相同，最重要的是何时构建新地图。ORB-SLAM3与ORBSLAM-Atlas论文阐述的条件稍有不同。
2.2.1.ORB-SLAM3中LOST条件
这里也不是严格的视觉丢失条件，而是视觉丢失后的处理。
- 短期丢失：通过IMU读数对当前帧状态进行估计，地图点根据估计的位姿投影到当前帧后设置较大搜索窗口进行匹配。大多数情况下，通过这种方式能够恢复系统位姿。若超过5秒仍未重定位成功，则进入下一个状态；
- 长期丢失：新的VI地图会被重新初始化，并成为active map。
2.2.2.ORBSLAM-Atlas中LOST条件
- 1、当前帧跟踪到的地图点数目：设 N N N为当前帧跟踪到的局部地图中的地图点数量。如果 N N N大于给定阈值，则认为跟踪良好，否则跟踪丢失。
- 2、相机位姿的可观测性：如果 N N N中的地图点的几何条件较差，则认为当前帧的位姿估计值精度较低，认为跟踪丢失。
从图中可以看到，当前帧跟踪到的地图点数量 N N N很多，肯定能满足第一条标准。但是跟踪到的点大多距离相机很远。太远的地图点会降低在平移运动上的估计值的精度（如：走一两步几乎不会影响远点在当前图像上的成像）。所以最后的估计的位姿精度较差，如果保留这些较差的估计值，那么就会直接影响到之后图优化的精度。 所以系统将这些较差的位姿剔除，从这时开始构建一个新的子地图。这样就将那些误差较大的轨迹排除在外。
图中的P1是一部分精度较差的位姿估计值，P2、P3则是一个闭环。图b因为保留了P1，以至于影响到了最后闭环优化的精度。而下面的图则直接剔除了P1部分，保证了最终轨迹的一个估计精度。论文也给了“相机位姿可观测性”的具体判断公式，通过相机位姿的误差协方差来估计其位姿的观测性。
首先假设 Active map 中每个地图点都估计的很准确，信息矩阵 Ω i , j \Omega_{i, j} Ωi,j表示第 j j j个地图点在第 i i i帧中的不确定性，相机 i i i的不确定性是由其和局部地图匹配的特征点。定义 T ^ i , w ∈ S E ( 3 ) \hat{T}_{i, w} \in S E(3) T^i,w∈SE(3)是当前相机的位姿估计值，属于李群，利用6个参数 ε i ε_i εi的无偏高斯向量对位姿的不确定性进行编码，该向量定义了在 T ^ i , w T̂ _{i,w} T^i,w周围逼近位姿真值的李代数：
T i , w = Exp ( ε i ) ⊕ T ^ i , w \mathbf{T}_{i, w}=\operatorname{Exp}\left(\varepsilon_{i}\right) \oplus \hat{\mathbf{T}}_{i, w} Ti,w=Exp(εi)⊕T^i,w ε i = ( x y z ω x ω y ω z ) ∼ N ( 0 , C i ) \varepsilon_{i}=\left(\begin{array}{llllll} x & y & z & \omega_{x} & \omega_{y} & \omega_{z} \end{array}\right) \sim \mathcal{N}\left(0, \mathbf{C}_{i}\right) εi=(xyzωxωyωz)∼N(0,Ci) H i ≃ ∑ j = 1 m i J i , j ⊤ Ω i , j J i , j \mathbf{H}_{i} \simeq \sum_{j=1}^{m_{i}} \mathbf{J}_{i, j}^{\top} \boldsymbol{\Omega}_{i, j} \mathbf{J}_{i, j} Hi≃j=1∑miJi,j⊤Ωi,jJi,j C i = H i − 1 \mathbf{C}_{i}=\mathbf{H}_{i}^{-1} Ci=Hi−1
其中 E x p ( ε i ) Exp(ε_i) Exp(εi)把一个六维的向量直接转换为了李代数，其协方差矩阵 C C C编码为相机位姿估计的准确性，而 J J J矩阵为相机位姿对观测的地图点的雅可比矩阵。由于平移的幅度很小，所以在评判中仅用 C C C的对角线上表示误差的值。
max ( σ x , σ y , σ z ) < σ t h t \max \left(\sigma_{x}, \sigma_{y}, \sigma_{z}\right)<\sigma_{t h}^{t} max(σx,σy,σz)<σtht [ σ x 2 σ y 2 σ z 2 σ ω x 2 σ ω y 2 σ ω z 2 ] = diag ( C i ) \left[\begin{array}{cccccc} \sigma_{x}^{2} & \sigma_{y}^{2} & \sigma_{z}^{2} & \sigma_{\omega_{x}}^{2} & \sigma_{\omega_{y}}^{2} & \sigma_{\omega_{z}}^{2} \end{array}\right]=\operatorname{diag}\left(\mathbf{C}_{i}\right) [σx2σy2σz2σωx2σωy2σωz2]=diag(Ci)
2.2.代码简析
Tracking类定义了一个Atlas，整个系统就一个地图集。ORB提取几乎没有太大变动，直接进入oid Tracking::Track()函数。初始化在Active-map中进行，代码中以pCurrentMap表示。
- 题外话，Tracking线程中有一个标志
bool mbInitWith3KFs;但是构造函数中被置为了false，其他地方再没有出现过。我记得在有篇论文中看到过，使用三帧初始化比两帧更为鲁棒，不知道这里是被阉割了还是没有实现。
跟踪过程的几种状态：
// 跟踪状态
enum eTrackingState{
SYSTEM_NOT_READY=-1, //系统没有准备好的状态,一般就是在启动后加载配置文件和词典文件时候的状态
NO_IMAGES_YET=0, //当前无图像,也可能是IMU先到达
NOT_INITIALIZED=1, //有图像但是没有完成初始化
OK=2, //正常跟踪状态
RECENTLY_LOST=3, //IMU模式：当前地图中的KF>10,且丢失时间<5秒。纯视觉模式没有该状态
LOST=4, //IMU模式：当前帧跟丢超过5s。纯视觉模式：重定位失败
OK_KLT=5
};
// 从Atlas中取出当前active的地图，没有的话，会新建一个子地图
Map* pCurrentMap = mpAtlas->GetCurrentMap();
// Step 2 处理时间戳异常的情况（IMU情况，后续不再看融合IMU的情况）
if(mState!=NO_IMAGES_YET)
{
// 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
{
cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
unique_lock<mutex> lock(mMutexImuQueue);
mlQueueImuData.clear();
CreateMapInAtlas();
return;
}
// 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回
else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)
{
cout << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl;
//根据是否是imu模式,进行imu的补偿
if(mpAtlas->isInertial())
{
// 如果当前地图imu成功初始化
if(mpAtlas->isImuInitialized())
{
cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
// ? BA2标志代表什么?,BA优化成功?
if(!pCurrentMap->GetIniertialBA2())
{
// 如果当前子图中imu没有经过BA2，重置active地图
mpSystem->ResetActiveMap();
}
else
{
// 如果当前子图中imu进行了BA2，重新创建新的子图
CreateMapInAtlas();
}
}
else
{
// 如果当前子图中imu没有初始化，重置active地图
cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
mpSystem->ResetActiveMap();
}
}
// 不跟踪直接返回
return;
}
}
初始化过程中通过H、E、RANSAC算法分解R、t的实现，由ORB-SLAM2的Initializer转到了TwoViewReconstruction，添加了相机模型的兼容性，但是Initializer里面的代码没删。初始化成功后进入具体的跟踪过程。当跟踪参考关键帧、恒速模型跟踪都失败时（判定为LOST的代码逻辑）：
if(mState==OK)
{
{
//参考关键帧跟踪、恒速模型跟踪
//纯视觉：bok = false 的情况
// TrackReferenceKeyFrame：nmatches<15
// TrackWithMotionModel: nmatches<20
}
//如果经过跟踪参考关键帧、恒速模型跟踪都失败
if (!bOK)
{ // case1：满足两个条件置为LOST
// 条件1：如果当前帧距离上次重定位帧不久（时间在1s内）
// mnFramesToResetIMU 表示经过多少帧后可以重置IMU，一般设置为和帧率相同，对应的时间是1s
// 条件2：单目+IMU 或者 双目+IMU模式
if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&
(mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO))
{
mState = LOST;
}
else if(pCurrentMap->KeyFramesInMap()>10)
{
// 当前地图中关键帧数目大于10 并且 当前帧距离上次重定位帧超过1s（隐藏条件）
// 状态标记为RECENTLY_LOST
cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
mState = RECENTLY_LOST;//留到后面进一步判断
mTimeStampLost = mCurrentFrame.mTimeStamp;
//mCurrentFrame.SetPose(mLastFrame.mTcw);
}
else//case2：跟丢时间超过1s,且当前地图关键帧不足10，置为LOST
{
mState = LOST;
}
}
}
else //跟踪不正常按照下面处理
{
if (mState == RECENTLY_LOST)
{
// 如果是RECENTLY_LOST状态（仅存在IMU模式下，纯视觉模式无此状态）
Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);
// bOK先置为true
bOK = true;
if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)) // IMU模式
{
// 如果当前地图中IMU已经成功初始化，就用IMU数据预测位姿
if(pCurrentMap->isImuInitialized())
PredictStateIMU();
else
bOK = false;
// case3：如果当前帧距离跟丢帧已经超过了5s（time_recently_lost默认为5）
// 将RECENTLY_LOST状态改为LOST
if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost)
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
else //重定位挽救一下，先不创建地图。通过MLPnP算法估计姿态
{
// Step 6.5 纯视觉模式则进行重定位。主要是BOW搜索
bOK = Relocalization();
if(!bOK)
{
// case4：纯视觉模式下重定位失败，挽救失败，状态置为LOST
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
}
else if (mState == LOST) // 上一帧为最近丢失且重定位失败时，LOST后的处理：
{
// Step 6.6 如果是LOST状态
// 开启一个新地图
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
if (pCurrentMap->KeyFramesInMap()<10)
{
// 当前地图中关键帧数目小于10，重置当前地图，重置的意思是将这几帧丢掉？
mpSystem->ResetActiveMap();
cout << "Reseting current map..." << endl;
}else
// 当前地图中关键帧数目超过10，创建新地图
CreateMapInAtlas();
// 干掉上一个关键帧
if(mpLastKeyFrame)
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);
return;
}
}
ORBSLAM-Atlas中提到的第二种判断条件在ORB-SLAM3的代码中我没找到，可能是我没看到，希望大家帮我指出来一下。
3.Local Mapping thread(局部建图线程)
向active map中新增/删减/优化关键帧以及地图点，上述的操作是通过维护一个靠近当前帧的局部窗口的关键帧进行实现。与此同时，IMU的参数被初始化然后被该线程通过本文提出的最大后验估计技术进行求解。
4.Loop and map merging thread(闭环和地图融合线程)
短期以及中期数据关联可以通过Tracking以及Local Mapping进行完成。而对于长期的数据关联，可通过重定位以及闭环实现。
ORB-SLAM采用的基于视觉词袋场景识别的重定位，若候选关键帧只有1个，召回率为50 ~ 80%。为了应对假阳性的出现，算法使用了时域校验以及几何校验，这两种手段能够使精确率达到100%的同时召回率为30~40%。至关重要的是，时域连续性检测将使场景识别滞后至少3个关键帧，同时召回率较低，这都是目前存在的问题。
为了应对这个问题，文中提出一种新的场景识别（召回率得到改善）以及多地图数据关联算法。一旦Local Mapping线程创建了关键帧，场景识别算法就会被激活并且寻找该帧在Atlas中的数据关联。若匹配的关键帧在active map中，则进行闭环；否则，则进行多地图间的数据关联，即将active map与匹配的map进行融合。一旦这个新的关键帧与匹配地图间的相对位姿被计算出，就定义一个在局部窗口，这个局部窗口包括匹配的关键帧以及这个关键帧的共视关键帧。在这个局部窗口中，我们会寻找中期数据关联，以提高闭环以及地图融合的精度。这个改进使得ORB-SLAM3比ORB-SLAM2具有更高的精度。
4.1.场景识别
与ORB-SLAM2基本类似，只是增加了重力向校验的步骤。具体的，计算出了当前关键帧在matched map（可能是active map或者其它地图）中的位姿 T a m ∈ S E ( 3 ) T_{am}∈SE(3) Tam∈SE(3)，检验pitch(俯仰角)和roll(横滚角)是否小于一定阈值来对场景识别结果进行校验。
NewDetectCommonRegions()：
-
Step 1 基于前一帧的历史信息判断是否进行时序几何校验, 注意这里是基于共视几何校验失败才会运行的代码.
核心函数：DetectAndReffineSim3FromLastKF().–>FindMatchesByProjection(). -
Step 2 分别找到3个最好的候选帧, 回环候选帧放在vpLoopBowCand中,融合候选帧放在vpMergeBowCand中
mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
-
Step 3.1 若当前关键帧没有被检测到回环,并且候选帧数量不为0,则对回环候选帧进行论文中第8页的2-5步.
mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs); -
Step 3.2 若当前关键帧没有被检测到融合,并且候选帧数量不为0,则对融合候帧进行论文中第8页的2-5步
mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);- 2.Local window.
- 3.3D aligning transformation.
- 4.Guided matching refinement.
- 5.Verification in three covisible keyframes.
图中，右上角坐标图中左下角的箭头画反了:D，这个函数有一点bug不知道自己理解对不对：
- Step 4 Verification in three covisible keyframes.用当前关键帧的相邻关键来验证前面得到的Tam(共视几何校验),通过之前计算的位姿转换地图点并通过投影搜索匹配点, 若大于一定数目的任务成功验证一次，当前关键帧的五个最佳共视帧中有三个或以上与pMostBowMatchesKF匹配成功的数目高于一定阈值则算校验成功。
4.2.视觉地图融合
如果在两个地图之间检测到了闭环，那么就要进行地图融合。然后用融合后的地图替换掉那两个地图。
- 下标 a a a 地图表示Active map，代码中是下标 c c c
- 下标 m m m地图表示Non-active map
由于 M a Ma Ma可能包含许多元素，融合它们可能需要很长时间，因此融合分为两个步骤。首先，在由 K K Ka和 K m Km Km的邻域定义的welding window（焊接窗口）中执行融合，随后在第二阶段，通过位姿图优化将校正传播到融合图的其余部分.
(1).Welding window assembly
Welding window包括当前关键帧 K a Ka Ka及其共视帧， K m Km Km以及其共视帧，以及被这些关键帧观测的地图点。在将它们包含在Welding window中之前，属于 M a Ma Ma的关键帧和地图点通过 T m a T_{ma} Tma进行变换，以使其与 M m Mm Mm对齐。
(2).融合
M a Ma Ma与 M m Mm Mm融合为一个新的active map。为了要删除重复的点， M m Mm Mm关键帧主动搜索匹配 M a Ma Ma中的点。对于每个匹配点，都会删除 M a Ma Ma中的点，并保留 M m Mm Mm中的点，并累积删除点的所有观测值，同时更新共视图以及本质图。
(3).Welding bundle adjustment
Welding window范围内的所有关键帧进行局部BA优化（见下图）。为了确定尺度自由度，观测到 M m Mm Mm的那些关键帧需要保持固定。优化完成后，Welding window区域中包含的所有关键帧都可以用于跟踪，实现地图 M m Mm Mm的快速/准确复用。
图中左边的虚线框与接地符十分的灵性哈哈哈哈，表示BA过程中mpMergeMatchedKF及其共视帧组成的窗口固定不优化，只提供约束。
(4).Essential-graph optimization
位姿图利用本质图在融合后的地图范围进行优化，Welding window范围内的关键帧保持固定。这一步的意义在于分摊误差。
5.重定位
ORB-SLAM3支持针孔以及鱼眼相机模型，ORB-SLAM1、2都是假设所有系统组件都符合针孔模型。对于重定位这项任务，ORB-SLAM1、2基于EPnP算法建立PnP求解器进行解算位姿，其中所有的公式都假设相机为标定好的针孔相机。为适配本文的方法，作者使用了一种独立于相机模型的求解器：Maximum Likelihood Perspective-n-Point algorithm (MLPnP)（最大后验PnP算法）。该算法实现了相机模型与求解算法的解耦，相机模型只是用来提供投影射线作为输入。
ORB-SLAM3在重定位的策略上做了一些改进。为了保证重定位不出错，重定位常常设置了严苛的条件，保证高精准率而识别率较低。旧的方法（ORB-SLAM1/2）中当3个关键帧完全匹配上后才判定为重定位成功。然而作者发现，三个关键帧经过了很长的时间。主要改进是，当当前关键帧与数据库的关键帧匹配上后，检测与当前关键帧具有共视关系的关键帧是否也能够匹配，如果可以则判定为重定位成功；否则才继续使用接下来的关键帧进行判定。
6.系统线程
论文还提到了各线程之间的一些关系。地图融合会在一个单独的线程中进行。在融合开始前，会将Local Mapping线程暂停，以防止新的 K F KF KF产生。在地图融合的过程中，跟踪线程会继续使用 Active map 来达到实时工作的目的。当融合结束后(代码中好像是在welding BA后就回复建图了)，跟踪线程会将融合后的地图作为新的 Active map 来使用。
7.参考
- ORBSLAM-Atlas、ORB-SLAM3
- ref1.ORBSLAM-Altas：多地图SLAM
- ORB-SLAM3系列-多地图管理