关键帧与地图点（二）：关键帧

本次我们要讲解的是ORBSLAM2中的关键帧，首先我们来看一下论文中关于关键帧的相关描述：每个关键帧 K i K_i Ki存储了以下内容：

相机的位姿 T i w T_{iw} Tiw，注意这里是从相机到世界系的变换矩阵
相机内参，包括主点和焦距
在这一帧中提取到的所有的去畸变后的ORB特征

地图点和关键帧的创建条件较为宽松，但是之后则会通过一个非常严格苛刻的删选机制负责剔除冗余的关键帧和错匹配的或不可跟踪的地图点。

对应于代码则是

    // SE3位姿和相机光心的坐标cv::Mat Tcw;    // 当前相机的位姿，世界坐标系到相机坐标系cv::Mat Twc;    // 当前相机位姿的逆cv::Mat Ow;     // 相机光心(左目)在世界坐标系下的坐标,这里和普通帧中的定义是一样的cv::Mat Cw; ///< Stereo middel point. Only for visualization/// 关键帧观测到的地图点std::vector<MapPoint*> mvpMapPoints;// BoW词典KeyFrameDatabase* mpKeyFrameDB;// 视觉单词ORBVocabulary* mpORBvocabulary;// 用于加速特征匹配的网格 // 其实应该说是二维的,第三维的 vector中保存的是这个网格内的特征点的索引std::vector< std::vector <std::vector<size_t> > > mGrid;// 共视图// 与该关键帧连接（至少15个共视地图点）的关键帧与权重std::map<KeyFrame*,int> mConnectedKeyFrameWeights;   // 共视关键帧中权重从大到小排序后的关键帧          std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;            // 共视关键帧中从大到小排序后的权重，和上面对应std::vector<int> mvOrderedWeights;                             // ===================== 生成树和闭环边 ========================// std::set是集合，相比vector，进行插入数据这样的操作时会自动排序bool mbFirstConnection;                     // 是否是第一次生成树KeyFrame* mpParent;                         // 当前关键帧的父关键帧 （共视程度最高的）std::set<KeyFrame*> mspChildrens;           // 存储当前关键帧的子关键帧，这个一般不止一个std::set<KeyFrame*> mspLoopEdges;           // 和当前关键帧形成回环关系的关键帧// Bad flagsbool mbNotErase;            // 当前关键帧已经和其他的关键帧形成了回环关系，因此在各种优化的过程中不应该被删除bool mbToBeErased;          // 将要被删除的标志bool mbBad;                 // 关键帧为Bad的标志 float mHalfBaseline;        // 对于双目相机来说,双目相机基线长度的一半. Only for visualizationMap* mpMap;                 // 局部地图/// 在对位姿进行操作时相关的互斥锁std::mutex mMutexPose;/// 在操作当前关键帧和其他关键帧的共视关系的时候使用到的互斥锁std::mutex mMutexConnections;/// 在操作和特征点有关的变量的时候的互斥锁std::mutex mMutexFeatures;

接下来我们需要了解一下ORBSLAM2中对于关键帧的选择策略，要插入新的关键帧，以下条件必须满足：

距离上一次全局重定位要经过至少20帧
局部建图线程处于空闲状态，或者从上次插入关键帧起经过了至少20帧
当前帧跟踪了至少50个地图点
当前帧跟踪的地图点数少于参考关键帧 K r e f K_{ref} Kref地图点数量的90%

这里不使用与其他关键帧的距离标准作为判断是否插入关键帧的条件，而是使用视觉变化来判断（条件4），条件1确保良好的重定位，条件3确保良好的跟踪。如果在局部建图线程忙时插入关键帧（条件2的第二部分），则会发送一个信号来停止局部BA的进行，以便它能够尽快处理新的关键帧。

对应的代码为

判断是否需要插入关键帧

/*** @brief 判断当前帧是否需要插入关键帧* * Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧* Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧* Step 3：得到参考关键帧跟踪到的地图点数量* Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧* Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量* Step 6：决策是否需要插入关键帧* @return true         需要* @return false        不需要*/
bool Tracking::NeedNewKeyFrame()
{// Step 1：纯VO模式下不插入关键帧if(mbOnlyTracking)return false;// If Local Mapping is freezed by a Loop Closure do not insert keyframes// Step 2：如果局部地图线程被闭环检测使用，则不插入关键帧if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())return false;// 获取当前地图中的关键帧数目const int nKFs = mpMap->KeyFramesInMap();// Do not insert keyframes if not enough frames have passed from last relocalisation// mCurrentFrame.mnId是当前帧的ID// mnLastRelocFrameId是最近一次重定位帧的ID// mMaxFrames等于图像输入的帧率//  Step 3：如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames)                                     return false;// Tracked MapPoints in the reference keyframe// Step 4：得到参考关键帧跟踪到的地图点数量// UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 // 地图点的最小观测次数int nMinObs = 3;if(nKFs<=2)nMinObs=2;// 参考关键帧地图点中观测的数目>= nMinObs的地图点数目int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);// Local Mapping accept keyframes?// Step 5：查询局部地图线程是否繁忙，当前能否接受新的关键帧bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();// Check how many "close" points are being tracked and how many could be potentially created.// Step 6：对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧int nNonTrackedClose = 0;  //双目或RGB-D中没有跟踪到的近点int nTrackedClose= 0;       //双目或RGB-D中成功跟踪的近点（三维点）if(mSensor!=System::MONOCULAR){for(int i =0; i<mCurrentFrame.N; i++){// 深度值在有效范围内if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])nTrackedClose++;elsenNonTrackedClose++;}}}// 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了// 单目时，为falsebool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);// Step 7：决策是否需要插入关键帧// Step 7.1：设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧float thRefRatio = 0.75f;// 关键帧只有一帧，那么插入关键帧的阈值设置的低一点，插入频率较低if(nKFs<2)thRefRatio = 0.4f;//单目情况下插入关键帧的频率很高    if(mSensor==System::MONOCULAR)thRefRatio = 0.9f;// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion// Step 7.2：很长时间没有插入关键帧，可以插入const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle// Step 7.3：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);// Condition 1c: tracking is weak// Step 7.4：在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertCloseconst bool c1c =  mSensor!=System::MONOCULAR &&             //只考虑在双目，RGB-D的情况(mnMatchesInliers<nRefMatches*0.25 ||       //当前帧和地图点匹配的数目非常少bNeedToInsertClose) ;                     //需要插入// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.// Step 7.5：和参考帧相比当前跟踪到的点太少 或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);if((c1a||c1b||c1c)&&c2){// If the mapping accepts keyframes, insert keyframe.// Otherwise send a signal to interrupt BA// Step 7.6：local mapping空闲时可以直接插入，不空闲的时候要根据情况插入if(bLocalMappingIdle){//可以插入关键帧return true;}else{mpLocalMapper->InterruptBA();if(mSensor!=System::MONOCULAR){// 队列里不能阻塞太多关键帧// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，// 然后localmapper再逐个pop出来插入到mspKeyFramesif(mpLocalMapper->KeyframesInQueue()<3)//队列中的关键帧数目不是很多,可以插入return true;else//队列中缓冲的关键帧数目太多,暂时不能插入return false;}else//对于单目情况,就直接无法插入关键帧了//? 为什么这里对单目情况的处理不一样?//回答：可能是单目关键帧相对比较密集return false;}}else//不满足上面的条件,自然不能插入关键帧return false;
}

这里的判断条件比较复杂，有些变量的含义还没有讲到，可以等到之后再仔细理解

插入关键帧：

/*** @brief 创建新的关键帧* 对于非单目的情况，同时创建新的MapPoints* * Step 1：将当前帧构造成关键帧* Step 2：将当前关键帧设置为当前帧的参考关键帧* Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints*/
void Tracking::CreateNewKeyFrame()
{// 如果局部建图线程关闭了,就无法插入关键帧if(!mpLocalMapper->SetNotStop(true))return;// Step 1：将当前帧构造成关键帧KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// Step 2：将当前关键帧设置为当前帧的参考关键帧// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧mpReferenceKF = pKF;mCurrentFrame.mpReferenceKF = pKF;// 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同// Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作if(mSensor!=System::MONOCULAR){// 根据Tcw计算mRcw、mtcw和mRwc、mOwmCurrentFrame.UpdatePoseMatrices();// We sort points by the measured depth by the stereo/RGBD sensor.// We create all those MapPoints whose depth < mThDepth.// If there are less than 100 close points we create the 100 closest.// Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）vector<pair<float,int> > vDepthIdx;vDepthIdx.reserve(mCurrentFrame.N);// 遍历当前帧的地图点for(int i=0; i<mCurrentFrame.N; i++){// 地图点深度float z = mCurrentFrame.mvDepth[i];if(z>0){// 第一个元素是深度,第二个元素是对应的特征点的idvDepthIdx.push_back(make_pair(z,i));}}if(!vDepthIdx.empty()){// Step 3.2：按照深度从小到大排序sort(vDepthIdx.begin(),vDepthIdx.end());// Step 3.3：从中找出不是地图点的生成临时地图点 // 处理的近点的个数int nPoints = 0;for(size_t j=0; j<vDepthIdx.size();j++){// 地图点idint i = vDepthIdx[j].second;bool bCreateNew = false;// 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1){bCreateNew = true;mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);}// 如果需要就新建地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪if(bCreateNew){cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);// 这些添加属性的操作是每次创建MapPoint后都要做的pNewMP->AddObservation(pKF,i);pKF->AddMapPoint(pNewMP,i);pNewMP->ComputeDistinctiveDescriptors();pNewMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pNewMP);mCurrentFrame.mvpMapPoints[i]=pNewMP;nPoints++;}else{// 因为从近到远排序，记录其中不需要创建地图点的个数nPoints++;}// Step 3.4：停止新建地图点必须同时满足以下条件：// 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）// 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出if(vDepthIdx[j].first>mThDepth && nPoints>100)break;}}}// Step 4：插入关键帧// 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸mpLocalMapper->InsertKeyFrame(pKF);// 插入好了，允许局部建图停止mpLocalMapper->SetNotStop(false);// 当前帧成为新的关键帧，更新mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;
}

最后我们来看一下关键帧对应的比较重要的函数

更新连接关系

//KeyFrame.cc
KeyFrame::UpdateConnections()
{//省略...
// Step 5 更新生成树的连接
if(mbFirstConnection && mnId!=0)
{// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系,将当前关键帧作为其子关键帧
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
// 添加子关键帧(即和子关键帧具有最大共视关系的关键帧就是当前关键帧)
void KeyFrame::AddChild(KeyFrame *pKF)
{unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.insert(pKF);
}
// 删除某个子关键帧
void KeyFrame::EraseChild(KeyFrame *pKF)
{unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.erase(pKF);
}
// 改变当前关键帧的父关键帧
void KeyFrame::ChangeParent(KeyFrame *pKF)
{unique_lock<mutex> lockCon(mMutexConnections);
// 添加双向连接关系
mpParent = pKF;
pKF->AddChild(this);
}
//获取当前关键帧的子关键帧
set<KeyFrame*> KeyFrame::GetChilds()
{unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens;
}
//获取当前关键帧的父关键帧
KeyFrame* KeyFrame::GetParent()
{unique_lock<mutex> lockCon(mMutexConnections);
return mpParent;
}
// 判断某个关键帧是否是当前关键帧的子关键帧
bool KeyFrame::hasChild(KeyFrame *pKF)
{unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens.count(pKF);
}

更新局部关键帧

void Tracking::UpdateLocalKeyFrames()
{//省略...
// 策略2.2:将自己的子关键帧作为局部关键帧(将邻居的子孙们拉拢入伙)
const set<KeyFrame*> spChilds = pKF->GetChilds();
for(set<KeyFrame*>::const_iterator sit=spChilds.begin(),
send=spChilds.end(); sit!=send; sit++)
{KeyFrame* pChildKF = *sit;
if(!pChildKF->isBad())
{if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{mvpLocalKeyFrames.push_back(pChildKF);pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
//? 找到一个就直接跳出for循环?
break;
}
}
}
// 策略2.3:自己的父关键帧(将邻居的父母们拉拢入伙)
KeyFrame* pParent = pKF->GetParent();
if(pParent)
{// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
//! 感觉是个bug!如果找到父关键帧会直接跳出整个循环
break;
}
}
// 省略....
}

其中大部分代码之后讲到三大线程时候会详细讲

关键帧与地图点（二）：关键帧相关推荐

基于Autoware制作高精地图（二）
基于Autoware制作高精地图(二) 开始慢慢进入正轨了,后面的更新可能会慢些,大家见谅,这次我们来讲一讲Unity和开源工具Maptoolbox的安装. 在安装Unity前我同时安装了一个Unit ...
Unity使用Isometric Z As Y Tilemap创建2.5D地图（二）如何按照正确遮挡顺序渲染图片
Unity使用Isometric Z As Y Tilemap创建2.5D地图(二)如何按照正确遮挡顺序渲染图片如何按照正确遮挡顺序渲染图片 1.创建多层Tilemap 2.使用Sorting La ...
SuperMap、Cesium叠加ArcGIS,高德，谷歌二维，卫星地图实现二三维地图切换
先初始化GIS场景: var viewer = new Cesium.Viewer("cesiumContainer"); 1.加载高德二维地图 //高德二维地图自带路网注记 va ...
三维点云地图转二维栅格地图
文章目录前言一.安装octomap 二.安装map_server 三.发布.转换并保存前言三维点云地图转二维栅格地图的实现需要1.地图转换工具--octomap:2.栅格地图保存工具--map ...
Android百度地图（二）结合方向传感器我们自己定位哪里走
Android百度地图(二)结合方向传感器我们自己定位哪里走本文代码在http://blog.csdn.net/xyzz609/article/details/51943556的基础上进一步修改,有 ...
python视频提取关键帧_一种视频关键帧提取算法的制作方法
本发明属于信息安全技术领域,涉及视频内容信息的提取,具体来说,是一种视频关键帧提取算法. 背景技术: 随着Internet的应用和普及,多媒体信息检索系统对社会各领域产生越来越大的影响.传统的信息检索 ...
高德地图markevents_GitHub - mingxuWang/Map: 高德地图API二次封装
Map组件设计文档组件设计目的分析当前各业务方向(销售端.商城.数据可视化.TMS)内地图相关应用的地图功能使用情况,封装Map组件供给各业务向进行使用. 将高德地图API进行二次封装,降低地图相 ...
百度地图API二次开发小经验分享
最近在做一个物流后台系统,需要用地图来把订单地址展示出来,需要在地图上批量框选坐标进行排单,需要看到配送员的实时位置等等功能. 在高德地图.腾讯地图.百度地图三者间,我选了百度地图,没有原因,个人偏好 ...
[android] 百度地图开发 (二).定位城市位置和城市POI搜索
一. 百度地图城市定位和POI搜索知识上一篇文章"百度地图开发(一)"中讲述了如何申请百度APIKey及解决显示空白网格的问题.该篇文章主要讲述如何定位城市位置.定位 ...

关键帧与地图点（二）：关键帧

关键帧与地图点（二）：关键帧相关推荐

最新文章

热门文章