轨迹跟踪器——Tracking
在上一篇文章中， 我们了解到 ORB-SLAM2 的系统对象 System 专门定义了一个轨迹跟踪器 ORB_SLAM2::Tracking 对象来完成相机运动的实时跟踪，也就是 SLAM 系统的前端里程计部分。本文中，我们从总体上解一下这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的跟踪过程我们将在第二部分中进行分析。
1. 成员变量
下面左侧的代码片段罗列了 Tracking 中表示跟踪状态和当前帧的 5 个成员变量。mState 和 mLastProcessedState 分别记录了当前帧和上一帧的跟踪状态，其枚举数值如下面的右侧代码所示。 成员变量 mSensor 记录了相机类型，它是 System 内部定义的一个枚举类型， 支持单目、双目、RGB-D三种形式的相机。mCurrentFrame 是当前帧对象 Frame，记录了图像的特征点、位姿矩阵等信息。 mImGray 则是当前帧对应的灰度图，它是由输入的 RGB 图像生成的。如果使用双目进行 SLAM，它对应的是左目的图像。
|
|
最初，ORB-SLAM 系统的提出是为了解决单目相机的定位建图问题的。由于单目相机不能直接提供空间信息，而且缺乏尺度。所以通常需要一个初始化的过程，建立空间坐标关系和相机的初始位姿， 以及基础参考尺度。下面罗列的这些成员变量就是用来进行初始化操作的。
ORB-SLAM2 会选择两帧具有足够多的匹配特征点和视差的图像，把相对较早的那帧图像当作参考帧进行初始化。下面的成员变量 mInitialFrame 记录了进行初始化时的参考帧，以后我们简称之为初始帧。 mvIniMatches 和 mvIniLastMatches 分别记录了当前帧和上一帧与初始帧的特征点匹配关系。mvbPrevMatched 记录着成功匹配的特征点像素坐标， 其实我并没有弄明白它到底有什么用，因为它只在调用 matcher.SearchForInitialization 时有用到。mvIniP3D 记录了成功三角化的特征点的3D坐标。
Frame mInitialFrame; //! 用于初始化的参考帧，简称初始帧
std::vector<int> mvIniMatches; //! 记录初始帧中各个特征点与当前帧匹配的特征点索引
std::vector<int> mvIniLastMatches; //! 记录初始帧与上一帧的匹配特征点关系
std::vector<cv::Point2f> mvbPrevMatched; //! 记录初始化过程中，上一次匹配的特征点坐标
std::vector<cv::Point3f> mvIniP3D; //! 记录初始化过程中，成功三角化的特征点的3D坐标
结束 SLAM 程序之后，为了方便导出相机的轨迹，作者用如下的这些 std::list 记录了所有的关键帧的位姿、数据、时间戳、跟踪状态。为了提高计算速度，降低系统的内存开销， ORB-SLAM2 并没有记录所有图像的数据，而是通过一些条件对每一帧图像进行了筛选。这些筛选出来的帧就是所谓的关键帧。
list<cv::Mat> mlRelativeFramePoses; //! 参考关键帧的位姿列表
list<KeyFrame*> mlpReferences; //! 参考关键帧列表
list<double> mlFrameTimes; //! 关键帧的时间戳
list<bool> mlbLost; //! 对应关键帧是否跟丢了
在分析系统对象 System 的时候，我们就已经注意到 ORB-SLAM2 还可以以纯定位的方式运行。在这种模式下，ORB-SLAM2 并不更新地图，就是一个视觉里程计。 在轨迹跟踪器 Tracking 中，有变量 mbOnlyTracking 记录这种工作模式。
在这种纯定位的工作模式下，作者还提供了一个标志 mbVO 反应视觉里程计的工作状态。如果该标志为 false，说明还有足够多的匹配点支持视觉里程计运行。 如果该标志为 true，基本上就跟丢了，系统将尝试进行重定位，来消除定位偏差(zero-drift)。
bool mbOnlyTracking; //! 只用于定位的标志，不进行局部建图(local mapping)
bool mbVO; //! 标志着地图中没有找到相应的匹配点(只适用于纯定位的模式)
下面是 ORB-SLAM2 中其它系统的对象指针，我们在类型 System 中都已经见到过了。只有最后的 mpInitializer 是轨迹跟踪器引入的，它只适用于单目相机进行地图初始化。
LocalMapping *mpLocalMapper; //! 局部地图管理器指针
LoopClosing *mpLoopClosing; //! 闭环检测器指针
System *mpSystem; //! ORB-SLAM2 系统对象指针
Viewer *mpViewer; //! 可视化对象
FrameDrawer *mpFrameDrawer; //! 帧渲染器
MapDrawer *mpMapDrawer; //! 地图渲染器
Initializer *mpInitializer; //! 地图初始化器，只用于单目相机
ORB-SLAM2 之所以冠以 ORB，是因为它通过 ORB 特征点的计算和匹配来建立稀疏的特征点地图。得益于特征点这一层抽象信息，ORB-SLAM2 可以同时支持单目相机、深度相机、双目相机等多种传感器。 一般情况下都是通过成员变量 mpORBextractorLeft 提取 ORB 特征的。对于双目相机还需要再提供一个关于右目的特征提取器。由于一些细微的配置不同， 在单目相机的初始化阶段，作者专门用 mpIniORBextractor 进行特征提取。
ORBextractor* mpORBextractorLeft; //! 单目、深度相机的特征提取器
ORBextractor* mpORBextractorRight; //! 对于双目相机还需要再提供一个特征提取器
ORBextractor* mpIniORBextractor; //! 专门用于单目初始化的特征提取器
下面的三个对象，在各个子系统中都有出现。其中 mpORBVocabulary 和 mpKeyFrameDB 分别是特征字典和关键帧数据库，它们是 ORB-SLAM2 通过词袋模型快速检索关键帧，进行回环检测、重定位的基础数据。 地图对象 mpMap 则是整个系统都在维护的对象，它是系统的输出结果。
ORBVocabulary* mpORBVocabulary; //! 词袋模型的特征字典
KeyFrameDatabase* mpKeyFrameDB; //! 关键帧数据库
Map* mpMap; //! 地图对象
在后面我们介绍相机的位姿估计时，会看到 ORB-SLAM2 在估计相机位姿的时候将先根据一个匀速模型估计一个不是很准的位姿初值， 然后通过匹配的特征点和优化方法得到一个较为精确的结果。下面的成员变量 mVelocity 就是用来记录相机的速度估计的。
cv::Mat mVelocity; //! 相机速度估计
int mnMatchesInliers; //! 当前帧中匹配点的数量
KeyFrame* mpReferenceKF; //! 当前参考关键帧
下面的这些成员变量记录了 Tracking 的配置，包括相机的内参、畸变、基线数据。基线，是双目相机特有的参数，它表示两个相机之间的距离。 对于深度相机，ORB-SLAM2规定了一个假想的基线，将之模拟成双目相机使用。此外，还包括一些阈值、比例系数。
cv::Mat mK; //! @brief 相机的内参矩阵
cv::Mat mDistCoef; //! @brief 相机的畸变系数
//! @brief 相机基线。对于双目就是两个相机之间的距离。
//! 对于RGBD相机，ORB SLAM2给定了一个假想的基线，模拟双目的数据。
float mbf;
int mMinFrames; //! 创建关键帧所需经历的最小帧数量
int mMaxFrames; //! 创建关键帧所需经历的最大帧数量
//! @brief 远近点判定阈值
//!
//! 对于双目或者深度相机，近点的深度是比较可信的，可以直接获取定位；
//! 而远处的点则需要在两个关键帧的匹配三角化得到具体的 3D 坐标。
//!
//! Points seen as close by the stereo/RGBD sensor are considered reliable
//! and inserted from just one frame. Far points requiere a match in two keyframes.
//!
float mThDepth;
float mDepthMapFactor; //! @brief 深度图比例系数，只适用于深度相机
除此之外，Tracking 还定义了一些成员变量记录着局部地图的关键帧和地图点，以及上一个关键帧的相关信息。
KeyFrame* mpLastKeyFrame; //! 上一个关键帧
Frame mLastFrame; //! 上一帧数据
unsigned int mnLastKeyFrameId; //! 上一个关键帧的ID
unsigned int mnLastRelocFrameId; //! 上次发生重定位的帧ID
bool mbRGB; //! RGB图像的颜色通道顺序，true: RGB, false: BGR
KeyFrame* mpReferenceKF; //! 当前参考关键帧
std::vector<KeyFrame*> mvpLocalKeyFrames; //! 局部地图中关键帧列表
std::vector<MapPoint*> mvpLocalMapPoints; //! 局部地图中地图点列表
list<MapPoint*> mlpTemporalPoints; //! 一个临时的地图点列表
2. 成员函数
类 Tracking 中定义了不少成员函数，我们主要关注如下的四个。其中构造函数负责初始化 SLAM 系统，加载相机内参、畸变系数等系统配置。 GrabImageStereo、GrabImageRGBD 和 GrabImageMonocular 已经在在介绍类 System 的时候见过了， 它们是具体处理传感器输入图像的接口。
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor); //! 构造函数
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp); //! 处理双目相机数据
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp); //! 处理深度相机数据
cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp); //! 处理单目相机数据
我们可以通过如下的 Set 接口来设定局部地图管理器、闭环探测器、可视化对象。
void SetLocalMapper(LocalMapping* pLocalMapper); //! 设置局部地图管理器
void SetLoopClosing(LoopClosing* pLoopClosing); //! 设置闭环探测器
void SetViewer(Viewer* pViewer); //! 设置可视化对象
此外，还可以还可以通过接口 ChangeCalibration 修改系统配置，通过接口 InformOnlyTracking 进入或者退出纯定位模式，通过接口 Reset 复位系统。
void ChangeCalibration(const string &strSettingPath); //! 加载新的配置
void InformOnlyTracking(const bool &flag); //! 进入纯定位模式
void Reset(); //! 复位系统
以上这些成员函数都是 public 的，我们可以直接调用。下面是 private 的成员函数，它们是跟踪相机位姿过程中需要调用的一些中间过程。 我们将在第二部分中进行详细分析。
void Track(); //! 具体执行相机位姿跟踪任务的函数。它的运行过程只用到了特征点，与输入的传感器类型没有关系。
void StereoInitialization(); //! 双目相机和深度相机的初始化函数
void MonocularInitialization(); //! 单目相机的初始化函数
void CreateInitialMapMonocular(); //! 生成单目相机所用的初始地图
bool TrackReferenceKeyFrame(); //! 根据参考关键帧估计相机位姿
void UpdateLastFrame(); //! 主要用于纯定位模式
bool TrackWithMotionModel(); //! 根据匀速模型估计相机位姿
bool Relocalization(); //! 根据词袋模型进行重定位
void UpdateLocalMap(); //! 更新局部地图
void UpdateLocalPoints(); //! 更新局部地图点
void UpdateLocalKeyFrames(); //! 更新局部的关键帧
bool TrackLocalMap(); //! 更新局部地图并优化相机位姿
void SearchLocalPoints(); //! 筛选局部地图点
void CheckReplacedInLastFrame(); //! 检查上一帧中是否有地图点被替换掉
bool NeedNewKeyFrame(); //! 判定是否需要新建关键帧
void CreateNewKeyFrame(); //! 新建关键帧
3. 构造函数
下面是 Tracking 的构造函数，它有很多输入参数，依次记录了 ORB-SLAM 系统对象、字典对象、帧渲染器、地图渲染器、地图对象、关键帧数据库对象、配置文件路径和相机类型。 后面的成员变量的初始化列表中，将输入的各个参数赋值给了对应的成员变量。同时给一些关键的成员变量赋予必要的初值，比如设置了跟踪状态的初值为 "NO_IMAGES_YET" 表示当前尚未接收到任何图像。
我并不喜欢一个函数有太多的输入参数，一方面是过多的输入参数将导致一行代码写的很长，看的时候比较费劲；另一方面是个人的脑容量有限，一次管理跟踪这么多参数有点吃力。 个人认为，不如将这个构造过程拆分成申请内存+初始化的两阶段构造过程。在构造函数中完成必要的内存申请，实例化 Tracking 对象。在初始化的过程完成各种系统对象指针的赋值，可以专门写一个初始化的函数， 也可以用一系列的 Set 接口或者直接赋值的方式逐一赋值。
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer,
Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor)
: mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL),
mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0) {
在构造函数中，先通过 OpenCV 的接口读取配置文件，获取了相机的光心和焦距参数，构建了相机的内参矩阵 mK。
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];
float cx = fSettings["Camera.cx"];
float cy = fSettings["Camera.cy"];
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
K.copyTo(mK);
然后加载了相机的镜头畸变系数。
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at<float>(0) = fSettings["Camera.k1"];
DistCoef.at<float>(1) = fSettings["Camera.k2"];
DistCoef.at<float>(2) = fSettings["Camera.p1"];
DistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
if(k3!=0)
{
DistCoef.resize(5);
DistCoef.at<float>(4) = k3;
}
DistCoef.copyTo(mDistCoef);
然后加载了相机基线、帧率，以及 ORB 特征点提取参数的配置。
|
|
然后根据相机类型构建必要的 ORB 特征提取器。正常我们只需要一个提取器 mpORBextractorLeft 就可以了，对于双目相机还需要一个右目的提取器 mpORBextractorRight， 对于单目相机还需要一个用于初始化的提取器 mpIniORBextractor。
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
接着，根据相机的基线和焦距给双目相机和深度相机计算了一个深度阈值。这两种相机可以直接估计出像素点到相机平面的距离，但是距离越远这个估计精度就越差。 所以设定了这个阈值，将特征点分成近点和远点，对于近点认为估计值比较准确可以直接应用它的的三维坐标。对于误差较大的远点，需要根据帧间的共视关系估计出一个较为精确的空间位置。
if(sensor==System::STEREO || sensor==System::RGBD)
mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
最后，由于一些深度相机输出的深度图是经过一个比例系数缩放过的，所以还需要加载这个比例系数，才能在实际计算的时候获得真正的尺度。
if(sensor==System::RGBD) {
mDepthMapFactor = fSettings["DepthMapFactor"];
if(fabs(mDepthMapFactor) < 1e-5)
mDepthMapFactor=1;
else
mDepthMapFactor = 1.0f/mDepthMapFactor;
}
}
4. 完
本文中，我们从总体上解一下这个数据类型的成员变量、成员函数、构造函数和一些主要的接口函数。 详细的跟踪过程我们将在第二部分中进行分析。