轨迹跟踪的总体过程
轨迹跟踪的主要目的是估计相机的位姿，很多vSLAM系统都将完成这一任务的部分称为视觉里程计。ORB-SLAM2 在一个独立的线程 TRACKING 中完成这一任务， 该线程将处理每一帧图像，提取ORB特征点，如果尚未初始化地图，则先进行初始化。再来根据地图信息和当前机器人运动状态来估计相机位姿或者进行重定位，然后在局部地图中完成轨迹跟踪， 并根据一定的规则生成新的关键帧。
我们在第一部分的介绍中，了解到 ORB_SLAM2 通过类 Tracking 将上述过程封装到一个 轨迹跟踪器 中完成。本文中， 我们将结合2015年的论文和代码从总体上了解轨迹跟踪的具体过程。
1. 总体过程浅析
右图是从原文的系统框图中截取的关于轨迹跟踪的过程。在论文的第 V 部分中，作者从 “Extract ORB” 到 “New KeyFrame Decision” 详细地介绍了框图中的四个过程。 由于单目相机的 SLAM 系统初始化有些复杂，作者专门在文章的第 IV 部分中论述了他的初始化方法。总体上来说，轨迹跟踪线程有五个环节构成：
- ORB 特征点提取: 对应框图中的 “Extrac ORB”。特征点的提取和匹配是 ORB-SLAM 系统得以运行的基础，通过图像特征点描述环境， 特征点的匹配关系在不同视角的图像之间建立了几何约束，进而可以估计出相机的位姿。
- 地图初始化: 对应框图中的“Map Initialization”。这个主要在强调单目相机的初始化。虽然深度相机和双目相机也涉及到地图初始化的问题， 但由于它们能够直接测量出物体到相机的距离，所以初始化过程比较简单，单帧的数据也可以完成。而单独一张图片是不能建立几何约束的，所以单目相机的初始化过程就要复杂很多。
- 相机位姿估计：对应框图中的“Initial Pose Estimation ...”。主要是根据前后两帧的数据进行特征点匹配估计相机位姿。 但是由于特征点的匹配存在很多不确定的因素，经常出现错误的匹配，或者匹配点很少的情况。为了得到可靠的初始估计，作者通过词袋模型给出了一种重定位的方法。
- 相机位姿优化：对应框图中的“Track Local Map”。在一个局部的地图中，可以建立多个帧之间的特征点匹配关系，进而可以构建更多更强的约束， 描述成一个 BA 优化问题，解之可以得到一个更准确的位姿估计。
- 生成关键帧：对应框图中的“New KeyFrame Decision”。要记录下所有图像之间的几何约束，对计算机的内存和算力都提出了很高的要求。 所以ORB-SLAM2只记录了一些满足特定条件的帧，这些帧就是关键帧。
我们在分析系统对象时了解到， ORB_SLAM2::System通过TrackStereo、TrackMonocular、TrackRGBD三个接口分别来处理双目、单目、深度相机的数据，它们以OpenCV的数据结构 cv::Mat 返回了相机的位姿估计。其实现过程也十分相似， 都先检查了系统的工作模式复位状态等一些标志位之后，调用ORB_SLAM2::Tracking的GrabImageXXX 的接口。如右图所示， 这三个 GrabImageXXX 的接口只是将彩色图转换成深度图，根据不同的传感器选择 ORB 特征提取器，构建数据对象 Frame，最后调用私有成员函数 Track 完成轨迹跟踪。
下面是函数 GrabImageMonocular 的实现，用于处理单目相机的数据。有两个输入参数 im 和 timestamp 分别记录着相机的图像数据和时间戳。 按照我个人的习惯，将const 引用形式的 timestamp 改成了值传递的形式。 这个函数大体上做了三件事情: 1. 在第3到13行中，判定输入的图像类型，若是彩色图则将之转换为灰度图； 2. 在15到18行中，根据轨迹跟踪器的状态构建对象 mCurrentFrame。若尚未完成初始化，使用 mpIniORBextractor 作为特征提取器，否则用 mpORBextractorLeft。 3. 关键是第20行调用 Track() 以刚刚构建的类 Frame 的对象为输入，完成了轨迹跟踪的任务，输出相机位姿。
cv::Mat Tracking::GrabImageMonocular(const cv::Mat & im, double timestamp) {
mImGray = im;
if(3 == mImGray.channels()) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
} else if(4 == mImGray.channels()) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
mCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
else
mCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
下面是函数 GrabImageRGBD 的实现，用于处理深度相机的数据，有三个输入参数 imRGB, imD, timestamp 分别记录着彩色图(或者灰度图)、深度图和时间戳。 所做的工作与 GrabImageMonocular 差别不大，只是在构建对象 mCurrentFrame 之前，先处理了一下深度图的尺度。深度图的存储方式并没有太多的标准， 比较常见的做法是，用一个无符号的16位整型的数据记录每个像素对应的深度值，单位是 mm。最大值 65535 对应着 65.535 米。实际上目前的深度相机测量距离都很短， 一般也就3到5米左右，而且距离太远了精度就无法保证了。所有有些相机厂商的深度图缩放比例并不是 1000，可能更大，比如我们所用的 TUM 数据集中的参数就是 5000。
cv::Mat Tracking::GrabImageRGBD(const cv::Mat & imRGB, const cv::Mat & imD, double timestamp) {
mImGray = imRGB;
cv::Mat imDepth = imD;
if (3 == mImGray.channels()) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
} else if (4 == mImGray.channels()) {
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
if((fabs(mDepthMapFactor-1.0f) > 1e-5) || imDepth.type() != CV_32F)
imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
mCurrentFrame = Frame(mImGray, imDepth, timestamp,
mpORBextractorLeft,
mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
下面是函数 GrabImageStereo 的实现，以左右两目的图像和时间戳为输入，处理双目相机的数据，输出相机位姿。 该函数也是先将彩色图转换为深度图，再构建对象 mCurrentFrame，最后调用 Track 函数。
cv::Mat Tracking::GrabImageStereo(const cv::Mat & imRectLeft, const cv::Mat & imRectRight, double timestamp) {
mImGray = imRectLeft;
cv::Mat imGrayRight = imRectRight;
if(mImGray.channels()==3) {
if(mbRGB) {
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
} else {
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
}
} else if(mImGray.channels()==4) {
if(mbRGB) {
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
} else {
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
}
}
mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
Track();
return mCurrentFrame.mTcw.clone();
}
2. ORB 特征点提取
所谓的特征点是图像中的一些特殊的点，它与周围的一些像素构成特定的模式，这种模式在不同的视角下观测变化并不大。特征点一般由关键点(Key-Point)和描述子(Descriptor)两部分构成， 其中关键点主要在记录像素坐标，描述子记录的是像素模式。特征点的计算方法和描述子有很多种方法，常见的有 SIFT, SURF, ORB 等著名的特征点。
ORB-SLAM 的轨迹跟踪器使用 ORB 特征点描述图像特征，建立帧间的匹配关系。进而建立几何约束，求解当前帧相对于上一帧的位姿变换关系。 ORB 提取的是 FAST 角点，采用 BRIEF 描述子描述特征。ORB 特征点的优点是计算起来比较快，适合用于有实时性需求的视觉里程计中。 缺点是原始的 ORB 特征点并没有记录方向信息，而且容易扎堆出现在一个局部的区域。为了提供稳定可靠的匹配关系，ORB-SLAM 做了一些细微的改动。
为了保证不同的距离上都有可用的特征点，ORB-SLAM 以 1.2 的缩放比例逐层构建了一个8层的图像金字塔，并为每层图像计算FAST角点，这样可以也就有了所谓的尺度不变性。 如果特征点能够均匀的分布在整个图片上，有助于提高系统的定位精度。为此，ORB-SLAM 将图像划分成了一些小格子，并尝试在每个小格子中提取出至少5个角点。 如果某个单元中无法提取足够多的角点，就降低FAST角点的判定阈值。如果实在检测不出5个角点来，就有多少是多少吧。ORB-SLAM 虽然没有对 BRIEF 描述子的计算进行什么改动， 但是它引入了一个方向的算子，可以在一定程度上评价 FAST 角点的朝向，如此得到的特征点就能在一定程度上描述旋转量。
在 GrabImageXXX 的接口函数中，Trcaking 专门定义了各种 ORB 特征点提取器，并用它们构造了 Frame 对象。特征的提取实际上是在 Frame 的构造过程中，有特征提取器完成的。 ORB-SLAM 专门写了ORBextractor 和ORBmatcher完成特征点提取和匹配， 详细的实现过程我们放到附录中讨论。
3. 自动的地图初始化方法
地图初始化主要是在确立坐标系。总的来说需要确定三个要素：坐标系形式、坐标原点、地图尺度。坐标形式是在说地图的建模方式，是2D的占用栅格？3D的点云？ 使用笛卡尔坐标系的 xyz 描述空间位置？还是地球坐标系的经纬高来描述？这些信息在建图之前，就已经规定好了。对于 ORB-SLAM2 而言，它的地图是 3D 的稀疏点云， 采用笛卡尔坐标系描述位置。
坐标原点的确定为整个地图建立了参考关系。参考地球坐标系，我们在使用 GPS 等设备定位的时候，给出的经纬高坐标就是相对于地球上由"格林尼治经线-赤道-海平面"相交的那一点的相对位置关系。 在 ORB-SLAM2 中，由于地图是随着系统的运行逐渐构建的，事先并没有一个特定的参考，所以它以采集第一帧图像的相机位置为参考原点。对于同一个环境，从不同的位置开始建图， 得到的结果是不一样的，但地图的大体结构应该差异不大。
地图的尺度是指地图的比例尺。还是拿世界地图来说，一张纸上能够画完整个世界，是把地球按照一定的比例缩小之后涂画上去的。地图是对我们周围环境的一个抽象表达， 它直接描述了空间中各点相对于原点的位置关系，给各个点坐标同乘以一个相同的系数并不会改变各点在空间中排布的形态。
对于同一张图像上的两个点，我们可以直接得到它们之间相差了多少个像素，但是不能给出这几个像素在被测量空间中相差了多少米。 对于深度相机或者双目相机，可以通过额外的结构光或者多个相机之间的安装位置估计出对应点距离相机平面的距离，也就是所谓的深度。进而可以估计出在被测量的空间中的相对位置。 经过比较精密的标定工作，我们可以事先确定传感器的深度估计值与真实世界的比例关系，进而确定尺度。
void Tracking::Track() {
if(mState==NO_IMAGES_YET)
mState = NOT_INITIALIZED;
mLastProcessedState=mState;
unique_lock lock(mpMap->mMutexMapUpdate);
if(mState==NOT_INITIALIZED) {
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
对于单目相机而言，这个尺度的确立就比较麻烦。仅根据相机的图像本身，我们只能在相机发生了位移之后，再根据匹配的特征点来估计相机的运动， 而且地图的尺度也只能是相对于前两帧的运动关系的一个相对值。这里涉及到一个尺度缺失的问题， 将在后文详细讨论单目相机的建图原理时详细解释。
地图初始化对于深度相机和双目相机而言比较简单，一帧图像就可以搞定。单目相机需要两帧具有一定视差的图像来完成这一任务。ORB的作者提出了一种自动化的初始化策略， 不需要像它的前身PTAM那样人为的选择两个具有较大视差的帧，使用起来十分方便。针对平面场景(planar scene)和非平面场景(non-planar scene)， ORB-SLAM2提供了单应(homography)和基础矩阵(fundamental matrix)两种几何模型。它会在两个线程中同时计算这两个模型，然后根据一些启发式信息选取一个较合适的模型。
右侧是 Track 函数的代码片段，我们只截取了函数一开始那段与初始化相关的代码。该函数先检查成员变量 mState 记录的状态，并对地图对象加锁，保护公共资源。 如果 mState 显示尚未完成初始化，将根据传感器类型调用相关的接口完成初始化操作，对于单目是 MonocularInitialization，对于深度和双目是 StereoInitialization。 关于这两个函数的具体实现，我们放到帧间位姿估计和地图初始化总体过程中详述。
4. 相机位姿的初始估计
完成了地图初始化之后，我们就可以在此基础上估计相机的位姿，有人把这一任务称为视觉里程计。 一般情况下ORB-SLAM都是先通过匀速运动模型粗略的估计相机位姿，再通过特征点匹配和优化方法给出一个较为精确的位姿估计。有时相机可能运动过快，导致里程计跟丢了，此时ORB-SLAM会通过词袋模型， 以及特征点匹配来重定位。
// Tracking::Track()的代码片段
if (mState == NOT_INITIALIZED) {
// 先给我老老实实的完成了初始化任务再来。
} else {
bool bOK;
if(!mbOnlyTracking) {
if(mState == OK) {
CheckReplacedInLastFrame();
if(mVelocity.empty() ||
mCurrentFrame.mnId < mnLastRelocFrameId+2) {
bOK = TrackReferenceKeyFrame();
} else {
bOK = TrackWithMotionModel();
if (!bOK)
bOK = TrackReferenceKeyFrame();
}
} else {
bOK = Relocalization();
}
} else {
// 省略纯定位工作模式的分支
}
}
我们只要判定地图已经进行了初始化，就可以正常的估计相机位姿了。如右边的代码片段所示，函数 Track() 检查当前的跟踪器的状态，通过后就可以估计相机位姿了。 ORB-SLAM2 有纯定位和建图两种工作模式，类 Tracking 通过成员变量 mbOnlyTracking 来进行区分，这里我们只考虑建图模式的实现。当系统状态mState处于OK时， 意味着当前的视觉里程计成功地跟上了相机的运动。如果跟丢了就只能进行重定位了。
论文中说如果上一帧图像成功的跟上了相机的运动， 并成功的估计了速度，就以匀速运动模型来估计相机的位姿。然后以此为参考，把上一帧中观测到的地图点投影到当前帧上，并在一个比较小的范围里搜索匹配的特征点。如果找不到足够多的匹配特征点， 就适当的放大搜索半径。找到了足够多的匹配特征点后，就进行一次优化提高相机位姿估计的精度。这就是第13行中调用的函数TrackWithMotionModel所实现的功能。
第11和15行中调用的TrackReferenceKeyFrame，在论文中并没有找到特定的描述。我理解它也是一个重定位只是没有Relocalization的搜索范围大而已。第9、10行的分支条件如果成立， 意味着上一帧可能进行了重定位，所以机器人的速度估计丢失了，帧ID也可能因为重定位而向前发生了跳转。此时再用匀速运动模型来估计位姿就显得不合适了，所以要从参考关键帧上估计位姿。 第14行的条件是说如果匀速运动模型失效了，可以先通过TrackReferenceKeyFrame补救一下。若仍然无法成功估计位姿，意味着我们跟丢了，需要进行重定位。 其实从代码来看，如果 TrackWithMotionModel 和 TrackReferenceKeyFrame 都失效了，当前帧将被抛弃掉，在下一帧图像到来的时候才有可能调用 Relocalization 进行重定位。 为什么不直接重定位再试一把呢，不知道作者是不是故意的。
5. 根据局部地图优化相机位姿
通过匀速运动模型或者重定位，我们已经得到了相机位姿的初始估计。同时轨迹跟踪器还找到了一个与当前帧初步匹配的地图点集合。 此时，类 Tracking 就把地图投影到当前帧上找到了更多的匹配地图点，进而再次优化相机的位姿，得到更为精准的估计。
// Tracking::Track()的代码片段
if(!mbOnlyTracking) {
if(bOK)
bOK = TrackLocalMap();
} else { // 省略纯跟踪模式下相关代码 }
mState = bOK ? OK : LOST;
毕竟这个优化过程还是前端里程计的一部分，计算时间不宜过长。为了限制这一搜索过程的复杂度，ORB-SLAM2 只对一个局部地图进行了投影。 这个局部地图实际上是当前帧在共视图中的两级邻接关键帧，包含了两个关键帧集合\(\mathcal{K_1}、\mathcal{K_2}\)。 其中，\(\mathcal{K_1}\)中的关键帧都与当前帧有共视的地图点，\(\mathcal{K_2}\)则是\(\mathcal{K_1}\)的元素在共视图中的邻接关键帧。
右侧是函数 Track() 中关于局部地图位姿优化的代码片段，省略了 else 分支下关于纯定位工作模式的语句。 局部变量 bOK 是在 Track() 函数中定义的一个布尔类型的数据，它反映了是否成功估计了相机位姿，若没有则意味着当前视觉里程计跟丢了，再进行局部地图更新就没有意义了。 具体的工作交给了函数 TrackLocalMap() 来完成。主要是更新局部地图，匹配更多的地图点，优化求解相机位姿。 详细的代码，放到根据局部地图优化位姿中讨论。
6. 生成关键帧
完成了基于局部地图的相机位姿优化之后，如果局部变量 mOK 仍然表示轨迹跟踪器一切正常，就可以根据一定条件生成关键帧了。关键帧的目的是对相机的数据进行采样， 尽量完整的描述地图环境的同时，减少需要保存的数据。达到降低内存和算力的开销，提高系统效率的目的。 论文中说要插入新的关键帧需要满足如下的几个条件：
- 如果发生了重定位，那么需要在20帧之后才能添加新的关键帧;
- LOCAL MAPPING线程处于空闲(idle)的状态，或者距离上次插入关键帧已经过去了20帧;
- 当前帧至少保留了50个匹配的地图点;
- 当前帧跟踪的地图点数量少于参考帧的90%。
下面是函数 Track() 判定并生成关键帧的语句，十分直接，调用NeedNewKeyFrame进行判断，调用CreateNewKeyFrame生成。详细的源码分析， 我们放到生成关键帧中。
// 省略状态更新以及释放临时资源相关的语句
if (NeedNewKeyFrame())
CreateNewKeyFrame();
// 省略进一步剔除野点的语句
}
// 省略保存轨迹数据相关的语句
在上面的代码片段中，我们省略了很多语句，它们主要是一些关于状态更新、释放资源、剔除野点、保存数据相关的语句。虽然和我们关注的轨迹跟踪总体逻辑没有太大关系， 但对于整个系统的运行而言还是很重要的。
7. 完
本文中，我们结合2015年的论文和代码从总体上了解轨迹跟踪的具体过程。虽然针对三种不同类型的相机，类 Tracking 中分别提供了 GrabImageMonocular, GrabImageRGBD, GrabImageStereo 三个接口，但最终都归结到成员函数 Track() 中完成跟踪任务。
我们还拆解了函数 Track()，大致讨论了轨迹跟踪任务的5个环节，本部分的其它文章将逐一讨论其中的地图初始化、相机位姿估计、根据局部地图优化位姿、生成关键帧四个环节。 ORB特征点提取放到附录中完成。