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<mCurrentFrame.mvKeysUn.size(); i++)
             mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;    //存储了mInitialFrame中哪些点将进行接下来的匹配
            if(mpInitializer)
                delete mpInitializer;
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);//这里实例化Initialize类的一个对象指针，初始化的值为选取的当前帧和测量误差1.0，RANSAC迭代次数200
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
	对于第二帧：
	if((int)mCurrentFrame.mvKeys.size()<=100)    特征点个数要大于100个，如果小于100，初始化失败，程序返回图像帧处理入口。
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(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<Initializer*>(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<mnLastRelocFrameId+2)  //当运动模型是空的，或者刚刚使用重定位
                {
                    bOK = TrackReferenceKeyFrame();   //使用参考关键帧模型跟踪
                }
                else
                {
                    bOK = TrackWithMotionModel();  //使用运动模型跟踪
                    if(!bOK)     //如果使用运动模型跟踪失败
                        bOK = TrackReferenceKeyFrame();   //使用参考关键帧模型
                }
            }
            else   //
            {
                bOK = Relocalization();  //重定位
            }
        }
else    //只跟踪，不建图
        {
            if(mState==LOST)
            {
                bOK = Relocalization();
            }
            else
            {
                if(!mbVO)
                {
                    if(!mVelocity.empty())
                    {
                        bOK = TrackWithMotionModel();
                    }
                    else
                    {
                        bOK = TrackReferenceKeyFrame();
                    }
                }
                else   这一部分的内容在mbVO=true的时候执行，具体不是太清楚，后续在研究。
                {
                    // In last frame we tracked mainly "visual odometry" points.
                    // We compute two camera poses, one from motion model and one doing relocalization.
                    // If relocalization is sucessfull we choose that solution, otherwise we retain
                    // the "visual odometry" solution.
                    bool bOKMM = false;
                    bool bOKReloc = false;
                    vector<MapPoint*> vpMPsMM;
                    vector<bool> 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; i<mCurrentFrame.N; i++)
                            {
                                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                                {
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }
                    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();