ORBSLAM2
关键帧
什么是关键帧?
通俗来说,关键帧就是几帧普通帧里面具有代表性的一帧。
为什么需要关键帧
1.相近帧之间信息冗余度很高,关键帧是取局部相近帧中最有代表性的一帧,可以降低信息冗余度。
2.关键帧选择时还会对图片质量、特征点质量等进行考察,在Bundle Fusion、RKD SLAM等RGB-D-SLAM相关方案中常常用普通帧的深度投影到关键帧上进行深度图优化,一定程度上关键帧是普通帧滤波和优化的结果,防止无用的或错误的信息进入优化过程而破坏定位建图的准确性。
3.如果所有帧全部参与计算,不仅浪费了算力,对内存也是极大的考验,这一点在前端vo中表现不明显,但在后端优化里是一个大问题,所以关键帧主要作用是面向后端优化的算力与精度的折中,使得有限的计算资源能够用在刀刃上,保证系统的平稳运行。假如你放松ORB_SLAM2 关键帧选择条件,大量产生的关键帧不仅耗计算资源,还会导致local mapping 计算不过来,出现误差累积
代码
bool Tracking::NeedNewKeyFrame()
{
if(mbOnlyTracking)//纯vo模式下不需要插入关键帧
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
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
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)//离重定位结果较近或者超出了关键帧的限制,就不插入关键帧
return false;
// Tracked MapPoints in the reference keyframe
int nMinObs = 3;//地图点最小观测数目
if(nKFs<=2)
nMinObs=2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);//参考关键帧中地图点的观测数目>=nminOBS的观测数目
// Local Mapping accept keyframes?
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();//局部建图线程是否繁忙??
// Check how many "close" points are being tracked and how many could be potentially created.
int nNonTrackedClose = 0;
int nTrackedClose= 0;
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++;
else
nNonTrackedClose++;
}
}
}
bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);//跟踪的地图点太少,或者没跟踪到的三维点太多,就可以插入关键帧,单目为false
// Thresholds
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
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;//当前帧的ID>上一个关键帧的ID+30,长时间没插入
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);//当前帧的ID>=上一关键帧+最小时间且局部建图线程是空闲状态的话
//Condition 1c: tracking is weak
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;//不是单目且当前帧和地图匹配点的数目非常少且需要插入,就插入
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);//当前帧的内点数比上参考帧的内点数,小于上面的阈值,并且 内点数要>15
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
if(bLocalMappingIdle)//局部建图线程是否空闲?
{
return true;
}
else
{
mpLocalMapper->InterruptBA();//不空闲的话要把局部建图线程的ba给关掉
if(mSensor!=System::MONOCULAR)//非单目?
{
if(mpLocalMapper->KeyframesInQueue()<3)//局部见图线程关键帧是否太少
return true;
else
return false;
}
else
return false;
}
}
else
return false;
}
创建关键帧
bool Tracking::NeedNewKeyFrame()
{
if(mbOnlyTracking)//纯vo模式下不需要插入关键帧
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
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
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)//离重定位结果较近或者超出了关键帧的限制,就不插入关键帧
return false;
// Tracked MapPoints in the reference keyframe
int nMinObs = 3;//地图点最小观测数目
if(nKFs<=2)
nMinObs=2;
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);//参考关键帧中地图点的观测数目>=nminOBS的观测数目
// Local Mapping accept keyframes?
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();//局部建图线程是否繁忙??
// Check how many "close" points are being tracked and how many could be potentially created.
int nNonTrackedClose = 0;
int nTrackedClose= 0;
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++;
else
nNonTrackedClose++;
}
}
}
bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);//跟踪的地图点太少,或者没跟踪到的三维点太多,就可以插入关键帧,单目为false
// Thresholds
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
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;//当前帧的ID>上一个关键帧的ID+30,长时间没插入
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);//当前帧的ID>=上一关键帧+最小时间且局部建图线程是空闲状态的话
//Condition 1c: tracking is weak
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;//不是单目且当前帧和地图匹配点的数目非常少且需要插入,就插入
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);//当前帧的内点数比上参考帧的内点数,小于上面的阈值,并且 内点数要>15
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
if(bLocalMappingIdle)//局部建图线程是否空闲?
{
return true;
}
else
{
mpLocalMapper->InterruptBA();//不空闲的话要把局部建图线程的ba给关掉
if(mSensor!=System::MONOCULAR)//非单目?
{
if(mpLocalMapper->KeyframesInQueue()<3)//局部见图线程关键帧是否太少
return true;
else
return false;
}
else
return false;
}
}
else
return false;
}