• ORB-SLAM2从理论到代码实现（十四）：KeyFrame类

    1. 原理分析

    KeyFrame为关键帧，关键帧之所以存在是因为优化需要，所以KeyFrame的几乎所有内容都是位优化服务的。该类中的函数较多，我们需要归类梳理一下，明白其功能原理，才能真正弄懂它的内容。

    图优化需要构建节点和变量，节点很好理解，就是关键帧的位姿，所以需要有读写位姿的功能，边分为两种，第一种边是和MapPoint之间的，所以需要有管理和MapPoint之间关系的函数，第二种边是和其他关键帧之间的，他们之间需要通过MapPoint产生联系，两帧能够共同观测到一定数量的MapPoint时则可以在他俩之间建立边，这种关系叫共视，所以需要有管理共视关系的函数，这种通过共视关系构建的优化模型叫做Covisibility Graph，但是，当需要优化较大范围的数据时，就会需要很大的计算量，因此需要简化，而ORB SLAM2中的Essential Graph就是Covisibility Graph的一种简化版，它通过“生成树（Spanning tree）”来管理各关键帧之间的关系，每个帧都有一个父节点和子节点，节点为其他关键帧，在构建优化模型时，只有具有父子关系的关键帧之间才建立边，换言之，Essential Graph就是Covisibility Graph的子集，这样就大大减少了边的数量，从而起到减小计算量的作用，因此，该类还需要有管理“生成树（Spanning tree）”的函数。

    为了更清晰地了解Covisibility Graph 和 Essential Graph之间的区别，我们可以利用下面几张图再详细解释一下

    图(a)没什么好说的

    图(b)中Covisibility graph是用来描述不同关键帧可以看到多少相同的地图点：每个关键帧是一个节点，如果两个关键帧之间的共视地图点数量大于15，则这两个节点之间建立边，边的权重是共视地图点的数量

    图(c)中Spanning tree就是生成树，保留了所有的节点（或者说关键帧），但给各个关键帧找了父节点和子节点，每帧只跟各自的父节点和子节点相连，与其他关键帧不连接，此即为spanning tree

    图(d)中即是essential graph，是根据spanning tree建立的图模型，它是是简版的covisibility graph。

    2. 函数功能介绍

    有了以上的分析，我们再回过头来看这个类中的这些函数，分析起来就容易很多了。我们按照上面的分析，对这些函数做一个归类，如下图所示

    下面我们按照分类，讲解其中重要的一些函数

    3. 构造函数

    3.1. Frame：有参数构造函数

    构造函数一共有三个参数：

    Frame &F：当前帧

    Map *pMap：地图Map

    KeyFrameDatabase *pKFDB：指针和关键帧数据集的指针

    1. {
    2.     //将下一帧的帧号赋值给mnId，然后自增1
    3.     mnId = nNextId++;
    4.  
    5.     //根据栅格的列数重置栅格的size
    6.     mGrid.resize(mnGridCols);
    7.     //将该真的栅格内信息拷贝了一份给关键帧类内的变量
    8.     for (int i = 0; i < mnGridCols; i++)
    9.     {
    10.         mGrid[i].resize(mnGridRows);
    11.         for (int j = 0; j < mnGridRows; j++)
    12.             mGrid[i][j] = F.mGrid[i][j];
    13.     }
    14.  
    15.     //最后将当前帧的姿态赋给该关键帧
    16.     SetPose(F.mTcw);
    17. }

    3.2. 位姿相关

    3.2.1. SetPose：设置位姿

    它只有一个参数Tcw_，这是传入的当前帧的位姿

    1. void KeyFrame::SetPose(const cv::Mat &Tcw_)
    2. {
    3.     unique_lock<mutex> lock(mMutexPose);
    4.     Tcw_.copyTo(Tcw);
    5.     cv::Mat Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
    6.     cv::Mat tcw = Tcw.rowRange(0, 3).col(3);
    7.     cv::Mat Rwc = Rcw.t();
    8.     Ow = -Rwc * tcw;//相机光心
    9.  
    10.     Twc = cv::Mat::eye(4, 4, Tcw.type());
    11.     Rwc.copyTo(Twc.rowRange(0, 3).colRange(0, 3));
    12.     Ow.copyTo(Twc.rowRange(0, 3).col(3));
    13.     // center为相机坐标系（左目）下，立体相机中心的坐标
    14.     // 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline,
    15.     // 因此可以看出，立体相机中两个摄像头的连线为x轴，正方向为左目相机指向右目相机
    16.     cv::Mat center = (cv::Mat_<float>(4, 1) << mHalfBaseline, 0, 0, 1);
    17.     // 世界坐标系下，左目相机中心到立体相机中心的向量，方向由左目相机指向立体相机中心
    18.     Cw = Twc * center;
    19. }

    4. Covisibility graph相关

    4.1. AddConnection：增加连接

    AddConnection(

    KeyFrame *pKF, // 需要关联的关键帧

    const int &weight) // 权重，即该关键帧与pKF共同观测到的3d点数量

    1. void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
    2. {
    3.     {
    4.         unique_lock<mutex> lock(mMutexConnections);
    5.         // std::map::count函数只可能返回0或1两种情况
    6.         // 此处0表示之前没有过连接，1表示有过连接
    7.         
    8.         //之前没有连接时，要用权重赋值，即添加连接
    9.         if (!mConnectedKeyFrameWeights.count(pKF)) 
    10.             mConnectedKeyFrameWeights[pKF] = weight;
    11.         //有连接，但权重发生变化时，也要用权重赋值，即更新权重
    12.         else if (mConnectedKeyFrameWeights[pKF] != weight) 
    13.             mConnectedKeyFrameWeights[pKF] = weight;
    14.         else
    15.             return;
    16.     }
    17.     
    18.     //更新最好的Covisibility
    19.     UpdateBestCovisibles();
    20. }

    4.2. UpdateBestCovisibles：更新最好的Covisibility

    每一个关键帧都有一个容器，其中记录了与其他关键帧之间的weight，每次当关键帧添加连接、删除连接或者连接权重发生变化时，都需要根据weight对容器内内容重新排序。该函数的主要作用便是按照weight对连接的关键帧进行排序，更新后的变量存储在mvpOrderedConnectedKeyFrames 和 mvOrderedWeights中。

    1. void KeyFrame::UpdateBestCovisibles()
    2. {
    3.     unique_lock<mutex> lock(mMutexConnections);
    4.     vector<pair<int, KeyFrame *>> vPairs;
    5.     vPairs.reserve(mConnectedKeyFrameWeights.size());
    6.     // 取出所有连接的关键帧，将元素取出放入一个pair组成的vector中，排序后放入vPairs
    7.     // mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>，而vPairs变量将共视的3D点数放在前面，利于排序
    8.     for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)
    9.         vPairs.push_back(make_pair(mit->second, mit->first));
    10.  
    11.     // 按照权重进行排序
    12.     sort(vPairs.begin(), vPairs.end());
    13.     list<KeyFrame *> lKFs; // keyframe
    14.     list<int> lWs;         // weight
    15.     for (size_t i = 0, iend = vPairs.size(); i < iend; i++)
    16.     {
    17.         //所以定义的链表中权重由大到小排列要用push_front
    18.         lKFs.push_front(vPairs[i].second);
    19.         lWs.push_front(vPairs[i].first);
    20.     }
    21.  
    22.     //更新排序好的连接关键帧及其对应的权重
    23.     mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
    24.     mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
    25. }

    4.3. UpdateConnections：更新连接

    该函数主要包含以下三部分内容：

    a. 首先获得该关键帧的所有MapPoint点，然后遍历观测到这些3d点的其它所有关键帧，对每一个找到的关键帧，先存储到相应的容器中。

    b. 计算所有共视帧与该帧的连接权重，权重即为共视的3d点的数量，对这些连接按照权重从大到小进行排序。当该权重必须大于一个阈值，便在两帧之间建立边，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）。

    c. 更新covisibility graph，即把计算的边用来给图赋值，然后设置spanning tree中该帧的父节点，即共视程度最高的那一帧。

    1. void KeyFrame::UpdateConnections()
    2. {
    3.     // 在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
    4.  
    5.     //===============对应a部分内容==================================
    6.     map<KeyFrame *, int> KFcounter;
    7.  
    8.     vector<MapPoint *> vpMP;
    9.  
    10.     {
    11.         // 获得该关键帧的所有3D点
    12.         unique_lock<mutex> lockMPs(mMutexFeatures);
    13.         vpMP = mvpMapPoints;
    14.     }
    15.  
    16.     //For all map points in keyframe check in which other keyframes are they seen
    17.     //Increase counter for those keyframes
    18.     // 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
    19.     for (vector<MapPoint *>::iterator vit = vpMP.begin(), vend = vpMP.end(); vit != vend; vit++)
    20.     {
    21.         MapPoint *pMP = *vit;
    22.  
    23.         if (!pMP)
    24.             continue;
    25.  
    26.         if (pMP->isBad())
    27.             continue;
    28.  
    29.         // 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
    30.         map<KeyFrame *, size_t> observations = pMP->GetObservations();
    31.  
    32.         for (map<KeyFrame *, size_t>::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
    33.         {
    34.             // 除去自身，自己与自己不算共视
    35.             if (mit->first->mnId == mnId)
    36.                 continue;
    37.             KFcounter[mit->first]++;
    38.         }
    39.     }
    40.  
    41.     // This should not happen
    42.     if (KFcounter.empty())
    43.         return;
    44.  
    45.     //===============对应b部分内容==================================
    46.     //If the counter is greater than threshold add connection
    47.     //In case no keyframe counter is over threshold add the one with maximum counter
    48.     // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
    49.     int nmax = 0;
    50.     KeyFrame *pKFmax = NULL;
    51.     int th = 15;
    52.  
    53.     // vPairs记录与其它关键帧共视帧数大于th的关键帧
    54.     // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    55.     vector<pair<int, KeyFrame *>> vPairs;
    56.     vPairs.reserve(KFcounter.size());
    57.     for (map<KeyFrame *, int>::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++)
    58.     {
    59.         if (mit->second > nmax)
    60.         {
    61.             nmax = mit->second;
    62.             // 找到对应权重最大的关键帧（共视程度最高的关键帧）
    63.             pKFmax = mit->first;
    64.         }
    65.         if (mit->second >= th)
    66.         {
    67.             // 对应权重需要大于阈值，对这些关键帧建立连接
    68.             vPairs.push_back(make_pair(mit->second, mit->first));
    69.             // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
    70.             // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
    71.             (mit->first)->AddConnection(this, mit->second);
    72.         }
    73.     }
    74.  
    75.     // 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
    76.     if (vPairs.empty())
    77.     {
    78.         // 如果每个关键帧与它共视的关键帧的个数都少于th，
    79.         // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
    80.         // 这是对之前th这个阈值可能过高的一个补丁
    81.         vPairs.push_back(make_pair(nmax, pKFmax));
    82.         pKFmax->AddConnection(this, nmax);
    83.     }
    84.  
    85.     // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
    86.     sort(vPairs.begin(), vPairs.end());
    87.     list<KeyFrame *> lKFs;
    88.     list<int> lWs;
    89.     for (size_t i = 0; i < vPairs.size(); i++)
    90.     {
    91.         lKFs.push_front(vPairs[i].second);
    92.         lWs.push_front(vPairs[i].first);
    93.     }
    94.  
    95.     //===============对应c部分内容==================================
    96.     {
    97.         unique_lock<mutex> lockCon(mMutexConnections);
    98.  
    99.         // mspConnectedKeyFrames = spConnectedKeyFrames;
    100.         // 更新图的连接(权重)
    101.         mConnectedKeyFrameWeights = KFcounter;
    102.         mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
    103.         mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
    104.  
    105.         // 更新生成树的连接
    106.         if (mbFirstConnection && mnId != 0)
    107.         {
    108.             // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
    109.             mpParent = mvpOrderedConnectedKeyFrames.front();
    110.             // 建立双向连接关系
    111.             mpParent->AddChild(this);
    112.             mbFirstConnection = false;
    113.         }
    114.     }
    115. }

    4.4. EraseConnection：移除连接

    清楚一个关键帧与其他帧对应的边

    1. void KeyFrame::EraseConnection(KeyFrame *pKF)
    2. {
    3.     bool bUpdate = false;
    4.     {
    5.         unique_lock<mutex> lock(mMutexConnections);
    6.         // 如果当前帧有连接关系，则删除
    7.         if (mConnectedKeyFrameWeights.count(pKF))
    8.         {
    9.             mConnectedKeyFrameWeights.erase(pKF);
    10.             bUpdate = true;
    11.         }
    12.     }
    13.  
    14.     // 如果删除了连接关系，便需要重新对权重进行排序
    15.     if (bUpdate)
    16.         UpdateBestCovisibles();
    17. }

    4.5. SetBadFlag：删除与该帧相关的所有连接关系

    需要删除的是该关键帧和其他所有帧、地图点之间的连接关系，但是删除会带来一个问题，就是它可能是其他节点的父节点，在删除之前需要告诉自己所有的子节点，换个爸爸，这个函数里绝大部分代码都是在完成这一步。有一片博客对这一函数的步骤讲解比较清晰，个人也比较认同，所以借鉴过来（博客链接：https://blog.csdn.net/weixin_39373577/article/details/85226187）：

    步骤一：遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系。

    步骤二：遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系。

    步骤三：清空和当前关键帧的共视关键帧集合和带顺序的关键帧集合。

    步骤四：共视图更新完毕后，还需要更新生成树。这个比较难理解。。。真实删除当前关键帧之前，需要处理好父亲和儿子关键帧关系，不然会造成整个关键帧维护的图断裂，或者混乱，不能够为后端提供较好的初值（理解起来就是父亲挂了，儿子需要找新的父亲，在候选父亲里找，当前帧的父亲肯定在候选父亲中）。

    步骤五：遍历所有把当前关键帧当成父关键帧的子关键帧。重新为他们找父关键帧。设置一个候选父关键帧集合（集合里包含了当前帧的父帧和子帧？）

    步骤六：对于每一个子关键帧，找到与它共视的关键帧集合，遍历它，看看是否有候选父帧集合里的帧，如果有，就把这个帧当做新的父帧。

    步骤七：如果有子关键帧没有找到新的父帧，那么直接把当前帧的父帧（爷）当成它的父帧

    配合下图可以看得更清楚：

    5. spanning tree 相关

    这一类函数所有的操作都是在围绕自己的子节点和父节点，其中子节点可能有多个，所以是一个容器mspChildrens，父节点只能有一个，所以是个变量mpParent。函数内容都比较简单，在此一并列出

    1. void KeyFrame::AddChild(KeyFrame *pKF) //增加子树
    2. {
    3.     unique_lock<mutex> lockCon(mMutexConnections);
    4.     mspChildrens.insert(pKF);
    5. }
    6.  
    7. void KeyFrame::EraseChild(KeyFrame *pKF) //删除子树
    8. {
    9.     unique_lock<mutex> lockCon(mMutexConnections);
    10.     mspChildrens.erase(pKF);
    11. }
    12.  
    13. void KeyFrame::ChangeParent(KeyFrame *pKF) //更换父节点
    14. {
    15.     unique_lock<mutex> lockCon(mMutexConnections);
    16.     mpParent = pKF;
    17.     pKF->AddChild(this);
    18. }
    19.  
    20. set<KeyFrame *> KeyFrame::GetChilds() //获取所有子节点
    21. {
    22.     unique_lock<mutex> lockCon(mMutexConnections);
    23.     return mspChildrens;
    24. }
    25.  
    26. KeyFrame *KeyFrame::GetParent() //获取父节点
    27. {
    28.     unique_lock<mutex> lockCon(mMutexConnections);
    29.     return mpParent;
    30. }
    31.  
    32. bool KeyFrame::hasChild(KeyFrame *pKF) //判断是否有子节点
    33. {
    34.     unique_lock<mutex> lockCon(mMutexConnections);
    35.     return mspChildrens.count(pKF);
    36. }

    6. MapPoint相关

    这一类函数的内容同样比较j简单，主要围绕存放MapPoint的容器mvpMapPoints进行。在此一并列出这些函数

    1. void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)//增加MapPoint
    2. {
    3.     unique_lock<mutex> lock(mMutexFeatures);
    4.     mvpMapPoints[idx] = pMP;
    5. }
    6.  
    7. void KeyFrame::EraseMapPointMatch(const size_t &idx)//删除MapPoint
    8. {
    9.     unique_lock<mutex> lock(mMutexFeatures);
    10.     mvpMapPoints[idx] = static_cast<MapPoint *>(NULL);
    11. }
    12.  
    13. void KeyFrame::EraseMapPointMatch(MapPoint *pMP)//删除MapPoint的匹配关系
    14. {
    15.     int idx = pMP->GetIndexInKeyFrame(this);
    16.     if (idx >= 0)
    17.         mvpMapPoints[idx] = static_cast<MapPoint *>(NULL);
    18. }
    19.  
    20. void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint *pMP)//替换MapPoint的匹配关系
    21. {
    22.     mvpMapPoints[idx] = pMP;
    23. }
    24.  
    25. set<MapPoint *> KeyFrame::GetMapPoints()//获取所有MapPoint
    26. {
    27.     unique_lock<mutex> lock(mMutexFeatures);
    28.     set<MapPoint *> s;
    29.     for (size_t i = 0, iend = mvpMapPoints.size(); i < iend; i++)
    30.     {
    31.         if (!mvpMapPoints[i])
    32.             continue;
    33.         MapPoint *pMP = mvpMapPoints[i];
    34.         if (!pMP->isBad())
    35.             s.insert(pMP);
    36.     }
    37.     return s;
    38. }
    39.  
    40. //获取被观测相机数大于等于minObs的MapPoint
    41. int KeyFrame::TrackedMapPoints(const int &minObs)
    42. {
    43.     unique_lock<mutex> lock(mMutexFeatures);
    44.  
    45.     int nPoints = 0;
    46.     const bool bCheckObs = minObs > 0;
    47.     for (int i = 0; i < N; i++)
    48.     {
    49.         MapPoint *pMP = mvpMapPoints[i];
    50.         if (pMP)
    51.         {
    52.             if (!pMP->isBad())
    53.             {
    54.                 if (bCheckObs)
    55.                 {
    56.                     if (mvpMapPoints[i]->Observations() >= minObs)
    57.                         nPoints++;
    58.                 }
    59.                 else
    60.                     nPoints++;
    61.             }
    62.         }
    63.     }
    64.  
    65.     return nPoints;
    66. }

    7. ComputeSceneMedianDepth：计算场景中的中位深度

    步骤一：获取每个地图点的世界位姿

    步骤二：找出当前帧Z方向上的旋转和平移，求每个地图点在当前相机坐标系中的z轴位置，求平均值。

    1. float KeyFrame::ComputeSceneMedianDepth(const int q)
    2. {
    3.     vector<MapPoint *> vpMapPoints;
    4.     cv::Mat Tcw_;
    5.     {
    6.         unique_lock<mutex> lock(mMutexFeatures);
    7.         unique_lock<mutex> lock2(mMutexPose);
    8.         vpMapPoints = mvpMapPoints;
    9.         Tcw_ = Tcw.clone();
    10.     }
    11.  
    12.     vector<float> vDepths;
    13.     vDepths.reserve(N);
    14.     cv::Mat Rcw2 = Tcw_.row(2).colRange(0, 3);
    15.     Rcw2 = Rcw2.t();
    16.     float zcw = Tcw_.at<float>(2, 3);
    17.     for (int i = 0; i < N; i++)
    18.     {
    19.         if (mvpMapPoints[i])
    20.         {
    21.             MapPoint *pMP = mvpMapPoints[i];
    22.             cv::Mat x3Dw = pMP->GetWorldPos();
    23.             float z = Rcw2.dot(x3Dw) + zcw; // (R*x3Dw+t)的第三行，即z
    24.             vDepths.push_back(z);
    25.         }
    26.     }
    27.  
    28.     sort(vDepths.begin(), vDepths.end());
    29.  
    30.     return vDepths[(vDepths.size() - 1) / q];
    31. }

    8. 修改建议

    KeyFrame与Frame的成员变量和功能有很大的相似性，可以把KeyFrame作为Frame的一个来实现

    参考文献

    主要内容来自下文，修改了一些描述，增加了一些注释

    ORB SLAM2源码解读(四)：KeyFrame类 - 古月居

  • 相关阅读:
    响应式编程-Project Reactor Mono 介绍
    记一次使用NetworkManager管理Ubuntu网络无效问题分析
    软考-系统集成项目管理中级--信息(文档)和配置管理
    git-新增业务代码分支
    低代码平台AWS PaaS_安装应用商店的标准应用（安装单一应用）
    GBase 8s是如何保证数据一致性
    Linux命令详解-find命令（二）
    Xcode预览(Preview)显示List视图内容的一个Bug及解决
    金融生产存储亚健康治理：升级亚健康 3.0 ，应对万盘规模的挑战
    智能手表上的音频（三）：音频文件播放
  • 原文地址：https://blog.csdn.net/xhtchina/article/details/125847856