@[toc]

1. 将初始化的两帧转化为关键帧

// Create KeyFrames 认为单目初始化时候的参考帧和当前帧都是关键帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB); // 第一帧
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpAtlas->GetCurrentMap(),mpKeyFrameDB);  // 第二帧

2. 计算初始关键帧和当前关键帧的描述子的BoW

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

3. 将关键帧插入到地图

mpAtlas->AddKeyFrame(pKFini);
mpAtlas->AddKeyFrame(pKFcur);

4. 遍历初始化得到的3D点来生成地图点MapPoints

4.1 用3D点构造MapPoint

MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpAtlas->GetCurrentMap());

4.2 添加该MapPoint可以被该KeyFrame的哪个特征点观测到

/**
 * @brief 给地图点添加观测
 *
 * 记录哪些KeyFrame的那个特征点能观测到该MapPoint \n
 * 并增加观测的相机数目nObs，单目+1，双目或者grbd+2
 * 这个函数是建立关键帧共视关系的核心函数，能共同观测到某些MapPoints的关键帧是共视关键帧
 * @param pKF KeyFrame
 * @param idx MapPoint在KeyFrame中的索引(即对应的特征点索引)
 */
void MapPoint::AddObservation(KeyFrame* pKF, int idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    tuple<int,int> indexes;
    //map.count(Key)返回值为1或者0，1返回存在，0返回不存在，
    //返回的是布尔类型的值，因为在map类型中所有的数据的Key值都是不同的，所以被count的数要么存在1次，要么不存在
    //? 感觉这一步没用，因为都会被下面覆盖掉，是不是因为鱼眼相机的关系，有待考察
    if(mObservations.count(pKF)){ // 已经存在，即已经添加过了观测关系
        indexes = mObservations[pKF];
    }
    else{   // 还没有
        indexes = tuple<int,int>(-1,-1);
    }

    // 如果是鱼眼相机
    if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){
        get<1>(indexes) = idx; // 右图中的特征点索引
    }
    else{ // 不是就全放在左图里
        get<0>(indexes) = idx;  // 左图中的特征点索引
    }
     // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=indexes;


    if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0) // 不是鱼眼相机 && 双目或者grbd
        nObs+=2;
    else // 单目
        nObs++;
}

4.3 从众多观测到该MapPoint的特征点中挑选最有代表性的描述子

/**
 * @brief 计算具有代表的描述子
 *
 * 由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子 \n
 * 先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值
 */
void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector<cv::Mat> vDescriptors; // 所有观测的描述子

    map<KeyFrame*,tuple<int,int>> observations;
    //获取所有观测，跳过坏点
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());
    // 遍历观测到该地图点的所有关键帧，获得orb描述子，并插入到vDescriptors中
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // mit->first取观测到该地图点的关键帧
        // mit->second取该地图点在关键帧中的索引
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad()){
            tuple<int,int> indexes = mit -> second;
            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

            // leftIndex==-1表示不存在
            if(leftIndex != -1){
                // 取对应的描述子向量  
                vDescriptors.push_back(pKF->mDescriptors.row(leftIndex));
            }
            //同上
            if(rightIndex != -1){
                // 取对应的描述子向量  
                vDescriptors.push_back(pKF->mDescriptors.row(rightIndex));
            }
        }
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them
    // 获得这些描述子两两之间的距离


    // N表示为一共多少个描述子
    const size_t N = vDescriptors.size();

    float Distances[N][N];// 将Distances表述成一个对称的矩阵
    for(size_t i=0;i<N;i++)
    {
        Distances[i][i]=0;//和自己的距离当然是0
        // 计算并记录不同描述子距离
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

    // Take the descriptor with least median distance to the rest
    // Step 4 选择最有代表性的描述子，它与其他描述子应该具有最小的距离中值
    int BestMedian = INT_MAX; // 记录最小的中值
    int BestIdx = 0;        // 最小中值对应的索引
    for(size_t i=0;i<N;i++)
    {
        // 第i个描述子到其它所有所有描述子之间的距离
        vector<int> vDists(Distances[i],Distances[i]+N);
        sort(vDists.begin(),vDists.end());
        int median = vDists[0.5*(N-1)];// 获得中值
        // 寻找最小的中值
        if(median<BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        unique_lock<mutex> lock(mMutexFeatures);
        // 最好的描述子，该描述子相对于其他描述子有最小的距离中值
        // 当方差过大时，中值比平均值更能表示样本的“中位状态”
        // 最好的描述子就是和其它描述子的中值距离最小
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

4.4 更新该MapPoint平均观测方向以及观测距离的范围

/** 
 * @brief 更新平均观测方向以及观测距离范围
 * @details 1、遍历所有观测到该点的关键帧,求观测的平均方向
 *          2、根据参考关键帧算最大观测距离和最小观测距离
 */
void MapPoint::UpdateNormalAndDepth()
{
    // Step 1 获得该地图点的相关信息
    map<KeyFrame*,tuple<int,int>> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;
        observations=mObservations;// 获得观测到该地图点的所有关键帧
        pRefKF=mpRefKF;             // 观测到该点的参考关键帧就是创建该MapPoint的那个关键帧
        Pos = mWorldPos.clone();    // 地图点在世界坐标系中的位置
    }


    if(observations.empty())
        return;
    // Step 2 计算该地图点的法线方向，也就是朝向等信息。
    // 能观测到该地图点的所有关键帧，对该点的观测方向归一化为单位向量，然后进行求和得到该地图点的朝向
    //初始值为0向量用于累加;但是放心每次累加的变量都是经过归一化之后的
    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    // 遍历所有观测到该点的关键帧，求观测的平均方向
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first; // 关键帧

        tuple<int,int> indexes = mit -> second; // 特征点所以
        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

        // 这里特征点索引只是用来确定是左相机还是右相机
        if(leftIndex != -1){
            cv::Mat Owi = pKF->GetCameraCenter(); // 获得相机中心
            cv::Mat normali = mWorldPos - Owi;//观测方向=由相机中心指向3D点在世界坐标系中的位置
            normal = normal + normali/cv::norm(normali);// 对所有关键帧对该点的观测方向归一化为单位向量进行求和 
            n++;
        }
        if(rightIndex != -1){
            cv::Mat Owi = pKF->GetRightCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }
    }

    // 根据参考关键帧算最大观测距离和最小观测距离


    cv::Mat PC = Pos - pRefKF->GetCameraCenter();// 参考关键帧相机指向3D点的向量（在世界坐标系下的表示）
    const float dist = cv::norm(PC);// 该点到参考关键帧相机的距离

    // 参考关键帧的特征点索引
    tuple<int ,int> indexes = observations[pRefKF];
    int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
    int level;
    // 这里特征点索引只是用来确定是左相机还是右相机
    if(pRefKF -> NLeft == -1){
        level = pRefKF->mvKeysUn[leftIndex].octave; //观测到该地图点的当前帧的特征点在金字塔的第几层
    }
    else if(leftIndex != -1){
        level = pRefKF -> mvKeys[leftIndex].octave;
    }
    else{
        level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
    }

    //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level]; //当前金字塔层对应的尺度因子，scale^n，scale=1.2，n为层数
    const int nLevels = pRefKF->mnScaleLevels; // 金字塔总层数，默认为8

    {
        unique_lock<mutex> lock3(mMutexPos);
        // 即最底层观测到该点时的距离 
        mfMaxDistance = dist*levelScaleFactor;// 观测到该点的距离最大值    实际距离 * 能观测到的最大的金字塔尺度= 实际能观测到的最大距离 
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1]; // 观测到该点的距离最小值 // 即最上层观测到该点时的距离
        mNormalVector = normal/n; // 获得平均的观测方向
    }
}

4.5 3D点添加进地图

mpAtlas->AddMapPoint(pMP);

5. 更新关键帧间的连接关系

/**
 * @brief 更新共视图和最大生成树的连接
 * 
 * 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度
 *    对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
 * 2. 并且该权重必须大于一个阈值，如果没有超过该阈值（15个点）的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
 * 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
 *    更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
 */
void KeyFrame::UpdateConnections(bool upParent)
{
    // 在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
    /*=================================================================================*/

    map<KeyFrame*,int> KFcounter;// 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数，也称为共视程度

    vector<MapPoint*> vpMP;
    // 获得该关键帧的所有地图点
    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    //For all map points in keyframe check in which other keyframes are they seen
    //Increase counter for those keyframes
    // 通过遍历地图点间接统计可以观测到这些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*,tuple<int,int>> observations = pMP->GetObservations();

        for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {   
            // 除去自身，自己与自己不算共视|| 关键帧已被丢弃|| 不在同一个地图
            if(mit->first->mnId==mnId || mit->first->isBad() || mit->first->GetMap() != mpMap)
                continue;
            // 这里的操作非常精彩！
            // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
            // mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
            // 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度
            KFcounter[mit->first]++;// 所以这里最后得到的是当前关键帧和其他关键帧的共视程度

        }
    }

    // This should not happen
    // 没有共视关系，直接退出 
    if(KFcounter.empty())
        return;

    //If the counter is greater than threshold add connection
    //In case no keyframe counter is over threshold add the one with maximum counter
    int nmax=0; // 记录最高的共视程度
    KeyFrame* pKFmax=NULL;
    int th = 15; // 共视地图点阈值

     // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    if(!upParent)  // upParent默认为true，相当于没用到
        cout << "UPDATE_CONN: current KF " << mnId << endl;
    // 遍历和当前关键帧具有共视关系的关键帧   
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(!upParent)
            cout << "  UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl;
        // 更新具有最佳共视关系的关键帧信息
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }
         // 建立共视关系至少需要大于等于th个共视地图点
        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);
    }

     //  Step 4 对满足共视程度的关键帧对更新连接关系及权重（从大到小）

    sort(vPairs.begin(),vPairs.end());// sort函数默认升序排列
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {   
        // push_front 后变成了从大到小顺序
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }
     //此时 vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
         // 更新图的连接(权重)
         // TODO AddConnection()里已经更新过mConnectedKeyFrameWeights了，这里这样不会覆盖吗？？
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

//        if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
//        {
//            mpParent = mvpOrderedConnectedKeyFrames.front();
//            mpParent->AddChild(this);
//            mbFirstConnection = false;
//        }

        // 当前帧是否是还没父关键帧 && 不是地图的第一帧
        //? 父关键帧只添加一次
        if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
        {
            /*if(!mpParent || mpParent->GetParent() != this)
            {
                KeyFrame* pBestParent = static_cast<KeyFrame*>(NULL);
                for(KeyFrame* pKFi : mvpOrderedConnectedKeyFrames)
                {
                    if(pKFi->GetParent() || pKFi->mnId == mpMap->GetInitKFid())
                    {
                        pBestParent = pKFi;
                        break;
                    }
                }
                if(!pBestParent)
                {
                    cout << "It can't be a covisible KF without Parent" << endl << endl;
                    return;
                }
                mpParent = pBestParent;
                mpParent->AddChild(this);
                mbFirstConnection = false;
            }*/
            // cout << "udt.conn.id: " << mnId << endl;
            mpParent = mvpOrderedConnectedKeyFrames.front();//该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent->AddChild(this); // 建立双向连接关系
            mbFirstConnection = false; //? 父关键帧只添加一次
        }

    }
}

6. 全局BA优化

BA的部分后面再讲

7. 取场景的中值深度，用于尺度归一化

/**
 * @brief 评估当前关键帧场景深度，q=2表示中值 只是在单目情况下才会使用
 * @details 其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值作为当前场景的平均深度
 * @param q q=2
 * @return Median Depth
 */
float KeyFrame::ComputeSceneMedianDepth(const int q)
{
    vector<MapPoint*> vpMapPoints;
    cv::Mat Tcw_;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPose);
        vpMapPoints = mvpMapPoints;
        Tcw_ = Tcw.clone();
    }

    vector<float> vDepths;
    vDepths.reserve(N);
    cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
    Rcw2 = Rcw2.t(); // 转置
    float zcw = Tcw_.at<float>(2,3);
    // 遍历每一个地图点,计算并保存其在当前关键帧下的深度
    for(int i=0; i<N; i++)
    {
        if(mvpMapPoints[i])
        {
            MapPoint* pMP = mvpMapPoints[i];
            cv::Mat x3Dw = pMP->GetWorldPos(); // 获取这个地图点的世界坐标
            float z = Rcw2.dot(x3Dw)+zcw;// (R*x3Dw+t)的第三行，即z
            vDepths.push_back(z);
        }
    }

    sort(vDepths.begin(),vDepths.end());

    return vDepths[(vDepths.size()-1)/q];
}

8. 相机位置，3D点尺度归一化

 // 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
    // Step 7 把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);
            pMP->UpdateNormalAndDepth(); // ORB3新加的
        }
    }

9. 将关键帧插入局部地图，更新归一化后的位姿、局部地图点