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地图MapKeyFrameDatabase *pKFDB指针和关键帧数据集的指针{ //将下一帧的帧号赋值给mnId然后自增1 mnId nNextId; //根据栅格的列数重置栅格的size mGrid.resize(mnGridCols); //将该真的栅格内信息拷贝了一份给关键帧类内的变量 for (int i 0; i mnGridCols; i) { mGrid[i].resize(mnGridRows); for (int j 0; j mnGridRows; j) mGrid[i][j] F.mGrid[i][j]; } //最后将当前帧的姿态赋给该关键帧 SetPose(F.mTcw); }3.2. 位姿相关SetPose设置位姿它只有一个参数Tcw_这是传入的当前帧的位姿void KeyFrame::SetPose(const cv::Mat Tcw_) { unique_lockmutex lock(mMutexPose); Tcw_.copyTo(Tcw); cv::Mat Rcw Tcw.rowRange(0, 3).colRange(0, 3); cv::Mat tcw Tcw.rowRange(0, 3).col(3); cv::Mat Rwc Rcw.t(); Ow -Rwc * tcw;//相机光心 Twc cv::Mat::eye(4, 4, Tcw.type()); Rwc.copyTo(Twc.rowRange(0, 3).colRange(0, 3)); Ow.copyTo(Twc.rowRange(0, 3).col(3)); // center为相机坐标系左目下立体相机中心的坐标 // 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline, // 因此可以看出立体相机中两个摄像头的连线为x轴正方向为左目相机指向右目相机 cv::Mat center (cv::Mat_float(4, 1) mHalfBaseline, 0, 0, 1); // 世界坐标系下左目相机中心到立体相机中心的向量方向由左目相机指向立体相机中心 Cw Twc * center; }4. Covisibility graph相关4.1.AddConnection增加连接AddConnection( KeyFrame *pKF, // 需要关联的关键帧 const int weight) // 权重即该关键帧与pKF共同观测到的3d点数量void KeyFrame::AddConnection(KeyFrame *pKF, const int weight) { { unique_lockmutex lock(mMutexConnections); // std::map::count函数只可能返回0或1两种情况 // 此处0表示之前没有过连接1表示有过连接 //之前没有连接时要用权重赋值即添加连接 if (!mConnectedKeyFrameWeights.count(pKF)) mConnectedKeyFrameWeights[pKF] weight; //有连接但权重发生变化时也要用权重赋值即更新权重 else if (mConnectedKeyFrameWeights[pKF] ! weight) mConnectedKeyFrameWeights[pKF] weight; else return; } //更新最好的Covisibility UpdateBestCovisibles(); }4.2.UpdateBestCovisibles更新最好的Covisibility每一个关键帧都有一个容器其中记录了与其他关键帧之间的weight每次当关键帧添加连接、删除连接或者连接权重发生变化时都需要根据weight对容器内内容重新排序。该函数的主要作用便是按照weight对连接的关键帧进行排序更新后的变量存储在mvpOrderedConnectedKeyFrames 和 mvOrderedWeights 中。void KeyFrame::UpdateBestCovisibles() { unique_lockmutex lock(mMutexConnections); vectorpairint, KeyFrame * vPairs; vPairs.reserve(mConnectedKeyFrameWeights.size()); // 取出所有连接的关键帧将元素取出放入一个pair组成的vector中排序后放入vPairs // mConnectedKeyFrameWeights的类型为std::mapKeyFrame*,int而vPairs变量将共视的3D点数放在前面利于排序 for (mapKeyFrame *, int::iterator mit mConnectedKeyFrameWeights.begin(), mend mConnectedKeyFrameWeights.end(); mit ! mend; mit) vPairs.push_back(make_pair(mit-second, mit-first)); // 按照权重进行排序 sort(vPairs.begin(), vPairs.end()); listKeyFrame * lKFs; // keyframe listint lWs; // weight for (size_t i 0, iend vPairs.size(); i iend; i) { //所以定义的链表中权重由大到小排列要用push_front lKFs.push_front(vPairs[i].second); lWs.push_front(vPairs[i].first); } //更新排序好的连接关键帧及其对应的权重 mvpOrderedConnectedKeyFrames vectorKeyFrame *(lKFs.begin(), lKFs.end()); mvOrderedWeights vectorint(lWs.begin(), lWs.end()); }4.3. UpdateConnections更新连接该函数主要包含以下三部分内容a. 首先获得该关键帧的所有MapPoint点然后遍历观测到这些3d点的其它所有关键帧对每一个找到的关键帧先存储到相应的容器中。b. 计算所有共视帧与该帧的连接权重权重即为共视的3d点的数量对这些连接按照权重从大到小进行排序。当该权重必须大于一个阈值便在两帧之间建立边如果没有超过该阈值的权重那么就只保留权重最大的边与其它关键帧的共视程度比较高。c. 更新covisibility graph即把计算的边用来给图赋值然后设置spanning tree中该帧的父节点即共视程度最高的那一帧。void KeyFrame::UpdateConnections() { // 在没有执行这个函数前关键帧只和MapPoints之间有连接关系这个函数可以更新关键帧之间的连接关系 //对应a部分内容 mapKeyFrame *, int KFcounter; vectorMapPoint * vpMP; { // 获得该关键帧的所有3D点 unique_lockmutex lockMPs(mMutexFeatures); vpMP mvpMapPoints; } //For all map points in keyframe check in which other keyframes are they seen //Increase counter for those keyframes // 即统计每一个关键帧都有多少关键帧与它存在共视关系统计结果放在KFcounter for (vectorMapPoint *::iterator vit vpMP.begin(), vend vpMP.end(); vit ! vend; vit) { MapPoint *pMP *vit; if (!pMP) continue; if (pMP-isBad()) continue; // 对于每一个MapPoint点observations记录了可以观测到该MapPoint的所有关键帧 mapKeyFrame *, size_t observations pMP-GetObservations(); for (mapKeyFrame *, size_t::iterator mit observations.begin(), mend observations.end(); mit ! mend; mit) { // 除去自身自己与自己不算共视 if (mit-first-mnId mnId) continue; KFcounter[mit-first]; } } // This should not happen if (KFcounter.empty()) return; //对应b部分内容 //If the counter is greater than threshold add connection //In case no keyframe counter is over threshold add the one with maximum counter // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度 int nmax 0; KeyFrame *pKFmax NULL; int th 15; // vPairs记录与其它关键帧共视帧数大于th的关键帧 // pairint,KeyFrame*将关键帧的权重写在前面关键帧写在后面方便后面排序 vectorpairint, KeyFrame * vPairs; vPairs.reserve(KFcounter.size()); for (mapKeyFrame *, int::iterator mit KFcounter.begin(), mend KFcounter.end(); mit ! mend; mit) { if (mit-second nmax) { nmax mit-second; // 找到对应权重最大的关键帧共视程度最高的关键帧 pKFmax mit-first; } 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); } // vPairs里存的都是相互共视程度比较高的关键帧和共视权重由大到小 sort(vPairs.begin(), vPairs.end()); listKeyFrame * lKFs; listint lWs; for (size_t i 0; i vPairs.size(); i) { lKFs.push_front(vPairs[i].second); lWs.push_front(vPairs[i].first); } //对应c部分内容 { unique_lockmutex lockCon(mMutexConnections); // mspConnectedKeyFrames spConnectedKeyFrames; // 更新图的连接(权重) mConnectedKeyFrameWeights KFcounter; mvpOrderedConnectedKeyFrames vectorKeyFrame *(lKFs.begin(), lKFs.end()); mvOrderedWeights vectorint(lWs.begin(), lWs.end()); // 更新生成树的连接 if (mbFirstConnection mnId ! 0) { // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧 mpParent mvpOrderedConnectedKeyFrames.front(); // 建立双向连接关系 mpParent-AddChild(this); mbFirstConnection false; } } }4.4. EraseConnection移除连接清楚一个关键帧与其他帧对应的边void KeyFrame::EraseConnection(KeyFrame *pKF) { bool bUpdate false; { unique_lockmutex lock(mMutexConnections); // 如果当前帧有连接关系则删除 if (mConnectedKeyFrameWeights.count(pKF)) { mConnectedKeyFrameWeights.erase(pKF); bUpdate true; } } // 如果删除了连接关系便需要重新对权重进行排序 if (bUpdate) UpdateBestCovisibles(); }4.5. SetBadFlag删除与该帧相关的所有连接关系需要删除的是该关键帧和其他所有帧、地图点之间的连接关系但是删除会带来一个问题就是它可能是其他节点的父节点在删除之前需要告诉自己所有的子节点换个爸爸这个函数里绝大部分代码都是在完成这一步。步骤一遍历所有和当前关键帧共视的关键帧删除他们与当前关键帧的联系。步骤二遍历每一个当前关键帧的地图点删除每一个地图点和当前关键帧的联系。步骤三清空和当前关键帧的共视关键帧集合和带顺序的关键帧集合。步骤四共视图更新完毕后还需要更新生成树。这个比较难理解。。。真实删除当前关键帧之前需要处理好父亲和儿子关键帧关系不然会造成整个关键帧维护的图断裂或者混乱不能够为后端提供较好的初值理解起来就是父亲挂了儿子需要找新的父亲在候选父亲里找当前帧的父亲肯定在候选父亲中。步骤五遍历所有把当前关键帧当成父关键帧的子关键帧。重新为他们找父关键帧。设置一个候选父关键帧集合集合里包含了当前帧的父帧和子帧步骤六对于每一个子关键帧找到与它共视的关键帧集合遍历它看看是否有候选父帧集合里的帧如果有就把这个帧当做新的父帧。步骤七如果有子关键帧没有找到新的父帧那么直接把当前帧的父帧爷当成它的父帧配合下图可以看得更清楚5.spanning tree相关这一类函数所有的操作都是在围绕自己的子节点和父节点其中子节点可能有多个所以是一个容器mspChildrens父节点只能有一个所以是个变量mpParent。函数内容都比较简单在此一并列出void KeyFrame::AddChild(KeyFrame *pKF) //增加子树 { unique_lockmutex lockCon(mMutexConnections); mspChildrens.insert(pKF); } void KeyFrame::EraseChild(KeyFrame *pKF) //删除子树 { unique_lockmutex lockCon(mMutexConnections); mspChildrens.erase(pKF); } void KeyFrame::ChangeParent(KeyFrame *pKF) //更换父节点 { unique_lockmutex lockCon(mMutexConnections); mpParent pKF; pKF-AddChild(this); } setKeyFrame * KeyFrame::GetChilds() //获取所有子节点 { unique_lockmutex lockCon(mMutexConnections); return mspChildrens; } KeyFrame *KeyFrame::GetParent() //获取父节点 { unique_lockmutex lockCon(mMutexConnections); return mpParent; } bool KeyFrame::hasChild(KeyFrame *pKF) //判断是否有子节点 { unique_lockmutex lockCon(mMutexConnections); return mspChildrens.count(pKF); }6. MapPoint相关这一类函数的内容同样比较j简单主要围绕存放MapPoint的容器mvpMapPoints进行。在此一并列出这些函数void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t idx)//增加MapPoint { unique_lockmutex lock(mMutexFeatures); mvpMapPoints[idx] pMP; } void KeyFrame::EraseMapPointMatch(const size_t idx)//删除MapPoint { unique_lockmutex lock(mMutexFeatures); mvpMapPoints[idx] static_castMapPoint *(NULL); } void KeyFrame::EraseMapPointMatch(MapPoint *pMP)//删除MapPoint的匹配关系 { int idx pMP-GetIndexInKeyFrame(this); if (idx 0) mvpMapPoints[idx] static_castMapPoint *(NULL); } void KeyFrame::ReplaceMapPointMatch(const size_t idx, MapPoint *pMP)//替换MapPoint的匹配关系 { mvpMapPoints[idx] pMP; } setMapPoint * KeyFrame::GetMapPoints()//获取所有MapPoint { unique_lockmutex lock(mMutexFeatures); setMapPoint * s; for (size_t i 0, iend mvpMapPoints.size(); i iend; i) { if (!mvpMapPoints[i]) continue; MapPoint *pMP mvpMapPoints[i]; if (!pMP-isBad()) s.insert(pMP); } return s; } //获取被观测相机数大于等于minObs的MapPoint int KeyFrame::TrackedMapPoints(const int minObs) { unique_lockmutex lock(mMutexFeatures); int nPoints 0; const bool bCheckObs minObs 0; for (int i 0; i N; i) { MapPoint *pMP mvpMapPoints[i]; if (pMP) { if (!pMP-isBad()) { if (bCheckObs) { if (mvpMapPoints[i]-Observations() minObs) nPoints; } else nPoints; } } } return nPoints; }7.ComputeSceneMedianDepth计算场景中的中位深度步骤一获取每个地图点的世界位姿步骤二找出当前帧Z方向上的旋转和平移求每个地图点在当前相机坐标系中的z轴位置求平均值。float KeyFrame::ComputeSceneMedianDepth(const int q) { vectorMapPoint * vpMapPoints; cv::Mat Tcw_; { unique_lockmutex lock(mMutexFeatures); unique_lockmutex lock2(mMutexPose); vpMapPoints mvpMapPoints; Tcw_ Tcw.clone(); } vectorfloat vDepths; vDepths.reserve(N); cv::Mat Rcw2 Tcw_.row(2).colRange(0, 3); Rcw2 Rcw2.t(); float zcw Tcw_.atfloat(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*x3Dwt)的第三行即z vDepths.push_back(z); } } sort(vDepths.begin(), vDepths.end()); return vDepths[(vDepths.size() - 1) / q]; }8. 修改建议KeyFrame与Frame的成员变量和功能有很大的相似性可以把KeyFrame作为Frame的一个来实现参考文献主要内容来自下文修改了一些描述增加了一些注释ORB SLAM2源码解读(四)KeyFrame类 - 古月居