这篇文章将向大家介绍orbslam3开源系统中的单目（+惯导）跟踪算法模块的实现逻辑分析。
跟踪模块在orbslam3中作为计算频率最高的算法模块，在实现中也作为程序的主线程来运行，其他的模块如局部建图（LocalMapping），回环检测（LoopClosing）以及可视化模块（Viewer）均以其他背景线程来并发运行。跟踪模块的主要作用为相机的运动模型（位姿）的实时跟踪和优化。
跟踪模块作为整个系统的入口，其接收从感知传感器捕获的环境数据，如图像数据，IMU，Lidar等，这里将以单目(+imu)场景，且以跟踪+定位（也有只跟踪的模式）的模式为例来进行分析介绍。
一、接收图像并进行处理的函数调用路径（以mono_euroc的example为例）为mono_euroc.cc文件中的SLAM.TrackMonocular(im,tframe)->system.cc文件中的mpTracker->GrabImageMonocular(imToFeed,timestamp,filename)->tracking.cc文件中的GrabImageMonocular函数将图像先转换成灰度图，然后构建Frame类的对象，最后调用Track()函数进行跟踪。这里对Frame构造数据帧对象做一下补充说明：Frame类支持多态的构造函数以支持多个应用场景，如单目，双目，深度相机等，在构造函数里首先直接在畸变的图像上进行ORB特征点和特征描述符的计算提取，然后对特征点进行去畸变的处理。
二、跟踪过程初始化：跟踪过程先判断当前的跟踪状态，跟踪的状态候选有[NOT_INITIALIZED, LOST, RECENTLY_LOST, OK]，当判断到系统当前跟踪状态还尚未初始化时（状态为NOT_INITIALIZED），则首先进行单目的初始化工作，其实现函数为MonocularInitialization，初始化的状态由mbReadyToInitializate变量来表示，系统启动时为false，其状态的判别和转换的逻辑详见[1]，完整的初始化过程需要两个间隔较短的图像帧且两帧的特征点的数量都超过了设定的阈值。当满足这样的条件时，根据这前后临近的两帧的匹配特征点集合基于对极几何和三角化测量方法（ReconstructWithTwoViews）去初始化相机位姿和地图点坐标，如果成功初始化相机位姿，则调用函数CreateInitialMapMonocular创建初始地图，将初始帧和当前帧作为关键帧加入到当前地图里，同时更新对应的地图点和关键帧之间的关联关系，并进行全局的BA优化以进一步优化相机初始位姿。
三、当跟踪状态为OK时，如果运动模型不可用，则调用TrackReferenceKeyFrame，如果运动模型可用，则先调用TrackWithMotionModel，如果用运动模型跟踪不成功，则回退到用TrackReferenceKeyFrame进行跟踪。其中: (1)、TrackReferenceKeyFrame将当前帧和参考关键帧（临近创建的关键帧）进行特征点匹配（如果匹配的特征点不够，则返回跟踪失败，需要进行重定位），并基于此通过Optimizer的PoseOptimization函数对当前帧进行位姿优化；（2）、TrackWithMotionModel根据运动模型预测当前的相机位姿，并将当前帧和上一帧进行投影搜索，如果匹配点过少，将加大阈值再次将当前帧和上一帧进行投影搜索，满足匹配条件则跟踪成功调用PoseOptimization函数进行优化，否则如果有IMU辅助预测，直接返回true，如果既搜索失败又没有IMU辅助预测，则返回false。TrackReferenceKeyFrame和TrackWithMotionModel分别基于参考关键帧和运动模型对当前帧的特征点进行匹配，并得到和参考关键帧或上一帧所匹配的特征点对应的地图点，Optimizer的PoseOptimization函数主要采用了前面帧得出的地图点世界坐标和对应的当前帧关键点的运动映射所建立的最小化重投影误差模型对相机的位姿和地图点进行联合优化，其具体采用了李代数和g2o的图优化方法，具体的介绍见后续的相关博文。
四、当跟踪的状态为RECENTLY_LOST，如果有IMU，则用IMU进行状态的辅助预测，保持状态为RECENTLY_LOST不变；否则调用Relocalization函数进行重定位，如果重定位成功，则状态转换为OK，否则状态转换为LOST。这里对重定位函数的实现也做一下说明，其主要基于当前帧从地图里查找候选关键帧(调用mpKeyFrameDB->DetectRelocalizationCandidates去检测)，然后基于每个候选帧分别和当前帧进行位姿优化（先采用的MLPnPSolver的优化求解方法，实时速度快，只优化相机位姿，找到匹配较优秀的结果作为初始值，和PoseOptimization相结合去进行进一步的更高精确度的位姿优化，具体的实现逻辑参考[2]。
五、当跟踪的状态为LOST，则重新创建新的地图，开始进入新的跟踪初始化状态。
六、当前帧本身的跟踪任务完成后，接下来需要调用TrackLocalMap对局部地图进行跟踪和相关的位姿和地图点的优化，在上一篇文章里也介绍过，局部地图没有专门的封装类，其主要的信息数据在Tracking类的两个变量中，分别为mvpLocalKeyFrames和mvpLocalMapPoints，在实现内部，其通过UpdateLocalMap函数的调用实现了局部关键帧和局部地图点的更新（只对当前的参考帧的匹配的地图点作为约束，约束量偏少，在更大的局部地图点中去搜索更多的匹配点，从而有更多的地图点加入到重投影误差的计算中来，对当前帧的位姿优化也更加准确），然后通过函数SearchLocalPoints去搜索匹配和当前帧在视锥体范围内的匹配的特征点，并基于这些特征点和地图点以及相机位姿等信息进行相机位姿优化。
七、接下来需要通过函数NeedNewKeyFrame来判断是否需要加入关键帧，根据多种条件，比如时间，比如有足够多的新的“接近点”等等，如果需要创建关键帧则调用CreateNewKeyFrame函数来创建关键帧，并将关键帧插入到局部建图器以便进行局部建图的工作。其中局部建图器的大体作用有：1、从尚未处理的关键帧队列里取出关键帧，对关键帧的地图点相关信息进行更新；2、更新关键帧的共视关键帧的图连接；3、将关键帧加入到地图集的当前地图中。这些具体的内容将会在局部建图器的文章中专门介绍。
问题：TrackLocalMap也是通过地图点和特征点的重投影误差采用的Optimizer的PoseOptimization函数进行的相机位姿和地图点的联合优化，为什么在当前帧的跟踪后面还要加上这个过程，其利用了更多的关键帧搜索地图点，因此优化会更准确些吗?回答：在 跟踪阶段，位姿优化依赖当前帧与前一帧（或参考关键帧）之间的特征点匹配关系，但 TrackLocalMap 可以通过当前帧的地图点去搜索局部的多个共视关键帧，查找更多的匹配的地图点。以从更多的视角来优化进一步提升定位精度和地图精度，使得SLAM系统在面对动态场景或挑战性环境时更加稳健。
References
Leave a Reply