ORB SLAM2学习笔记之mono_kitti（四）

探索Track函数之MonocularInitialization（单目初始化）

Tracking.cpp里面的Track()函数可以说是 Tracking 线程最主体部分了，来看看调用Track()函数的过程：

Tracking线程中初始化函数用的是MonocularInitialization()，在第一次进入这个函数之前，初始化了mpInitializer（单目初始器）赋值为null，所以一开始会进入下面的if语句：

void Tracking::MonocularInitialization()
{

    //(第一次进入函数时)初始化tracking时赋了null
    //(第二次进入函数时)第一次新建了mpInitializer，所以执行else
    if(!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {

            //初始化mono的时候，初始帧和上一帧都是当前帧赋值
            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)
                delete mpInitializer;

            // 这里实例化Initialize类的一个对象指针，初始化的值为选取的当前帧和测量误差1.0，RANSAC迭代次数200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);  //先都填上-1 现在还没匹配呢

            return;
        }
    }

第二次进入这个函数的时候已经创建了一个mpInitializer，所以会进入else语句：

    else
    {
        // Try to initialize
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        ORBmatcher matcher(0.9,true);

        // mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
        // mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
        // mvbPrevMatched，mvIniMatches获得更新
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

        // Check if there are enough correspondences
        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)

        //如果初始化成功，intializer得到Rcw, tcw
        if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
            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));
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);

            //Tcw赋值给mCurrentFrame
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            // 将三角化得到的3D点包装成MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();
        }
    }
}

第一次进函数

先看if里面的，利用当前帧 mCurrentFrame创建了两个Frame对象，分别是 mInitialFrame（初始帧）和 mLastFrame（上一帧），课可见一开始的时候，上一帧就是当前帧。

mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);

mvIniMatches用于存储 mInitialFrame, mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引，所以初始化 mpInitializer后还要给 mvIniMatches填上 -1，以记录现在还未初始化。

第二次进函数

第二次进函数就已经有了两帧图像了，分别是 mInitialFrame（初始帧第一帧）， mCurrentFrame（当前帧第二帧），有些细节比如判特征点数量的之类的就不说了，接下来初始化一个matcher，用于匹配，参数分别为mfNNratio（匹配分数设置）和mbCheckOrientation（要不要检查方向）。接下来开始匹配，函数最后得到的nmatches记录了匹配数：

int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

参数：mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配。mvIniMatches存储 mInitialFrame, mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引。
好，接下来mpInitializer要开始初始化了：

mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated

mpintializer初始化后，如果成功，会得到Rcw（旋转阵）, tcw（平移向量）, mvIniP3D(地图点的三维信息)，成功后会进入一个if语句中，这个先不谈，先看这个Initialize函数：

Initialize函数

Initialize函数属于Initializer类，这是一个跟初始化相关的类，函数大致是讲计算 基础矩阵(F) 和 单应性矩阵(H)，然后选取其中一个模型，恢复出最开始两帧之间的相对姿态以及3d点。主要的步骤是下面这些：

① 组织特征点对：

mvMatches12.reserve(mvKeys2.size()); //mvKeys2：当前帧特征点
mvbMatched1.resize(mvKeys1.size()); //mvKeys1：参考帧特征点

for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    {
        if(vMatches12[i]>=0)
        {
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
            mvbMatched1[i]=true;
        }
        else
            mvbMatched1[i]=false;
    }

    // 匹配上的特征点的个数
    const int N = mvMatches12.size();

    // Indices for minimum set selection
    // 新建一个容器vAllIndices，生成0到N-1的数作为特征点的索引
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

vMatches12就是前面说的 mvIniMatches，而 mvMatches12则是装了一个个 初始、当前帧的match 的 vector。vAllIndices 是装了所有match的索引（一个索引对应一个pair）

② 在所有匹配特征点对中随机选择8对匹配特征点：

    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    DUtils::Random::SeedRandOnce(0);

    for(int it=0; it<mMaxIterations; it++)
    {
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;

            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

迭代赋值，采取RANSAC的方法每组随机取八个点，取mMaxIterations组。mvSets是一个装有mMaxIterations这么多个vector的vector，它装载的vector中是随机的八个点的索引。
③ 调用多线程分别用于计算fundamental matrix 和 homography

vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;

    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

    // Compute ratio of scores
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //得到R21,t21
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //得到R21,t21

利用FindHomography和FindFundamental多线程计算H和F，RH是两个矩阵的评分，高于一个阈值就选用其中的一个。ReconstructH 和 ReconstructF利用之前得到的H和F解算位姿R21,t21，并三角化计算3d点vP3D，看了一下代码结构，vP3D这个量应该是第一帧初始帧下的坐标，但这时相机光心坐标设置为0，所以vP3D应该是世界坐标。
FindHomography中运用了个技巧——归一化直接线性计算（Normalized DLT），下面是MVG的PPT上讲的算法流程：

之前对这个归一化疑惑了很久参考了一下PPT，好像是蒙特卡洛仿真效果比较好见下图。路过的大神能稍微解释一下吗～～

FindFundamental就没仔细看了。。。反正就是算F矩阵呗。。太菜了看不懂不看了。。
至此，Initialize 函数任务完成，得到了两帧之间的相对姿态以及3d点坐标和每个点三角化的成功与否的vector。

剔除没有三角化的点

当Initialize 函数执行完并且return true后，进入if语句，首先进行的是根据返回的 vbTriangulated 剔除没有三角化成功的点。

  for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
  {
       if(mvIniMatches[i]>=0 && !vbTriangulated[i])
      {
            mvIniMatches[i]=-1;
            nmatches--;
      }
  }

设置帧的Pose

详情见注释

 // 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);

CreateInitialMapMonocular()

经过上面不痛不痒的一些操作，接下来进入大头部分…根据三角化后的3d点进行MapPoints的包装。CreateInitialMapMonocular函数适合分段讲，层次比较清晰。

第一步：将初始帧和当前帧转化成关键帧，KeyFrame类是关键帧类：

KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

包括各种参数的传递，传递格子grid的信息，设置关键帧的Pose。mpMap和mpKeyFrameDB在System初始化的时候已经new过了。

第二步：将初始和当前关键帧的描述子转为BoW

pKFini->ComputeBoW();
pKFcur->ComputeBoW();

第三步：将关键帧插入到地图

mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur); //mspKeyFrames获得更新

mspKeyFrames是一个set类，详情请见set类参考网站。

第四步：将3D点包装成MapPoints

for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        //mvIniP3D是第一张初始帧的相机坐标系下的点，由于第一个是世界坐标系所以就是世界坐标系下的点
        cv::Mat worldPos(mvIniP3D[i]);

        //用3D点构造MapPoint
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);

        // 初始帧一个 i 对应当前帧一个 mvIniMatches[i]，都是索引
        // 把用 mvIniP3D 初始化好的 MapPoint 分别送入初始帧和当前帧的 mvpMapPoints 中
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        // 对该MapPoint添加观测
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        // 从观测到该MapPoint的关键帧中挑选区分度最高的描述子
        //更新该MapPoint平均观测方向以及观测距离的范围
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        //在地图中添加该MapPoint
        mpMap->AddMapPoint(pMP);
    }
    

worldPos是之前Initialize函数得到的mvIniP3D，pMP是根据当前帧和 3d点worldPos 和地图mpMap初始化的MapPoint对象。

接下来的操作AddMapPoint 和 AddObservation都是pKFini和pKFcur一对一对进行操作的，即在关键帧中添加地图点，地图点选择能被哪个帧观测到。AddMapPoint中的重要参数mvpMapPoints根据索引记录了每个地图点。AddObservation中一个重要的参数是mObservations，是个map类，键值对为关键帧和mappoint的索引，如下表所示。

keyframe	id
pKFini	i
pKFcur	mvIniMatches[i]

然后是 ComputeDistinctiveDescriptors，函数计算最有代表性的描述子，先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。UpdateNormalAndDepth 函数没细看，是更新平均观测方向和观测距离范围的。最后在map里添加地图点。

第五步：更新连接关系

pKFini->UpdateConnections();
pKFcur->UpdateConnections();

下图所示为权重连接示意图：

这个部分主要是用于利用各关键帧的共视程度（covisibility）建立帧与帧之间的连接关系。在关键帧和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数。首先，从调用这个函数的关键帧得到所有特征点，然后根据这些特征点mappoints得到观测值也就是之前提到的mObservations，函数中的 KFcounter 记录当前帧和与其他关键帧之间的权重的。vPairs 这个参数用于记录权重大于阈值的帧，一旦发现大于阈值就把帧连接上，该帧的mConnectedKeyFrameWeights获得更新，其值描述了该帧和添加的帧的共视权重，详解见注释：

void KeyFrame::UpdateConnections()
{
    //===============1==================================
    map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数

    vector<MapPoint*> vpMP;

    {
        // 获得该关键帧的所有3D点
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
    // 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        if(!pMP)
            continue;

        if(pMP->isBad())
            continue;

        // 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
        map<KeyFrame*,size_t> observations = pMP->GetObservations();

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            // 除去自身，自己与自己不算共视
            if(mit->first->mnId==mnId)
                continue;
            //权重 + 1
            KFcounter[mit->first]++;
        }
    }
    // This should not happen
    if(KFcounter.empty())
        return;

    //===============2==================================
    // 超过阈值就添加连接
    // 如果没有超过该阈值的权重，那么就只保留权重最大的边
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second > nmax)
        {
            nmax = mit->second;
            // 找到对应权重最大的关键帧（共视程度最高的关键帧）
            pKFmax = mit->first;
        }
        if(mit->second >= th)
        {
            // 对应权重需要大于阈值，对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second,mit->first));
            // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
            // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    // 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
    if(vPairs.empty())
    {
        // 如果每个关键帧与它共视的关键帧的个数都少于th，
        // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
        // 这是对之前th这个阈值可能过高的一个补丁
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由小到大
    sort(vPairs.begin(),vPairs.end());
    
    //lKFs和lWs由大到小
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    //===============3==================================
    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        // 更新图的连接(权重)
        mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        // 更新生成树的连接
        if(mbFirstConnection && mnId!=0)
        {
            // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyFrames.front();
            // 建立双向连接关系
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}

第六步：全局优化

    // Bundle Adjustment
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

    // BA优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    

第七步：中值深度归一化

这步用的函数ComputeSceneMedianDepth里用到了dot运算，详情见 dot用法

    // Set median depth to 1
    // !!!将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 points
    // 把3D点的尺度也归一化到1
    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);
        }
    }

第八步：收尾工作，参数交接

包括比如localmapping线程插入关键帧，当前帧pose的设置，“上一关键帧”（当前帧）的传递，参考帧的传递，MapDrawer画图等，最后状态置为OK。

    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;// 初始化成功，至此，初始化过程完成
}

至此，MonocularInitialization工作完成。。。。正式进入Track！