ORB_SLAM2学习笔记（3）之 单目初始化（初始化地图）

ORB_SLAM2学习笔记（3）之 单目初始化（2）

初始化地图

//创建初始地图
void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames
    //初始帧与当前帧都为关键帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

    //生成词袋向量
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();

    // Insert KFs in the map
    //把关键帧导入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

    // Create MapPoints and asscoiate to keyframes
    //遍历所有匹配点
    for(size_t i=0; i<mvIniMatches.size();i++)
    {
        //没有匹配的跳过 开始设置的 -1
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        //创建空间点：把三角化之后的3D点转化为构建地图的地图点
        cv::Mat worldPos(mvIniP3D[i]);
        //创建地图点 ： 空间点， 当前帧， 地图
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
        // 为该MapPoint添加属性：
        // a.观测到该MapPoint的关键帧
        // b.该MapPoint的描述子
        // c.该MapPoint的平均观测方向和深度范围
        //在初始帧与当前帧中加入空间点
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
        //加入可以观测到该空间点的 关键帧
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);
        //计算能看见空间点关键帧的区分度最高的描述子
        pMP->ComputeDistinctiveDescriptors();
        //更新该MapPoint平均观测方向以及观测距离的范围
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        //mvIniMatches下标i表示在初始化参考帧中的特征点的序号
        //将加入好地图点以及描述子深度等信息的地图 赋值 在当前帧的地图点上
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        //外点不用管
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        //把转化好的地图点放地图上
        mpMap->AddMapPoint(pMP);
    }

    // Update Connections
    //更新关键帧间的连接关系
    //在关键帧与空间点之间建立联系，也就是建立一个边，每条边都有权重，也就是关键帧看到公共点的个数
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

    // Bundle Adjustment
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
    //后端进行局部优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    // Set median depth to 1
    //得到初始帧所有特征点深度的中值
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    //逆深度
    float invMedianDepth = 1.0f/medianDepth;
    //如果平均深度小于0或者初始化的地图点小于100 则重新初始化
    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }
    //
    // Scale initial baseline
    //得到世界到相机的T
    cv::Mat Tc2w = pKFcur->GetPose();
    //得到空间坐标/中值
    //Tc2w.col(3).rowRange(0,3) 得到数组第4列的前3个值
    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++)
    {
        //如果存在则进行尺度变换（有的讲解说是归一化，但是源码写的invMedianDepth为所有深度的中位数）
        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;
}