ORB-SLAM2梳理——track线程（一）
本科毕设所研究的是基于直接法的视觉里程计技术，采用的开源方案是DSO（Direct Sparse Odometry）。在引入光度校准和光度仿射变换之后，并且联合优化了所涉及的所有参数，DSO的效果很棒，被誉为目前效果最好的直接法的方案，但是它只是一个odometry，并不是一个完整视觉slam系统。因此，为了更好的研究视觉SLAM，对ORB-SLAM2的论文以及代码进行了研究，在这里写下自己的一些拙见，欢迎各位大神指出文中的不足和错误之处。
以ORB-SLAM2中传感器为单目相机运行EuRoc数据集为例分析流程。
首先分析运行参数，一共需要五个参数，并且需要按照特定的顺序排列，
| argv[0] | 可执行文件 |
|---|---|
| argv[1] | ORB词典文件 strVocFile |
| argv[2] | 参数设置文件 strSettingsFile |
| argv[3] | 图像文件 vstrImageFilenames |
| argv[4] | 时间戳 vTimestamps |
加载图片和曝光时间：LoadImages(string(argv[3]), string(argv[4]), vstrImageFilenames, vTimestamps);
系统初始化：ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); //读取配置文件
mpVocabulary = new ORBVocabulary();bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); //构建orb字典对象，读取orb字典
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); //创建关键帧数据库
mpMap = new Map(); //创建地图
mpFrameDrawer = new FrameDrawer(mpMap); mpMapDrawer = new MapDrawer(mpMap, strSettingsFile); // 可视化部分
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//track线程，在这个线程会读取配置文件进行相机内参矩阵的构建，畸变向量的构建，还会根据配置文件设置一些参数。
//最重要的是会构建两个特征提取的对象：
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST); //对于传感器为单目相机所特有，用于初始化部分的特征点提取，提取数量是设置的两倍。
另外此部分还会初始化另外两个线程，目前还未仔细去看，在分析完track线程之后再讨论
利用for循环遍历所有图像帧，图像帧处理入口：SLAM.TrackMonocular(im,tframe);
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
如果是第一帧：（或者未初始化成功）
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
参数：灰度图、时间戳、初始ORB特征提取对象、ORB词典对象、相机内参矩阵、畸变向量、剩下两个参数不清楚（看yaml文件里也没有，可能对于单目相机不重要）
ExtractORB(0,imGray);// 提取ORB特征
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors); //orb特征提取采用的是重载的运算符()。
UndistortKeyPoints(); //对特征点去畸变
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); //利用opencv提供的函数去畸变
ComputeImageBounds(imGray) //计算去畸变之后图像的边界，只对第一帧执行。
AssignFeaturesToGrid(); //将关键点划分到格子中
如果不是第一帧：（或者初始化成功）
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
将ORB提取对象换成了mpORBextractorLeft，其他基本不变。
在构建图像帧对象Frame()之后，开启跟踪。构建的对象为mCurrentFrame，此时该图像帧已经提取了特征点并计算了相应的描述子，具体的实现过程后续讨论。
开始跟踪：Track()；如果还未进行初始化，首先进行初始化操作，初始化操作需要连续的两个帧，并且两个帧的特征点匹配对数要大于100对，之后在进行三角化获得初始深度，构建初始地图。如果不满足条件则初始化失败，等下后续满足条件的连续两帧。
MonocularInitialization(); //单目初始化
对于第一帧：
if(mCurrentFrame.mvKeys.size()>100) //特征点个数要大于100个
{
mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size()); //调整vector的大小
for(size_t i=0; i(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
ORBmatcher matcher(0.9,true);
//对两帧的特征点进行匹配，参数：第一帧(初始帧)，第二帧(当前帧)，前一帧的特征点，两帧匹配特征点的索引，窗口大小(此参数不太了解)
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
if(nmatches<100) //如果匹配对数小于100，不能初始化成功，即初始化失败，程序返回图像帧入口。
{
delete mpInitializer;
mpInitializer = static_cast(NULL);
return;
}
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated) //当初始化的条件满足之后，执行初始化。
//初始化成功之后，将会删除初始化操作中三角化未成功的匹配点对，然后建立初始地图。
CreateInitialMapMonocular();
//至此，单目初始化的流程已全部走完，具体实现后续讨论。
在初始化成功之后，后续到来的帧，利用下面的代码构建帧Frame对象（前面已经提到过，这里再讲一下）。
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
构建了帧Frame的对象之后，执行Track()。
接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断）
1 只进行跟踪不建图，此时mbOnlyTracking为true
2 同时跟踪和建图，此时mbOnlyTracking为false。
初始化之后ORB-SLAM有三种跟踪模型可供选择
(a) TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿，根据匀速运动模型对上一帧的地图点进行跟踪，优化位姿。
(b) TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点，将上一帧的位姿作为当前帧的初始值，通过优化3D-2D的重投影误差来获得位姿。
(c）Relocalization()；重定位模型：计算当前帧的BoW，检测满足重定位条件的候选帧，通过BoW搜索当前帧与候选帧的匹配点大于15个点就进行PnP位姿估计并优化。
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
CheckReplacedInLastFrame(); // 检查并更新上一帧被替换的MapPoints，更新Fuse函数和SearchAndFuse函数替换的MapPoints
if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM;
vector vbOutMM;
cv::Mat TcwMM;
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
bOKReloc = Relocalization();
if(bOKMM && !bOKReloc)
{
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
for(int i =0; iIncreaseFound();
}
}
}
}
else if(bOKReloc)
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
} ////这一部分的内容在mbVO=true的时候执行，具体不是太清楚，后续在研究
}
}
在相应的跟踪状态下，采用相应的跟踪模型得到姿态初始估计之后，跟踪LocalMap，得到更多的匹配，并优化当前位姿。LocalMap：当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系，在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
在TrackLocalMap()之后；更新恒速运动模型TrackWithMotionModel中的mVelocity。
if(!mLastFrame.mTcw.empty()
{
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc;
}
判断是否要新加关键帧。
if(NeedNewKeyFrame())
CreateNewKeyFrame();
至此，orb-slam2的track线程基本走完，本文着重讨论的track线程的流程梳理。每个流程的具体实现的函数及代码在后续博客中讨论，其中包括：
/// ORBextracor的构造函数
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
/// 提取ORB特征的重载运算符()
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
/// 特征点匹配函数
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// 初始化函数
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)
//建立初始单目地图
CreateInitialMapMonocular();
// 三种跟踪模型
TrackReferenceKeyFrame()
TrackWithMotionModel()
Relocalization()
/// 跟踪局部地图
TrackLocalMap()
/// 判断是否要加关键帧以及如何创建关键帧
if(NeedNewKeyFrame())
CreateNewKeyFrame();