ORB-SLAM2 源码阅读笔记：System.cc与Tracking.cc程序详解

本文从\ORB_SLAM2\Examples\ROS\ORB_SLAM2\src\ros_mono.cc开始，记录了ORB_SLAM2框架中System.cc与Tracking.cc代码的相关阅读笔记，在各位大佬的解析基础上加入了自己的一些理解。

ORB-SLAM是一个基于特征点的实时单目SLAM系统，在大规模的、小规模的、室内室外的环境都可以运行。该系统包含了所有SLAM系统共有的模块：跟踪（Tracking）、建图（Mapping）、重定位（Relocalization）、闭环检测（Loop closing）。ORB-SLAM2在ORB-SLAM的基础上，还支持标定后的双目相机和RGB-D相机。

ORB-SLAM2源码结构如下图所示，Examples为相关应用示例，包含单目图像数据库Monocular的调用例子，例如Tum、Kitti、Euroc数据集的调用；深度图像数据库RGB-D的调用例子，例如TUM数据集的调用；ROS订阅方式的摄像头调用例子，包括单目摄像头、深度摄像头等的调用，以及AR的demo示例。include中包含.h头文件，src中为.cc源文件。

ORB-SLAM2主要分为三个线程进行，分别是跟踪Tracking、定位与建图LocalMapping 和 闭环检测LoopClosing。三个线程分别存放在对应的三个文件中，分别是Tracking.cpp、LocalMapping.cpp和LoopClosing.cpp文件：

1）主线程：Tracking线程为主线程，主要工作是从图像中提取ORB特征，根据上一帧进行姿态估计，或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧。
2）Local mappng线程：主要完成局部地图构建，包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用局部捆集调整（Local BA），最后再对插入的关键帧进行筛选，去除多余的关键帧。
3）Loop closing线程：主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用BOW进行探测，然后通过Sim3算法计算相似变换。闭环校正，主要是闭环融合和Essential Graph的图优化。
4）Viewer线程：对估计的位姿和特征点进行可视化显示。

目 录

RB-SLAM2 源码解析与阅读笔记一)

1、示例Examples （ros_mono.cc）

这里以ROS示例中的ros_mono.cc源文件为例进行详细阅读，其他示例与之类似，不再赘述。

ros_mono.cc里的main函数如下：

int main(int argc, char **argv)
{
     
    // ros初始化
    ros::init(argc, argv, "Mono");
    ros::start();
    if(argc != 3)
    {
     
        cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary path_to_settings" << endl;        
        ros::shutdown();
        return 1;
    }    

    // 创建SLAM系统. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
    // 我们在运行代码时，需要输入的两个参数就是 词袋路径 和 设置文件路径 ORB_SLAM2::System::MONOCULAR表示创建SLAM系统为单目SLAM系统，true表示进行可视化操作。

    ImageGrabber igb(&SLAM);
    ros::NodeHandle nodeHandler;
    // 传入图像进行追踪 -- 通过一个循环过程将图像传入SLAM系统中进行追踪。
    ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);
    
    // ros::spin() ROS消息回调处理函数,调用后不会再返回
    // 将会进入循环， 一直调用回调函数GrabImage()
    ros::spin();

    // 停止SLAM系统并保存轨迹 Stop all threads
    SLAM.Shutdown();
    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    ros::shutdown();

    return 0;
}

从代码中能够看出，main()函数执行了以下几个操作：对ROS进行初始化；创建SLAM系统，调用System(）主流程进行初始化以及开启ORB_SLAM里的三个线程；通过ROS订阅摄像头图像信息并传入图像进行追踪，调用System::TrackMonocular(）处理每帧图像；调用shutdown(）停止SLAM系统中的所有线程；保存轨迹路径。

ros_mono.cc里的GrabImage()回调函数如下：

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
{
     
    // 将ros订阅的消息转为图像矩阵 Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
     
        cv_ptr = cv_bridge::toCvShare(msg);
    }
    catch (cv_bridge::Exception& e)
    {
     
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    // 调起System::TrackMonocular(）处理每帧图像
    mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
}

2、SLAM系统文件（System.cc）

ORB_SLAM2的作者将整个系统进行了完整的装并定义了一个System类作为系统入口。

2.1、单目跟踪 System::TrackMonocular()

上文中的TrackMonocular()函数也在System.cc中，先跳转进去看一下里面做了哪些工作：

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{
     
    if(mSensor!=MONOCULAR)
    {
     
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

    // Check mode change -- 先检查系统状态
    {
     
        unique_lock<mutex> lock(mMutexMode); //在该声明周期内对 mMutexMode进行上锁操作，不允许其他线程修改定位模式
        if(mbActivateLocalizationMode) // 如果激活定位模式
        {
     
            mpLocalMapper->RequestStop(); // 局部地图建图请求停止
            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped()) // 等待局部建图完全停止
            {
     
                usleep(1000);
            }

            mpTracker->InformOnlyTracking(true); // Track线程模式为 OnlyTracking
            mbActivateLocalizationMode = false; // 定位跟踪模式设置完成，标志位置为false
        }
        if(mbDeactivateLocalizationMode) // 如果失活定位模式
        {
     
            mpTracker->InformOnlyTracking(false); // 失活OnlyTracking模式
            mpLocalMapper->Release(); // 清空局部地图
            mbDeactivateLocalizationMode = false; // 标志位置为false
        }
    }

    // Check reset
    {
     
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset) // 如果reset，就将tracker重置
    {
     
        mpTracker->Reset();
        mbReset = false;
    }
    }
    // 获取RGBD图像到系统中
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState; // TrakingState为Tracker的State 
                                        // 更新追踪状态
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; // TrackedPoints为当前帧的Mappoints 
                                                               // 更新地图点向量
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; // TrackedKeyPointsUn为当前帧的去畸变的关键点 
                                                             // 更新关键点向量

    return Tcw; //返回当前相机的位姿
}

在TrackMonocular()函数中主要执行以下操作：首先检验传感器类型，若传感器类型错误则直接退出；之后检验定位模式的变化，若定位模式被激活，则关闭局部建图功能，系统只进行定位，若定位模式被关闭，则之前建立的局部地图被释放（清除）；之后检验系统是否被重启（有UI界面控制），若系统重启，则重启追踪器，为追踪器输入图像数据，返回世界坐标系到相机坐标系的变换矩阵，更新变量。

可以看出程序的真正入口是在GrabImageMonocular() 中，该函数在Tracking.cc中实现，后文中再解读。
先继续看一下这个System类包含了哪些关于SLAM系统的工作：

2.2、构造函数 System::System()

函数输入如下：

const string &strVocFile,	//词袋文件路径
const string &strSettingsFile,	//设置文件路径
const eSensor sensor,	//传感器类型：MONOCULAR,STEREO,RGBD
const bool bUseViewer	//是否可视化

构造函数System()中的主要内容如下所示，具体操作为判断传感器类型；检验配置文件；载入词袋；实例化相关成员变量，包括局部建图线程、回环检测线程和视图线程的实例化；各个线程之间交叉设置指针。

//Check settings file -- 将配置文件读入到内存中，存储到fsSettings变量中
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);	

//Load ORB Vocabulary --  实例化词袋管理，这里要加载一个一百多兆的词袋
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);

//Create KeyFrame Database -- 实例化关键帧数据管理
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

//Create the Map -- 实例化地图
mpMap = new Map();

//Create Drawers. These are used by the Viewer
//实例化观测器：帧的观测器和地图观测器
mpFrameDrawer = new FrameDrawer(mpMap); //ORBSLAM跑起来以后维护右边那个图，图下面显示关键帧信息；
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile); //ORBSLAM跑起来以后维护左边那个点云和关键帧位置的图；

//Initialize the Tracking thread 实例化Tracking线程，会根据strSettingFile读入相机参数
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);

//Initialize the Local Mapping thread and launch
//实例化局部优化线程（BA）
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

//Initialize the Loop Closing thread and launch
//实例化回环检测线程（全局优化）
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

//Initialize the Viewer thread and launch
if(bUseViewer)
{
     	//如果可视化，创建一个可视化的线程
	mpViewer = new Viewer(this,mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
	mptViewer = new thread(&Viewer::Run, mpViewer);
	mpTracker->SetViewer(mpViewer);
}

//Set pointers between threads
//设置每个线程中所包含的 其他线程指针
 mpTracker->SetLocalMapper(mpLocalMapper);
 mpTracker->SetLoopClosing(mpLoopCloser);

 mpLocalMapper->SetTracker(mpTracker);
 mpLocalMapper->SetLoopCloser(mpLoopCloser);

 mpLoopCloser->SetTracker(mpTracker);
 mpLoopCloser->SetLocalMapper(mpLocalMapper);      

2.3、保存轨迹 System::SaveKeyFrameTrajectoryTUM()

SaveKeyFrameTrajectoryTUM()函数中的主要代码如下，功能为求数据集中每一关键帧的位姿：

void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
     
    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames(); //获取所有的关键帧
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); //对关键帧进行排序

    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    //cv::Mat Two = vpKFs[0]->GetPoseInverse();
    ofstream f;
    f.open(filename.c_str());
    f << fixed;
    
    for(size_t i=0; i<vpKFs.size(); i++)
    {
     
        KeyFrame* pKF = vpKFs[i]; 
       // pKF->SetPose(pKF->GetPose()*Two); 
        if(pKF->isBad())
            continue;

        cv::Mat R = pKF->GetRotation().t();
        vector<float> q = Converter::toQuaternion(R);
        cv::Mat t = pKF->GetCameraCenter();
        //将旋转矩阵和平移向量写入文件
        f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
          << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
    }

    f.close();
    cout << endl << "trajectory saved!" << endl;
}

3、跟踪Tracking （Tracking.cc）

接下来进行跟踪线程的代码阅读与学习解析。Tracking线程在上文2.1中获取图像数据后，会传给函数GrabImageStereo()、GrabImageRGBD()或GrabImageMonocular()函数进行预处理，这里依旧以单目GrabImageMonocular()函数为例进行具体阅读。

3.1、图像预处理 Tracking::GrabImageMonocular()

GrabImageMonocular()函数的代码如下：

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
{
     
    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();
}

可以看出该函数功能为将图像转为灰度图像并初始化当前帧mCurrentFrame，之后调起Track()函数进行跟踪过程，输出世界坐标系到该帧相机坐标系的变换矩阵。

3.2、跟踪 Tracking::Track()

接下来阅读并解析Track()函数，代码如下（代码超长，务必耐心…）：

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<mutex> 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：跟踪 -----------------------------------------------------------------------
    else
    {
     
        // System is initialized. Track Frame.
        bool bOK;
        // 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(); //Local Mapping线程可能会将关键帧中某些MapPoints进行替换，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints
                
                // 步骤2.1：跟踪上一帧或者参考帧或者重定位
                // 状态：运动模型是空的或刚完成重定位
                // mCurrentFrame.mnId
                // 应该只要mVelocity不为空，就优先选择TrackWithMotionModel
                // mnLastRelocFrameId为上一次重定位的那一帧
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
     
                    // 将上一帧的位姿作为当前帧的初始位姿
                    // 通过BoW的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点都对应3D点重投影误差即可得到位姿
                    bOK = TrackReferenceKeyFrame(); // 跟踪参考帧
                }
                else
                {
     
                    // 根据恒速模型设定当前帧的初始位姿
                    // 通过投影的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点所对应3D点的投影误差即可得到位姿
                    bOK = TrackWithMotionModel(); // 根据固定运动速度模型预测当前帧的位姿
                    if(!bOK)
                        // TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
                        // 最后通过优化得到优化后的位姿
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
     
                // BOW搜索，PnP求解位姿
                bOK = Relocalization(); // 重定位成功与否
            }
            // 两两跟踪（恒速模型跟踪上一帧、跟踪参考帧）
        }        
        else
        {
     
            // Localization Mode: Local Mapping is deactivated
            // 只进行跟踪tracking，局部地图不工作
            // 步骤2.2：跟踪上一帧或者参考帧或者重定位
            // 状态：tracking跟丢了
            if(mState==LOST)
            {
     
                bOK = Relocalization(); // 判断重定位成功与否标志
            }
            else
            {
     
                // mbVO是mbOnlyTracking为true时的才有的一个变量
                // mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常，
                // mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏....
                if(!mbVO)
                {
     
                    // In last frame we tracked enough MapPoints in the map
                    // mbVO为0则表明此帧匹配了很多的3D map点，非常好
                    if(!mVelocity.empty())
                    {
     
                        bOK = TrackWithMotionModel();
                    }
                    else
                    {
     
                        bOK = TrackReferenceKeyFrame();
                    }
                }
                else
                {
     
                    // 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.
                    // mbVO为1，则表明此帧匹配了很少的3D map点，少于10个，又要跪的节奏，既做跟踪又做定位
                    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)
                        {
     
                            // 更新当前帧的MapPoints被观测程度
                            // 搜索局部关键帧后搜集所有局部MapPoints，然后将局部MapPoints和当前帧进行投影匹配，
                            // 得到更多匹配的MapPoints后进行Pose优化
                            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;
                }
            }
        }
        // 将最新的关键帧作为reference frame，
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
        // If we have an initial estimation of the camera pose and matching. Track the local map.
        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();
        }
        if(bOK)
            mState = OK;
        else
            mState=LOST;
        // Update drawer
        mpFrameDrawer->Update(this);
        // If tracking were good, check if we insert a keyframe
        if(bOK)
        {
     
            // Update motion model
            if(!mLastFrame.mTcw.empty())
            {
     
                // 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
                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; // 其实就是Tcl
            }
            else
                mVelocity = cv::Mat();

            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

            // Clean VO matches
            // 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
            for(int i=0; i<mCurrentFrame.N; i++) // 遍历当前帧所有MapPoint
            {
     
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(pMP)
                    // 排除UpdateLastFrame函数中为了跟踪增加的MapPoints
                    if(pMP->Observations()<1)
                    {
     
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                    }
            }

            // Delete temporal MapPoints
            // 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
            // 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
            // 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
            for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
     
                MapPoint* pMP = *lit;
                delete pMP;
            }
            // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
            mlpTemporalPoints.clear();

            // Check if we need to insert a new keyframe
            // 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
            if(NeedNewKeyFrame())
                CreateNewKeyFrame();
            // We allow points with high innovation (considererd outliers by the Huber Function)
            // pass to the new keyframe, so that bundle adjustment will finally decide
            // if they are outliers or not. We don't want next frame to estimate its position
            // with those points so we discard them in the frame.
            // 删除那些在bundle adjustment中检测为outlier的3D map点
            for(int i=0; i<mCurrentFrame.N;i++)
            {
     
                if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        // 跟踪失败，并且relocation也没有搞定，只能重新Reset
        if(mState==LOST)
        {
     
            if(mpMap->KeyFramesInMap()<=5)
            {
     
                cout << "Track lost soon after initialisation, reseting..." << endl;
                mpSystem->Reset();
                return;
            }
        }

        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;
        // 保存上一帧的数据
        mLastFrame = Frame(mCurrentFrame);
    }

    // 步骤3.记录位姿信息，用于轨迹复现-------------------------------------------------------------
    // Store frame pose information to retrieve the complete camera trajectory afterwards.
    if(!mCurrentFrame.mTcw.empty())
    {
     
        // 计算相对姿态T_currentFrame_referenceKeyFrame
        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
        mlRelativeFramePoses.push_back(Tcr);
        mlpReferences.push_back(mpReferenceKF);
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
        mlbLost.push_back(mState==LOST);
    }
    else
    {
     
        // This can happen if tracking is lost
        // 如果跟踪失败，则相对位姿使用上一次值
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }
}

总结一下上述函数中实现的操作与功能，主要为：

1. 判断追踪tracking状态：如果是未初始化（NOT_INITIALIZED），则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()函数进行初始化，并更新地图视图。
2. 对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking进行判断）：
   a. 只进行跟踪不建图；
   b. 同时跟踪和建图。
3. 初始化之后ORB-SLAM有三种跟踪模型可供选择：
   a. TrackWithMotionModel() 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。
   b. TrackReferenceKeyFrame() 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。
   c. Relocalization() 重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。
4. 这三个模型的选择方法：
   首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪彻底丢失，要么等待相机回转，要不进行重置。
5. 跟踪的具体步骤为：
   2.1. 跟踪上一帧或者参考帧或者重定位；
   2.2. 在帧间匹配得到初始的姿态后，现在对当前帧进行跟踪得到更多的匹配，并优化当前位姿、当前帧local map、当前帧的MapPoints、当前关键帧与其它关键帧共视关系；
   2.3. 更新恒速运动模型TrackWithMotionModel()中的mVelocity；
   2.4. 清除UpdateLastFrame()函数中为当前帧临时添加的MapPoints；
   2.5. 清除临时的MapPoints，这些MapPoints在TrackWithMotionModel()的UpdateLastFrame()函数里生成（仅双目和rgbd）；
   2.6. 检测并插入关键帧，对于双目会产生新的MapPoints。

下面对Track()函数中引用的各个子函数逐一进行详细解析，其中一些短小的函数这里就不再赘述了。

3.3、单目初始化 Tracking:: MonocularInitialization()

在上述Track()函数中，一开始对单目和非单目分别执行MonocularInitialization()、StereoInitialization()函数进行了初始化，这里依旧以单目初始化MonocularInitialization()函数为例进行详细阅读，代码如下：

void Tracking::MonocularInitialization()//单目初始化
{
     
    // 如果单目初始器还没有被创建，则创建单目初始器
    if(!mpInitializer)
    {
     
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {
     
            // 步骤1：得到用于初始化的第一帧，初始化需要两帧
            mInitialFrame = Frame(mCurrentFrame);
            // 记录最后的一帧
            mLastFrame = Frame(mCurrentFrame);
            // mvbPrevMatched最大的情况就是所有特征点都被跟踪上
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
                mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
            // 这两句是多余的，因为先前有if（!mpInitializer）
            if(mpInitializer)
               delete mpInitializer;
            // 由当前帧构造初始器 sigma:1.0 iterations:200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
    }
    else // 如果是第二次进入，已经创建了初始器
    {
     
        // Try to initialize
        // 步骤2：如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
        // 如果当前帧特征点太少，重新构造初始器
        // 因此只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
     
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
        // Find correspondences
        // 步骤3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
        // mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
        // mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点
        ORBmatcher matcher(0.9,true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
        // Check if there are enough correspondences
        // 步骤4：如果初始化的两帧之间的匹配点太少，重新初始
        if(nmatches<100)
        {
     
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            return;
        }
        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
 
        // 步骤5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
        if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
     
           // 步骤6：删除那些无法进行三角化的匹配点
            for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
            {
     
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
     
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }
            // Set Frame Poses
            // 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            // 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到该帧的变换矩阵
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);
 
            // 步骤6：将三角化得到的3D点包装成MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();
        } 
    }
}

MonocularInitialization()函数的功能为并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云，得到初始两帧的匹配、相对运动、初始MapPoints。

该函数的具体操作步骤总结如下：
（1）当第一次进入该方法的时候，没有先前的帧数据，将当前帧保存为初始帧和最后一帧，并初始化一个初始化器；
（2）第二次进入该方法的时候，已经有初始化器了；
（3）利用ORB匹配器，对当前帧和初始帧进行匹配，对应关系小于100个时失败；
（4）利用八点法的对极约束，启动两个线程分别计算单应矩阵和基础矩阵，并通过score判断用单应矩阵回复运动轨迹还是使用基础矩阵回复运动轨迹；
（5）将初始帧和当前帧创建为关键帧，并创建地图点MapPoint；
（6）通过全局BundleAdjustment优化相机位姿和关键点坐标；
（7）设置单位深度并缩放初试基线和地图点；
（8）其他变量的初始化。

3.4、三角化生成MapPoints Tracking:: CreateInitialMapMonocular()

在MonocularInitialization()函数的最后调用了CreateInitialMapMonocular()函数，于是这里对这个函数进行详细阅读，函数功能为单目摄像头三角化生成MapPoints，具体代码如下：

void Tracking::CreateInitialMapMonocular()
{
     
    // 创建两帧，一个为初始帧，一个为当前帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
    // 步骤1：将初始关键帧的描述子转为BoW
    pKFini->ComputeBoW();
    // 步骤2：将当前关键帧的描述子转为BoW
    pKFcur->ComputeBoW();
    // Insert KFs in the map
    // 步骤3：将关键帧插入到地图
    // 凡是关键帧，都要插入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);
    // Create MapPoints and asscoiate to keyframes
    // 步骤4：将3D点包装成MapPoints
   for(size_t i=0; i<mvIniMatches.size();i++)//遍历所有匹配
    {
     
       if(mvIniMatches[i]<0)
            continue;
        //Create MapPoint.
        cv::Mat worldPos(mvIniP3D[i]);
        // 步骤4.1：用3D点构造MapPoint
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
        // 步骤4.2：为该MapPoint添加属性：
        // a.观测到该MapPoint的关键帧
        // b.该MapPoint的描述子
        // c.该MapPoint的平均观测方向和深度范围
        // 步骤4.3：表示该KeyFrame的哪个特征点可以观测到哪个3D点
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
        // a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);
        // b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
        pMP->ComputeDistinctiveDescriptors();
        // c.更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();
        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
        //Add to Map
        // 步骤4.4：在地图中添加该MapPoint
        mpMap->AddMapPoint(pMP);
    }
    // Update Connections
    // 步骤5：更新关键帧间的连接关系
    // 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();
    // Bundle Adjustment 
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl；
    // 步骤5：BA优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);
    // Set median depth to 1
    // 步骤6：!!!将MapPoints的中值深度归一化到1，并归一化两帧之间变换
    // 评估关键帧场景深度，q=2表示中值
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;
 
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
     
       cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    } 
    // Scale initial baseline
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w); 
    // Scale point
    // 把3D点的尺度也归一化到
    vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
    {
     
        if(vpAllMapPoints[iMP])
        {
     
            MapPoint* pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }
 
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur); 
    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur; 
    mvpLocalKeyFrames.push_back(pKFcur)
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints=mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;
    mLastFrame = Frame(mCurrentFrame);
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints)
    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
    mpMap->mvpKeyFrameOrigins.push_back(pKFini);
    mState=OK;// 初始化成功，至此，初始化过程完成 
}

3.5、重定位 Tracking:: Relocalization()

Relocalization()重定位函数的功能为从之前的关键帧中找出与当前帧之间拥有充足匹配点的候选帧，利用Ransac迭代，通过PnP求解位姿。具体代码解析如下：

bool Tracking::Relocalization()
{
     
    // Compute Bag of Words Vector
    // 步骤1：计算当前帧特征点的Bow映射
    mCurrentFrame.ComputeBoW();
    // Relocalization is performed when tracking is lost当跟踪丢失执行重定位
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // 步骤2：找到与当前帧相似的候选关键帧
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
    if(vpCandidateKFs.empty())//如果没找到候选关键帧，返回
        return false;
    const int nKFs = vpCandidateKFs.size();//候选关键帧个数
    // We perform first an ORB matching with each candidat
    // If enough matches are found we setup a PnP solver
    //我们首先执行与每个候选匹配的ORB匹配
    //如果找到足够的匹配，我们设置一个PNP解算器
    ORBmatcher matcher(0.75,true);
    vector<PnPsolver*> vpPnPsolvers;
    vpPnPsolvers.resize(nKFs);
    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nKFs);
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nKFs);
    int nCandidates=0;
    for(int i=0; i<nKFs; i++)
    {
     
        KeyFrame* pKF = vpCandidateKFs[i];
        if(pKF->isBad())
            vbDiscarded[i] = true;//去除不好的候选关键帧
        else
        {
     
            // 步骤3：通过BoW进行匹配
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            if(nmatches<15)//如果匹配点小于15剔除
            {
     
                vbDiscarded[i] = true;
                continue;
            }
            else//用pnp求解
            {
     
                // 初始化PnPsolver
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }
    // Alternatively perform some iterations of P4P RANSAC可选地执行P4P RANSAC的一些迭代
    // Until we found a camera pose supported by enough inliers直到早到符合很多内点的相机位置
    bool bMatch = false;
    ORBmatcher matcher2(0.9,true);
    while(nCandidates>0 && !bMatch)
    {
     
        for(int i=0; i<nKFs; i++)
        {
     
            if(vbDiscarded[i])
                continue;
            // Perform 5 Ransac Iterations
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
            // 步骤4：通过EPnP算法估计姿态
            PnPsolver* pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
            // If Ransac reachs max. iterations discard keyframe
            if(bNoMore)
            {
     
                vbDiscarded[i]=true;
                nCandidates--;
            }
            // If a Camera Pose is computed, optimize
            if(!Tcw.empty())
            {
     
                Tcw.copyTo(mCurrentFrame.mTcw);
                set<MapPoint*> sFound;
                const int np = vbInliers.size();//内点个数 
                for(int j=0; j<np; j++)
                {
     
                    if(vbInliers[j])
                    {
     
                        mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
                        sFound.insert(vvpMapPointMatches[i][j]);
                    }
                    else
                        mCurrentFrame.mvpMapPoints[j]=NULL;
                }
 
                // 步骤5：通过PoseOptimization对姿态进行优化求解 
                int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                if(nGood<10)
                    continue;
                for(int io =0; io<mCurrentFrame.N; io++)
                    if(mCurrentFrame.mvbOutlier[io])
                        mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
 
                // If few inliers, search by projection in a coarse window and optimize again
                // 步骤6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
                if(nGood<50)
                {
     
                   int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
                    if(nadditional+nGood>=50)
                    {
     
                        nGood = Optimizer::PoseOptimization(&mCurrentFrame);//优化 
                        // If many inliers but still not enough, search by projection again in a narrower window
//如果许多内点仍然不够，则在较窄的窗口中再次用投影搜索
                        // the camera has been already optimized with many points
                        if(nGood>30 && nGood<50)
                        {
     
                            sFound.clear();
                            for(int ip =0; ip<mCurrentFrame.N; ip++)
                                if(mCurrentFrame.mvpMapPoints[ip])
                                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                            nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
                            // Final optimization
                            if(nGood+nadditional>=50)
                            {
     
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                                for(int io =0; io<mCurrentFrame.N; io++)
                                    if(mCurrentFrame.mvbOutlier[io])
                                       mCurrentFrame.mvpMapPoints[io]=NULL;
                            }
                        }
                    }
                } 
                // If the pose is supported by enough inliers stop ransacs and continue
                if(nGood>=50)
                {
     
                    bMatch = true;
                    break;
                }
            }
        }
    } 
    if(!bMatch)
    {
     
        return false;
    }
    else
    {
     
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }
}

总结一下该函数的具体操作有如下步骤：
（1）先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧；
（2）构建PnP求解器，标记杂点，准备好每个关键帧和当前帧的匹配点集；
（3）用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置；
（4）返回成功或失败。

3.6、特征匹配与姿态估计 Tracking:: TrackReferenceKeyFrame()

TrackReferenceKeyFrame()函数的主要功能为计算当前帧的词包，将当前帧的特征点分到特定层的nodes上，之后对属于同一node的描述子进行匹配，并根据匹配对估计当前帧的姿态，最后根据姿态剔除误匹配。具体代码解析如下：

bool Tracking::TrackReferenceKeyFrame()
{
     
    // Compute Bag of Words vector
    // 步骤1：将当前帧的描述子转化为BoW向量
    mCurrentFrame.ComputeBoW();
    // We perform first an ORB matching with the reference keyframe
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.7,true);
    vector<MapPoint*> vpMapPointMatches;
 
    // 步骤2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
    // 特征点的匹配关系由MapPoints进行维护
    int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
    if(nmatches<15)//匹配少于15个
        return false;
 
    // 步骤3:将上一帧的位姿态作为当前帧位姿的初始值
    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
    // 步骤4:通过优化3D-2D的重投影误差来获得位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
    // Discard outliers
    // 步骤5：剔除优化后的outlier匹配点（MapPoints）
    int nmatchesMap = 0;
    for(int i =0; i<mCurrentFrame.N; i++)
    {
     
        if(mCurrentFrame.mvpMapPoints[i])
        {
     
            if(mCurrentFrame.mvbOutlier[i])
            {
     
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--; 
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                nmatchesMap++;
        }
    }
    return nmatchesMap>=10;
}

总结一下该函数的具体操作有如下步骤：
（1）按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近，首先求解当前帧的BOW向量；
（2）搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对跟踪失败；
（3）将当前帧的位置假定到上一帧位置；
（4）通过最小二乘法优化相机的位姿；
（5）抛弃无用的杂点，当match数大于等于10的时候，返回true成功。

3.7、恒速运动模型 Tracking::TrackWithMotionModel()

TrackWithMotionModel()函数的具体代码解析如下：

bool Tracking::TrackWithMotionModel()
{
     
    ORBmatcher matcher(0.9,true);
    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points
    // 步骤1：对于双目或rgbd摄像头，根据深度值为上一关键帧生成新的MapPoints
    // （跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围）
    // 在跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少
    // 这个函数的功能就是补充增加RGBD和双目相机上一帧的MapPoints数
    UpdateLastFrame();
    // 根据Const Velocity Model(认为这两帧之间的相对运动和之前两帧间相对运动相同)估计当前帧的位姿
    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);//当前帧位置等于mVelocity*mLastFrame.mTcw
 
    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
    // Project points seen in previous frame
    int th;
    if(mSensor!=System::STEREO)//非双目搜索范围系数设为15
        th=15;
    else
        th=7;
 
    // 步骤2：根据匀速度模型进行对上一帧的MapPoints进行跟踪
    // 根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
    // If few matches, uses a wider window search
    // 如果跟踪的点少，则扩大搜索半径再来一次
    if(nmatches<20)
    {
     
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
    }
 
    if(nmatches<20)//如果匹配点少于20，返回
        return false;
    // Optimize frame pose with all matches
    // 步骤3：优化位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
    // Discard outliers
    // 步骤4：优化位姿后剔除outlier的mvpMapPoints
    int nmatchesMap = 0;
    for(int i =0; i<mCurrentFrame.N; i++)
    {
     
        if(mCurrentFrame.mvpMapPoints[i])
        {
     
            if(mCurrentFrame.mvbOutlier[i])
            {
     
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                nmatchesMap++;
        }
    }    
 
    if(mbOnlyTracking)//如果仅跟踪
    {
     
        mbVO = nmatchesMap<10;
        return nmatches>20;
    }
    return nmatchesMap>=10;
}

总结一下该函数的具体操作有如下步骤：
（1）先通过上一帧的位姿和速度预测当前帧相机的位姿；
（2） 通过PnP方法估计相机位姿，在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围；
（3）然后进行一次BA算法，通过最小二乘法优化相机的位姿；
（4）优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用。

3.8、更新最近帧位姿 Tracking::UpdateLastFrame()

UpdateLastFrame()函数的具体代码解析如下：

void Tracking::UpdateLastFrame()
{
     
    // Update pose according to reference keyframe
    // 步骤1：更新最近一帧的位姿
    KeyFrame* pRef = mLastFrame.mpReferenceKF;
    cv::Mat Tlr = mlRelativeFramePoses.back(); 
    mLastFrame.SetPose(Tlr*pRef->GetPose()); // Tlr*Trw = Tlw 1:last r:reference w:world
 
    // 如果上一帧为关键帧，或者单目的情况，则退出
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;
 
    // 步骤2：对于双目或rgbd摄像头，为上一帧临时生成新的MapPoints
    // 注意这些MapPoints不加入到Map中，在tracking的最后会删除
    // 跟踪过程中需要将将上一帧的MapPoints投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配
    // Create "visual odometry" MapPoints
    // We sort points according to their measured depth by the stereo/RGB-D sensor
    // 步骤2.1：得到上一帧有深度值的特征点
 
    vector<pair<float,int> > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);
    for(int i=0; i<mLastFrame.N;i++)
    {
     
        float z = mLastFrame.mvDepth[i];
        if(z>0)//如果深度大于0
        {
     
            vDepthIdx.push_back(make_pair(z,i));
        }
    } 
    if(vDepthIdx.empty())//如果没深度值则退出 
        return;
    // 步骤2.2：按照深度从小到大排序
    sort(vDepthIdx.begin(),vDepthIdx.end());
 
    // We insert all close points (depth
    // If less than 100 close points, we insert the 100 closest ones.
    // 步骤2.3：将距离比较近的点包装成MapPoints 
    int nPoints = 0;
    for(size_t j=0; j<vDepthIdx.size();j++)
    {
     
        int i = vDepthIdx[j].second;
        bool bCreateNew = false;
        MapPoint* pMP = mLastFrame.mvpMapPoints[i];
        if(!pMP)
            bCreateNew = true;
        else if(pMP->Observations()<1)
        {
     
            bCreateNew = true;
        } 
        if(bCreateNew)
        {
     
            // 这些生成MapPoints后并没有通过：
            // a.AddMapPoint、
            // b.AddObservation、
            // c.ComputeDistinctiveDescriptors、
            // d.UpdateNormalAndDepth添加属性，
            // 这些MapPoint仅仅为了提高双目和RGBD的跟踪成功率
            cv::Mat x3D = mLastFrame.UnprojectStereo(i);
            MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
            mLastFrame.mvpMapPoints[i]=pNewMP; // 添加新的MapPoint 
            // 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
            mlpTemporalPoints.push_back(pNewMP);
            nPoints++;
        }
        else
        {
     
            nPoints++;
        }
        if(vDepthIdx[j].first>mThDepth && nPoints>100)
            break;
    }
}

总结一下该函数的具体操作有如下步骤：
（1）更新最近一帧的位姿；
（2）对于双目或rgbd摄像头，为上一帧临时生成新的MapPoints，这些MapPoints不加入到Map中，在tracking的最后会删除，跟踪过程中需要将将上一帧的MapPoints投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配。

3.9、更新局部地图点 Tracking::TrackLocalMap()

TrackLocalMap()函数的具体代码解析如下：

bool Tracking::TrackLocalMap()
{
     
    // We have an estimation of the camera pose and some map points tracked in the frame.
    // We retrieve the local map and try to find matches to points in the local map.
    // Update Local KeyFrames and Local Points
    // 步骤1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
 
    UpdateLocalMap();
    // 步骤2：在局部地图中查找与当前帧匹配的MapPoints
    SearchLocalPoints();
    // Optimize Pos
    // 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，
    // 步骤3：更新局部所有MapPoints后对位姿再次优化
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = 0;
 
    // Update MapPoints Statistics
    // 步骤3：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
    for(int i=0; i<mCurrentFrame.N; i++)
    {
     
        if(mCurrentFrame.mvpMapPoints[i])
        {
     
            // 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
            if(!mCurrentFrame.mvbOutlier[i])
            {
     
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                if(!mbOnlyTracking)
                {
     
                    // 该MapPoint被其它关键帧观测到过
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                        mnMatchesInliers++;
                }
                else
                    // 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
                    mnMatchesInliers++;
            }
            else if(mSensor==System::STEREO)
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
        }
    }
 
    // Decide if the tracking was succesful
    //More restrictive if there was a relocalization recently
    // 步骤4：决定是否跟踪成功
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50
        return false; 
    if(mnMatchesInliers<30)
        return false;
    else
        return true;
}

总结一下该函数的具体操作有如下步骤：
（１）更新Covisibility Graph， 更新局部关键帧；
（２）根据局部关键帧，更新局部地图点，接下来运行过滤函数 isInFrustum ；
（３）将地图点投影到当前帧上，超出图像范围的舍弃；
（４）当前视线方向v和地图点云平均视线方向n, 舍弃n*v （５）舍弃地图点到相机中心距离不在一定阈值内的点；
（６）计算图像的尺度因子 isInFrustum 函数结束；
（７）进行非线性最小二乘优化；
（８）更新地图点的统计量。

3.10、判断是否需要生成新的关键帧 Tracking::NeedNewKeyFrame()

NeedNewKeyFrame()函数的功能为判断是否需要生成新的关键帧，确定关键帧的标准。其具体代码解析如下：

bool Tracking::NeedNewKeyFrame()
{
     
    // 步骤1：如果用户在界面上选择重定位，那么将不插入关键帧
    // 由于插入关键帧过程中会生成MapPoint，因此用户选择重定位后地图上的点云和关键帧都不会再增加
    if(mbOnlyTracking)//如果仅跟踪，不选关键帧
        return false;
    //If Local Mapping is freezed by a Loop Closure do not insert keyframes
    // 如果局部地图被闭环检测使用，则不插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;
    const int nKFs = mpMap->KeyFramesInMap();//关键帧数
 
    // Do not insert keyframes if not enough frames have passed from last relocalisation
    // 步骤2：判断是否距离上一次插入关键帧的时间太短
    // mCurrentFrame.mnId是当前帧的ID
    // mnLastRelocFrameId是最近一次重定位帧的ID
    // mMaxFrames等于图像输入的帧率
    // 如果关键帧比较少，则考虑插入关键帧
    // 或距离上一次重定位超过1s，则考虑插入关键帧
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        return false;
 
    // Tracked MapPoints in the reference keyframe
    // 步骤3：得到参考关键帧跟踪到的MapPoints数量
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
 
    int nMinObs = 3;
    if(nKFs<=2)
        nMinObs=2;
    int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);//获取参考关键帧跟踪到的MapPoints数量

    // Local Mapping accept keyframes?
    // 步骤4：查询局部地图管理器是否繁忙
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
 
    // Stereo & RGB-D: Ratio of close "matches to map"/"total matches"
    //双目和RGBD：比率接近地图匹配数/总匹配数
    // "total matches = matches to map + visual odometry matches"
    //总匹配数=地图匹配数+视觉里程计匹配数
    // Visual odometry matches will become MapPoints if we insert a keyframe.
    // This ratio measures how many MapPoints we could create if we insert a keyframe.
    //这个比率测量如果我们插入一个关键帧，我们可以创建多少个MapPoints
    // 步骤5：对于双目或RGBD摄像头，统计总的可以添加的MapPoints数量和跟踪到地图中的MapPoints数量
 
    int nMap = 0;//地图匹配数
    int nTotal= 0;//总匹配数
    if(mSensor!=System::MONOCULAR)// 双目或rgbd
    {
     
       for(int i =0; i<mCurrentFrame.N; i++)//遍历当前帧所有匹配点
        {
     
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)//map点的速度在合理范围内
            {
     
                nTotal++;// 总的可以添加mappoints数
                if(mCurrentFrame.mvpMapPoints[i])
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//mappoint能被观测
                        nMap++;// 被关键帧观测到的mappoints数，即观测到地图中的MapPoints数量
            }
        }
    }
    else
    {
     
        // There are no visual odometry matches in the monocular case
        nMap=1;
        nTotal=1;
    }
    const float ratioMap = (float)nMap/(float)(std::max(1,nTotal));
    // 步骤6：决策是否需要插入关键帧
    // Thresholds
    // 设定inlier阈值，和之前帧特征点匹配的inlier比例
    float thRefRatio = 0.75f;
    if(nKFs<2)
        thRefRatio = 0.4f;// 关键帧只有一帧，那么插入关键帧的阈值设置很低
    if(mSensor==System::MONOCULAR)
        thRefRatio = 0.9f;
 
    // MapPoints中和地图关联的比例阈值
    float thMapRatio = 0.35f;
    if(mnMatchesInliers>300)
        thMapRatio = 0.20f;
 
    // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
    // 很长时间没有插入关键帧
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
    // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
    // localMapper处于空闲状态
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
 
    // Condition 1c: tracking is weak
    // 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
    const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f) ;
    // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
    // 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15); 
    if((c1a||c1b||c1c)&&c2)
    {
     
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA
        //如果mapping接受关键帧，则插入关键帧，否则发送信号到中断BA
        if(bLocalMappingIdle)
        {
     
           return true;
        } 
        else
        {
     
            mpLocalMapper->InterruptBA();//中断BA
            if(mSensor!=System::MONOCULAR)
            {
     
                // 队列里不能阻塞太多关键帧
                // tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
                // 然后localmapper再逐个pop出来插入到mspKeyFrames
                if(mpLocalMapper->KeyframesInQueue()<3)//队列中关键帧小于3
                    return true;
                else
                    return false;
            }
            else
                return false;
        }
    }
    else
        return false;
}

总结一下该函数的具体操作有如下步骤：
（１）在上一次进行重定位之后，过了20帧数据，或关键帧数小于20个，不满足不能生成；
（２）在上一个关键帧插入之后，过了20帧，或局部建图是空闲状态，不满足不能生成；
（３）前帧跟踪到大于若干个点，不满足不能生成；
（４）前帧的跟踪点数小于90%的参考关键帧跟踪点数，并且当前帧跟踪点数大于15，不满足不能生成。

3.11、生成新的关键帧 Tracking::CreateNewKeyFrame()

CreateNewKeyFrame() 函数的具体代码解析如下：

void Tracking::CreateNewKeyFrame()
{
     
    if(!mpLocalMapper->SetNotStop(true))
        return;
    // 步骤1：将当前帧构造成关键帧
    KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
    // 步骤2：将当前关键帧设置为当前帧的参考关键帧
    // 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
    mpReferenceKF = pKF;
    mCurrentFrame.mpReferenceKF = pKF;
    // 这段代码和UpdateLastFrame中的那一部分代码功能相同
    // 步骤3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
    if(mSensor!=System::MONOCULAR)
    {
     
        // 根据Tcw计算mRcw、mtcw和mRwc、mOw
        mCurrentFrame.UpdatePoseMatrices();
        // We sort points by the measured depth by the stereo/RGBD sensor.
        // We create all those MapPoints whose depth < mThDepth.
        // If there are less than 100 close points we create the 100 closest.
        // 步骤3.1：得到当前帧深度小于阈值的特征点
        // 创建新的MapPoint, depth < mThDepth
        vector<pair<float,int> > vDepthIdx;
        vDepthIdx.reserve(mCurrentFrame.N);
        for(int i=0; i<mCurrentFrame.N; i++)
        {
     
           float z = mCurrentFrame.mvDepth[i];
            if(z>0)
            {
     
                vDepthIdx.push_back(make_pair(z,i));
            }
        }
 
        if(!vDepthIdx.empty())
        {
     
            // 步骤3.2：按照深度从小到大排序
            sort(vDepthIdx.begin(),vDepthIdx.end());
            // 步骤3.3：将距离比较近的点包装成MapPoints
            int nPoints = 0;
            for(size_t j=0; j<vDepthIdx.size();j++)
            {
     
               int i = vDepthIdx[j].second;
                bool bCreateNew = false;
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(!pMP)
                   bCreateNew = true;
                else if(pMP->Observations()<1)
                {
     
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
                }
                if(bCreateNew)
                {
     
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                    MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
                    // 这些添加属性的操作是每次创建MapPoint后都要做的
                    pNewMP->AddObservation(pKF,i);
                    pKF->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);
                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                    nPoints++;
                }
                else
                {
     
                    nPoints++;
                }
                // 这里决定了双目和rgbd摄像头时地图点云的稠密程度
                // 但是仅仅为了让地图稠密直接改这些不太好，
                // 因为这些MapPoints会参与之后整个slam过程
                if(vDepthIdx[j].first>mThDepth && nPoints>100)
                    break;
            }
        }
    }
    mpLocalMapper->InsertKeyFrame(pKF);
    mpLocalMapper->SetNotStop(false);
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;
}

总结一下该函数的具体操作有如下步骤：
（1）将当前帧构造成关键帧；
（2）将当前关键帧设置为当前帧的参考关键帧；
（3）对于双目或rgbd摄像头，为当前帧生成新的MapPoints。

System.cc与Tracking.cc程序暂时看到这里，后续业务如果有更深入的需求再进行深造。

参考文献与资料 ---------------------------------------------------------------------------------------
1、 https://blog.csdn.net/qq_20123207/article/details/82587130
2、https://www.cnblogs.com/zhengmeisong/p/7811524.html
3、https://blog.csdn.net/u014709760/article/details/87922386?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.channel_param
4、https://blog.csdn.net/lwx309025167/article/details/80421077
5、https://blog.csdn.net/huangjingwei13/article/details/78673154
6、https://blog.csdn.net/qq_20123207/article/details/82728412

写在最后 ------------------------------------------------------------------------------------------------
自从听过“看别人的代码就是从别人的排泄物中推测对方早餐吃的是什么，这份工作既艰难又恶心 ”这样的一句话后，手里别人的代码突然就不香了… 但还是要看，毕竟自己不会写…

沉迷工作的乔木小姐
2020.11.18