ORB-SLAM （四）tracking單目初始化

單目初始化以及通過三角化恢復出地圖點

單目的初始化有專門的初始化器，只有連續的兩幀特征點均>100個才能夠成功構建初始化器。

// mvbPreMatched是第一幀中的所有特征點；mvIniMatches標記匹配狀態，未匹配上的標為-1；如果返回nmatches<100,初始化失敗，重新初始化過程
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

若成功獲取滿足特征點匹配條件的連續兩幀，並行計算分解基礎矩陣和單應矩陣（獲取的點恰好位於同一個平面），得到幀間運動（位姿），vbTriangulated標記一組特征點能否進行三角化。mvIniP3D是cv::Point3f類型的一個容器，是個存放3D點的臨時變量。

該函數對應Initialize.cpp文件，需要完成較多工作，后面再介紹。

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

最后初始化兩幀位姿並且將mvIniP3D中的點包裝成MapPoint，構建初始地圖。

mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
mCurrentFrame.SetPose(Tcw);
CreateInitialMapMonocular();

構建初始地圖就是將這兩關鍵幀以及對應的地圖點加入地圖（mpMap）中，需要分別構造關鍵幀以及地圖點

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

地圖點中需要加入其一些屬性：

1. 觀測到該地圖點的關鍵幀（對應的關鍵點）；

2. 該MapPoint的描述子；

3. 該MapPoint的平均觀測方向和觀測距離。

for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;
        //Create MapPoint from mvIniP3D
        cv::Mat worldPos(mvIniP3D[i]);
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);

        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
        mpMap->AddMapPoint(pMP);
    }

還需要更新關鍵幀之間連接關系（以共視地圖點的數量作為權重）：

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

對這兩幀姿態進行全局優化重投影誤差（LM）：

Optimizer::GlobalBundleAdjustemnt(mpMap,20);

注意這里使用的是全局優化，和回環檢測調整后的大回環優化使用的是同一個函數。

接下來，需要歸一化第一幀中地圖點深度的中位數；如果深度<0或者這時發現優化后第二幀追蹤到的地圖點<100，也需要重新初始化。

否則，將深度中值作為單位一，歸一化第二幀的位姿與所有的地圖點。

    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 current pose
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    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);
        }
    }

最后和StereoInitialization()中一樣，需要更新局部地圖，局部關鍵幀和局部地圖點；並且更新LastFrame，LastKeyFrame，以及當前幀的ReferenceKF（上一幀，上一關鍵幀，參考關鍵幀）。

這就是單目初始化的大體步驟，其中最關鍵算法是通過初始連續兩幀的對極約束恢復出相機姿態和地圖點的部分（Initializer.cpp）留在下一節說。