ORB-SLAM 代码笔记（四）tracking代码结构

首先要清楚ORB-SLAM视觉跟踪的原理，然后对tracking.cc中的函数逐个讲解

代码的前面部分是从配置文件中读取校准好的相机参数（内参和畸变参数，以及双目的深度测量设定），并且加载ORB特征点提取的参数（特征点数，金字塔层数，变化尺度，以及提取Fast关键点的阈值）；以及四个线程之间锁的代码。

接下来是将从摄像头或者数据集读入的图像封装成Frame类型对象：

这里以单目为例，无论图片是RGB，BGR， 还是RGBA，BGRA，均转化为灰度图，放弃彩色信息。然后将当前读入帧封装为Frame类型的mCurrentFrame对象；为了让单目成功初始化（单目的初始化需要通过平移运动归一化尺度因子），初始化时mpIniORBextractor提取的特征点数量设定为普通帧的2倍。

mpIniORBextractor = new ORBextractor(*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

有了mCurrentFrame对象，就可以进入跟踪模块（tracking()函数）， 经过一系列跟踪处理（重点），可以返回当前帧的位姿。

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
{
    mImGray = im;

    if(mImGray.channels()==)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    Track();

    return mCurrentFrame.mTcw.clone();
}

具体的tracking步骤如下：

（1）初始化

首先是初始化，分为双目、RGBD，以及单目两种不同的初始化过程，单目的初始化需要注意的较多，可以先看void Tracking::StereoInitialization()函数：

如果检测到的特征点数 N>500，则将当前帧构建为第一个关键帧，关键帧通过当前帧，地图点，以及BoW构建；同时，mpMap地图中也需要加入该关键帧（mpMap包含图像帧以及相应的地图点）， 对每一个特征点，通过反投影得出3D地图点，地图点中通过3D位置信息，对应的关键帧，以及mpMap构建。

这其中的关系有点让人混乱，其实直观地去理解很容易：

mpMap就是我们整个位姿与地图（可以想象成ORB-SLAM运行时的那个界面世界），MapPoint和KeyFrame都被包含在这个mpMap中。因此创建这三者对象（地图，地图点，关键帧）时，三者之间的关系在构造函数中不能缺少。另外，由于一个关键帧提取出的特征点对应一个地图点集，因此需要记下每个地图点的在该帧中的编号；同理，一个地图点会被多帧关键帧观测到，也需要几下每个关键帧在该点中的编号。

地图点，还需要完成两个运算，第一个是在观测到该地图点的多个特征点中（对应多个关键帧），挑选出区分度最高的描述子，作为这个地图点的描述子；

pNewMP->ComputeDistinctiveDescriptors();

第二个是更新该地图点平均观测方向与观测距离的范围，这些都是为了后面做描述子融合做准备。

pNewMP->UpdateNormalAndDepth();

最后，还需要在局部地图中（为了实现局部Bundle Adjustment）插入当前关键帧，还需要初始化局部关键帧和局部地图点。将上一帧，上一关键帧，以及上一关键帧Id号初始化为当前帧。参考关键帧，当前帧的参考关键帧，当前地图中的参考地图点（集），并初始化当前相机姿态。

void Tracking::StereoInitialization()
{
    if(mCurrentFrame.N>)
    {
        // Set Frame pose to the origin
        mCurrentFrame.SetPose(cv::Mat::eye(,,CV_32F));

        // Create KeyFrame
        KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

        // Insert KeyFrame in the map
        mpMap->AddKeyFrame(pKFini);

        // Create MapPoints and asscoiate to KeyFrame
        for(int i=; i<mCurrentFrame.N;i++)
        {
            float z = mCurrentFrame.mvDepth[i];
            if(z>)
            {
                cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
                pNewMP->AddObservation(pKFini,i);
                pKFini->AddMapPoint(pNewMP,i);
                pNewMP->ComputeDistinctiveDescriptors();
                pNewMP->UpdateNormalAndDepth();
                mpMap->AddMapPoint(pNewMP);

                mCurrentFrame.mvpMapPoints[i]=pNewMP;
            }
        }

        cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;

        mpLocalMapper->InsertKeyFrame(pKFini);

        mLastFrame = Frame(mCurrentFrame);
        mnLastKeyFrameId=mCurrentFrame.mnId;
        mpLastKeyFrame = pKFini;

        mvpLocalKeyFrames.push_back(pKFini);
        mvpLocalMapPoints=mpMap->GetAllMapPoints();
        mpReferenceKF = pKFini;
        mCurrentFrame.mpReferenceKF = pKFini;

        mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

        mpMap->mvpKeyFrameOrigins.push_back(pKFini);

        mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

        mState=OK;
    }
}

单目的初始化则相对负责，放在下一篇笔记中去讲。