1.ORB-SLAM2
本部分对ORB-SLAM2中的状态以及切换逻辑进行分析。
1.1 ORB-SLAM2中有哪些状态
在前面的文档中也提到过，ORB-SLAM2中的状态在Tracking.h文件中以枚举形式定义，如下。
下面简单对各状态进行介绍：
- SYSTEM_NOT_READY: 表示系统是否已经完成了工作前的各项准备。在可视化相关部分中被
FrameDrawer调用，在核心代码中没有被调用。 - NO_IMAGES_YET: 没有影像输入状态，系统初始化完成后被赋的默认状态，和
NOT_INITIALIZED状态相关。Track()函数中此状态会被直接转换为NOT_INITIALIZED状态。 - NOT_INITIALIZED: 系统没有初始化状态。系统中如果没有初始化，就会调用单目或双目初始化函数进行初始化。
- OK: Tracking良好的状态。如果
TrackWithMotionModel()、TrackReferenceKeyframe()、TrackLocalMap()函数运行正常，就说明Tracking良好。 - LOST: Tracking丢失的状态。表明当前视觉Tracking的流程在某一步卡住了，无法正常估计位姿。进入此状态后，会尝试调用重定位函数恢复。
上面这些状态，在系统实现中，主要是通过Tracking类的public成员变量mState进行赋值和访问的，如下。
下面再对各状态间的转换关系进行简单分析。
1.2 ORB2-NO_IMAGES_YET
NO_IMAGES_YET状态的切换关系可总结如下：
NO_IMAGES_YET→OKNO_IMAGES_YET→NOT_INITIALIZED
如前面所说，任何一个ORB-SLAM2系统都是从NO_IMAGES_YET状态开始的。因为在Tracking的构造函数中，mState变量就被赋值为NO_IMAGES_YET作为默认值，如下。
所以这里我们也从NO_IMAGES_YET状态开始分析，看看进入Track()函数前、函数中、以及出Track()函数后系统的状态切换。通过分析代码，我们可以得到下图。
可以看到，当我们以NO_IMAGES_YET状态作为输入进入Track()函数时，首先就会被转换为NOT_INITIALIZED状态。所以其实从这里往下，都是NOT_INITIALIZED状态要做的事情。简单来说就是调用单目(MonocularInitialization())或者双目(StereoInitialization())初始化函数进行初始化。在这两个初始化函数中，如果初始化成功，会直接修改Tracking::mState变量，变为OK状态，否则不改变当前状态。比如，我们以双目初始化函数为例，如下。
可以看到，如果正确执行了所有代码，就把系统状态设为OK。如果当前状态还是为NOT_INITIALIZED，Track()函数中就没有什么操作，退出函数，进入下一次调用了。
这里需要稍微注意的是还涉及到一个“中间状态”。简单来说就是在Track()函数中根据输入状态A进行了一系列操作，得到了新的状态B。但这并没有结束，在Track()函数的后续处理中，又基于B状态得到了C状态作为输出。虽然从Track()函数的整体输入输出状态来看，是从状态A到状态C的过程，但其实是经历了个中间状态B：State A → (State B) → State C，而非直接切换过去的。以这里的实例来说，输出的OK状态并不是直接由NO_IMAGES_YET状态转换过来的，而是NO_IMAGES_YET → NOT_INITIALIZED → OK。
1.3 ORB2-NOT_INITIALIZED
NOT_INITIALIZED→OKNOT_INITIALIZED→NOT_INITIALIZED
当系统为NOT_INITIALIZED时，状态切换如下。
其实和上面类似的，当没有初始化的时候，会调用单目(MonocularInitialization())或者双目(StereoInitialization())初始化函数进行初始化。如果初始化成功，会直接修改Tracking::mState变量，变为OK状态，否则不改变当前状态。
1.4 ORB2-OK
OK状态的切换关系可总结如下：
OK→OKOK→LOST
通过阅读代码，整理的状态切换如下。
当系统状态为OK时，系统首先会调用帧间位姿估计函数TrackWithMotionModel()或TrackReferenceKeyframe()。如果Tracking失败，Tracking::mState就会被设为LOST，并结束Track()函数。如果Tracking成功，则继续调用TrackLocalMap()函数跟踪局部地图，如果跟踪成功，系统状态为OK，否则为LOST。
1.5 ORB2-LOST
LOST状态的切换关系可总结如下：
LOST→OKLOST→LOST
通过阅读代码，整理的状态切换如下。
可以看到，当状态为LOST的时候，系统首先会调用Relocalization()函数进行重定位，如果重定位失败，系统状态依然还是LOST。如果重定位成功，再调用TrackLocalMap()跟踪局部地图，如果又成功了，那么系统就切换到OK状态，否则还是LOST状态。
如果仔细观察的话可以发现，ORB2中LOST状态的切换逻辑和OK状态几乎是一样的。唯一的区别在于第一步：是调用TrackWithMotionModel()或TrackReferenceKeyframe()进行帧间位姿估计还是调用Relocalization()函数进行重定位。
2.ORB-SLAM3
本部分对ORB-SLAM3中的状态以及切换逻辑进行分析。
2.1 ORB-SLAM3中有哪些状态
与ORB-SLAM2类似的，ORB-SLAM3中的状态也都是定义在Tracking.h文件中，如下所示。
相比于ORB-SLAM2，ORB-SLAM3多了一些状态，下面也简单的逐一介绍：
- SYSTEM_NOT_READY: 与ORB-SLAM2中一样，表示系统是否已经完成了工作前的各项准备。在可视化相关部分中被
FrameDrawer调用，在核心代码中没有被调用。 - NO_IMAGES_YET: 与ORB-SLAM2中一样，表示没有影像输入状态，系统初始化完成后被赋的默认状态，和
NOT_INITIALIZED状态相关。Track()函数中此状态会被直接转换为NOT_INITIALIZED状态。 - NOT_INITIALIZED: 与ORB-SLAM2中一样，表示系统没有初始化的状态。系统中如果没有初始化，就会调用单目或双目初始化函数进行初始化。
- OK: 与ORB-SLAM2中一样，表示Tracking良好的状态。如果
TrackWithMotionModel()、TrackReferenceKeyframe()、TrackLocalMap()函数运行正常，就说明Tracking良好。 - LOST: 与ORB-SLAM2中一样，表示Tracking丢失的状态。
- RECENTLY_LOST: ORB-SLAM3中新增的状态，可以理解为是一种“短暂的丢失”，介于
OK和LOST状态之间。如果进入了RECENTLY_LOST状态，系统会利用IMU观测进行短暂辅助。 - OK_KLT: 在系统中没有被使用。
与ORB-SLAM2一样的，系统通过Tracking::mState变量来赋值或访问当前状态。
下面再对各状态间的转换关系进行简单分析。
2.2 ORB3-NO_IMAGES_YET
NO_IMAGES_YET状态的切换关系可总结如下：
NO_IMAGES_YET→OKNO_IMAGES_YET→NOT_INITIALIZED
通过阅读代码，整理的状态切换如下。
可以看到，这部分逻辑和ORB-SLAM2是一模一样的，这里就不再赘述了。
2.3 ORB3-Prejudgement
由于ORB-SLAM3中引入了IMU，导致出现了很多更为复杂的判断语句。在Track()函数中一开始就有一个对于NO_IMAGES_YET状态的判断，只要当前状态不为NO_IMAGES_YET（也就是说为NOT_INITIALIZED、OK、LOST、RECENTLY_LOST之一），都会经过这个判断，如下所示。
这个判断层层嵌套，但最主要的运行逻辑只有三条线，整理的流程图如下。
简单来说，程序首先会判断上一帧时间戳是不是大于当前帧时间戳，如果是的话，就调用CreateNewMapInAtlas()函数新建一个地图并返回也就对应图中的分支③，如下所示。
但这个函数可不仅仅只是新建个函数那么简单，在函数内部会将Tracking::mState变量修改为NO_IMAGES_YET，如下所示。
而如果说上一帧时间戳不大于当前帧时间戳，则继续判断当前帧时间戳是否大于上一帧时间戳加1秒(数据是否存在跳变，因为引入了IMU，对跳变比较敏感)。如果是的话，再继续判断IMU的状态，这里一连三个和IMU相关的if语句，涉及是否使用了IMU？IMU初始化是否完成？IMU优化是否完成？不同的分支对应不同的结果，但不管怎么说，只有两种情况，要么是维持现有状态不变(分支②)，要么是状态变为NO_IMAGES_YET(分支③)，然后退出Track()函数。最后，如果数据质量良好，则一路往下，进入Track()函数的后续步骤(分支①)。
由于这个判断在多个状态的切换中都会执行，所以在后续内容中，我们就把它简化为“Prejudgement”模块，不再展示里面的细节了。
2.4 ORB3-NOT_INITIALIZED
NOT_INITIALIZED状态的切换关系可总结如下：
NOT_INITIALIZED→OKNOT_INITIALIZED→NOT_INITIALIZEDNOT_INITIALIZED→NO_IMAGES_YET
通过阅读代码，整理的状态切换如下。
可以看到，和ORB-SLAM2中NOT_INITIALIZED状态切换流程类似，唯一不同的地方是加入了Prejudgement模块，这个模块又引入了一个新的可能输出的状态(NO_IMAGES_YET)。
2.5 ORB3-OK
OK状态的切换关系可总结如下：
OK→OKOK→RECENTLY_LOSTOK→NO_IMAGES_YETOK→LOST
通过阅读代码，整理的状态切换如下。
可以看到，相比于ORB-SLAM2，判断逻辑要复杂很多。但其实会发现，左边部分的核心流程和ORB-SLAM2还是一样的。都是调用TrackWithMotionModel()或TrackReferenceKeyframe()进行帧间位姿估计，调用TrackLocalMap()进行局部地图位姿估计。之所以这个图看起来复杂很多有两个原因：一是在ORB-SLAM2中，只要bOK为false，那么系统状态就直接被设为LOST，本次Track()函数也就结束了。但正如前面说的，ORB-SLAM3里引入了RECENTLY_LOST，这是一种介于OK和LOST之间的一种状态，目的在于在短时间内丢失的情况下再“抢救一下”。在代码中，为了“巧妙”地实现状态切换，进行了很多判断(图中灰底部分)。二是在ORB-SLAM3中引入了多地图策略，这样的好处是，在原本可能会LOST的时候再“兜个底”，判断一下能否新建地图，如果可以的话，调用CreateNewMapInAtlas()函数新建一个地图，再把状态设为NO_IMAGES_YET，而非直接设为LOST(图中蓝底部分)。可以看出来，这其实是一种双重保障，一方面，先看能不能进入RECENTLY_LOST状态，如果不行的话，再看能不能新建个地图进入NO_IMAGES_YET状态，最后如果都不行的话，只能设为LOST。
2.6 ORB3-LOST
LOST状态的切换关系可总结如下：
LOST→LOSTLOST→NO_IMAGES_YET
通过阅读代码，整理的状态切换如下。
对于LOST状态，可以看到和ORB-SLAM2相差还是比较大的。在ORB-SLAM3中，首先会经过Prejudgement，输出三个分支。如果顺利通过Prejudgement的话，再看当前地图中的关键帧个数，如果满足条件的话，状态设为LOST；如果不满足条件的话就会新建一个地图，然后状态设为NO_IMAGES_YET。而在ORB-SLAM2中，如果进入了LOST状态，会尝试通过Relocalization()重定位函数和TrackLocalMap()局部地图跟踪函数尝试再次进入OK状态。但在ORB-SLAM3中，如果进入了LOST，就不存在直接回到OK状态的可能(可以间接回到OK状态，比如LOST→NO_IMAGES_YET→OK)。
2.7 ORB3-RECENTLY_LOST
RECENTLY_LOST状态的切换关系可总结如下：
RECENTLY_LOST→OKRECENTLY_LOST→RECENTLY_LOSTRECENTLY_LOST→NO_IMAGES_YET
通过阅读代码，整理的状态切换如下。
它是ORB-SLAM3中新增的状态，用于指示短暂“跟丢”的情况。而且可以发现，其实和ORB-SLAM2中的逻辑是有一定相似之处的。当系统进入RECENTLY_LOST状态以后，还是会先经过Prejudgement判断。如果顺利通过的话，则再判断是不是有IMU。如果没有IMU的话，则尝试采用Relocalization()重定位函数进行定位(这部分是和ORB-SLAM2中LOST状态的前半部分类似的)。如果重定位失败，则将状态设为LOST，之后新建地图，状态最终被设为NO_IMAGES_YET，退出Track()函数。如果使用了IMU，再判断IMU是否初始化、丢失的时间是否大于阈值。根据这两个条件来设置bOK。这两个条件结束以后，如果bOK为false，那么系统状态就被设为LOST，然后走前面说过的LOST状态的常规后处理步骤(新建地图，最终设为NO_IMAGES_YET并退出Track()函数)。如果bOK为true，则尝试调用TrackLocalMap()进行局部地图跟踪，并将结果返回给bOK(这一部分是和ORB-SLAM2中LOST状态的后半段类似的)。最后，根据bOK的结果设置系统状态为RECENTLY_LOST(bOK=false)或OK(bOK=true)。
所以可以看出来其实ORB-SLAM3中RECENTLY_LOST状态其实是和ORB-SLAM2中的LOST状态有着诸多相似之处的。而且其实如图中灰色部分所示，他一系列的判断都是为了看是“直接放弃”进LOST分支，还是“再努力一下”进TrackLocalMap()分支。所以可以仔细对比ORB-SLAM2的LOST状态和ORB-SLAM3的RECENTLY_LOST状态，可以发现一些很有趣的东西。如果我们把一些无关紧要的内容去掉(Prejudgement模块、LOST之后的常规处理、图中灰色部分)，再将其和ORB-SLAM2的LOST状态比较，这样就可以得到下面的图。
可以看到，相比于ORB-SLAM2，ORB-SLAM3主要就是多了黄色部分，也就是IMU的使用。而这一部分又可以细分为两个部分。第一部分就是如果使用了IMU并且状态OK，就直接跳到TrackLocalMap()，而不经过Relocalization()函数。第二部分就是如果TrackLocalMap()失败了，也不直接将状态设为LOST，而是设为RECENTLY_LOST。这个其实也是RECENTLY_LOST状态的核心。
3.典型场景状态切换分析
通过上面对于ORB-SLAM2和ORB-SLAM3状态切换的仔细分析，我们就可以“预测”当出现某些场景或情况时，ORB-SLAM2和ORB-SLAM3的不同之处以及可能产生的结果。这里简单列举几个进行分析。
3.1 视觉短暂失效(前后无交集)
一个常见的情况是纯视觉丢失。一开始视觉影像正常，突然在某时刻相机坏了一段时间，然后相机又好了，此时，视觉恢复后和丢失时的视野没有且以后也都无交集，如下图所示。
在这种情况下，首先看ORB-SLAM2的流程，如下。
可以看到，进入LOST状态以后，能不能回的来，重新变成OK状态主要取决于Relocalization()和TrackLocalMap()这两个函数。然而这两个函数都依赖于恢复后的视觉和丢失之前的视野的交集。如果一直没有交集就一直是LOST状态，无论多长时间。
而相比于ORB-SLAM2中相对直接的判断逻辑，ORB-SLAM3中还考虑了地图中关键帧的个数这个因素，不同关键帧个数会导致进入不同的分支和系统状态。这里我们就先认为视觉短暂失效前已经有10个以上的关键帧了，核心流程如下。
首先，视觉Tracking正常进行(已经积累了10+关键字)，某一帧开始，视觉丢了，那么TrackWithMotionModel()或TrackReferenceKeyframe()函数就会失败，又因为当前地图中有超过10个关键帧，所以系统状态被设为RECENTLY_LOST。当下一帧再进来的时候，视觉还是丢的、状态为RECENTLY_LOST、没用IMU，所以Reloocalization()重定位函数不成功，系统状态被短暂设为LOST，然后紧接着就新建一个新地图，并把当前状态设为NO_IMAGES_YET。然后当下一帧进来的时候，由于视觉还没恢复，所以单目或双目初始化都无法成功，状态又被设为NOT_INITIALIZED。再下一帧进来的时候，类似的，初始化还是不成功，状态还是NOT_INITIALIZED。到这里系统就会一直循环，直到视觉恢复的那一帧，初始化成功，系统状态变为OK。最后，按照常规OK状态的流程运行，直到结束。
3.2 视觉短暂失效(前后有交集)
另一种情况是，一开始视觉影像正常，突然在某时刻相机坏了一段时间，然后相机又好了，此时，视觉恢复后和丢失时代视野有交集，如下图所示。
在这种情况下，首先看ORB-SLAM2的流程，如下。
和第一种情况类似，系统首先是OK状态，然后进入LOST状态。而当恢复后的视野和之前有重叠的时候，Relocalization()函数和TrackLocalMap()函数执行成功，这样系统状态就直接被设为OK，也就认为系统又被抢救“活过来”了，可以继续Tracking。但问题是系统“失忆”了，它并不知道丢失的那段时间里到底发生了什么、相机做了什么运动、去了哪里。如下图所示。
相机沿着蓝色轨迹运动，忽然在某一位置(橙色点)视觉丢失了，相机又沿着红色虚线(真值)运动了一段时间，到了下一个位置(橙色点)，此时视觉又恢复了。然后相机沿着绿色轨迹继续运动，系统也可以继续Tracking。但是视觉丢失的那段时间里，真实的轨迹到底是什么样子呢？系统完全无法得知，是黄色轨迹1、蓝色轨迹2还是灰色轨迹3，无法确定。所以从这个角度来说，对于SLAM任务而言，在某些场景下，Tracking的连续性要更加重要一些。
我们这里也是假设在视觉良好的阶段已经累积了10+关键帧。对于ORB-SLAM3而言，核心流程如下。
其实流程和上面一种情况是一模一样的。原因在于在这个场景中， ORB-SLAM3的策略中重定位并没有发挥实质性的作用。在图中可以看到，Relocalization()重定位函数只在RECENTLY_LOST状态下才被调用。但是根据上面的分析，在进入RECENTLY_LOST的时候，此时视觉还是丢的，这样做重定位肯定是失败的。而且此时状态又被设为了NO_IMAGES_YET，这样最终不断迭代那些视觉丢失的帧是在NOT_INITIALIZED状态的流程里。跳出异常情况也是在这里。就是当影像数据恢复以后，单目或双目初始化成功，系统状态就被设为OK，然后转入OK状态流程。这两个地图可不可能被融合呢？答案是肯定的。在ORB-SLAM3中有MergeMap的相关函数。如果当前地图的相机又运动到了上一个地图范围，符合某些判断条件，ORB-SLAM3就会启动相应的地图融合函数。
3.3 视觉-IMU联合
最后，我们再模拟一种场景，那就是当视觉丢失时，IMU正常工作(已经初始化、各种状态也都OK)，通过IMU来短暂估计位姿的情况，如下图所示。
对于ORB-SLAM3，还是假设在视觉正常的情况下已经积累了10+关键帧，流程如下。
首先，在视觉正常的情况下，系统状态一直为OK，某时刻视觉丢失，这时TrackWithMotionModel()/TrackReferenceKeyframe()就失败了，由于地图中已经有10+关键帧，状态被设为RECENTLY_LOST。然后下一帧输入，由于使用了IMU，且IMU各项状态良好，所以直接到TrackLocalMap()函数。但由于此时视觉并未恢复，Tracking肯定还是失败的，所以系统状态还是被设为RECENTLY_LOST。这样不断有新的帧输入，系统会反复执行RECENTLY_LOST状态的流程。直到某时刻视觉恢复，某帧有了内容。这时TrackLocalMap()执行成功，系统状态直接被赋值为OK。之后就会进入正常的OK状态流程。
当然，这里也有个小分支，如下图灰色箭头所示。
如果RECENTLY_LOST的时间过长，超过了阈值，系统就会将当前状态设为LOST，然后新建地图，并将状态设为NO_IMAGES_YET。这样之后的流程就和3.2部分的类似了。循环处理输入影像帧的任务就落到了NOT_INITIALIZED状态流程中。直到某帧有了影像内容，初始化完成，系统便会重新进入OK状态流程。
本文作者原创,未经许可不得转载,谢谢配合
