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）留在下一节说。