1. Tracking框架
Tracking线程流程框图：
各流程对应的主要函数
2. Tracking整体流程图
上面这张图把Tracking.cc讲的特别明白。
tracking线程在获取图像数据后，会传给函数GrabImageStereo、GrabImageRGBD或GrabImageMonocular进行预处理，这里以GrabImageMonocular为例。
GrabImageMonocular(const cv::Mat &im, const double ×tamp)
| 函数功能 | 1. 将图像转为mImGray并初始化mCurrentFrame； 2. 进行tracking过程，输出世界坐标系到该帧相机坐标系的变换矩阵 |
| im | 输入图像 |
| timestamp | 时间戳 |
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
{
mImGray = im;//读取图像
// 步骤1：将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
// 步骤2：构造Frame
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
// 步骤3：跟踪
Track();
return mCurrentFrame.mTcw.clone();
}
数据，流到Track()，由于代码超长，分几段粘贴注释。
void Tracking::Track()
| 步骤 | 1. 判断tracking状态：如果是未初始化（NOT_INITIALIZED），则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()进行初始化，并更新地图视图。 2.对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断） （1）只进行跟踪不建图 （2）同时跟踪和建图： 初始化之后ORB-SLAM有三种跟踪模型可供选择 a.TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。 b.TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。 c.Relocalization()；重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。 这三个模型的选择方法： 首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪彻底丢失，要么等待相机回转，要不进行重置。 |
2.1. 初始化部分
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态
// SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock lock(mpMap->mMutexMapUpdate);
// 步骤1：初始化
if(mState==NOT_INITIALIZED)//判断是否初始化
{
if(mSensor==System::STEREO || mSensor==System::RGBD)//双目或深度相机
StereoInitialization();//双目初始化
else
MonocularInitialization();//单目初始化
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
}
2.3. 跟踪
2.3.1. 跟踪上一帧或者参考帧或者重定位
else// 步骤2：跟踪
{
// System is initialized. Track Frame.系统完成初始化，跟踪帧
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)运用运动模型或重定位初始化相机位姿估计
// 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
// 正常初始化成功
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// 运动模型是空的或刚完成重定位
// mCurrentFrame.mnIdSetCurrentCameraPose(mCurrentFrame.mTcw);
2.4.1. 清除UpdateLastFrame中为当前帧临时添加的MapPoints
清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
检测并插入关键帧，对于双目会产生新的MapPoints
// 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
for(int i=0; iObservations() 
