文章目录
- 一. 函数的意义
- 二、Tracking::NeedNewKeyFrame()
- 1. 函数讲解
- 2. 函数代码
- 三、Tracking::CreateNewKeyFrame()
- 1. 函数讲解
- 2. 函数代码
- 四、总结
一. 函数的意义
前面几篇文章讲过跟踪线程的核心问题–找到新来帧的位姿，这两个函数起到的作用是连接跟踪线程和局部建图线程，这两个线程靠关键帧来连接。这两个函数一个还判断是否需要创建关键帧，一个是创建新关键帧的函数，在前面的学习中关键帧可能没有发挥到太大的作用，但是在接下来的两个线程中，关键帧是两大研究的对线之一（两大研究对象是，地图点和关键帧）。所以他的作用及其重要，学习这两个函数，我们的目标是知道，什么情况需要新的关键帧，关键帧应具有什么样的特点。
二、Tracking::NeedNewKeyFrame()
1. 函数讲解
该函数分为以下几个步骤。本函数为一个有返回值的函数，如果返回值为true则白表示，需要插入关键帧，如果返回值为false则表示不需要插入关键帧。判断是否需要关键帧，首先我们要明白为什么要插入关键帧，是因为跟踪得不太好，需要插入一个关键帧，让结果更加可靠。所以这里有一个必要的条件，也就是代码总出现得c2，和参考关键帧相比跟踪得点太少，或者当前帧没跟踪到得点太多（跟踪到得小于100，没跟踪到得大于70）。然后就是判断能不能插入关键帧，c1a说很长时间没插入可以插入，因为很长时间没插入，时间太就前面得关键帧可能就不靠谱了，c1b说满足插入关键帧的最小间隔并且localMapper处于空闲状态可以插入，是因为这个时候不会对局部建图产生影响，在这种情况下可以尽量得使得位姿估计准确。c1c时双目和RGBD情况下得判断条件，他和c2其实说的是一件事情，只不过RGBD需要更苛刻得条件才会去插入关键帧。我们要知道得是双目和RGBD因为有深度信息，所以关键帧数量没有单目那么高。
- Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧
- Step 2：如果距离上一次重定位较近，或者关键帧数目超出最大限制，不插入关键帧
- Step 3：得到参考关键帧跟踪到的地图点数量
- Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
- Step 5：对于双目或RGBD，统计可以添加的有效地图点总数和跟踪到的地图点数量
- Step 6：决策是否需要插入关键帧
2. 函数代码
/**
* @brief 判断当前帧是否需要插入关键帧
* @return true 需要
* @return false 不需要
*/
bool Tracking::NeedNewKeyFrame()
{
// 仅定位模式不插入关键帧
if(mbOnlyTracking)
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;
// 如果关键帧数量小于等于2，则设定最小观测数量为2
if(nKFs<=2)
nMinObs=2;
// 获取参考关键帧地图点中观测的数目>= nMinObs的地图点数目
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
// Local Mapping accept keyframes?
// 查询局部地图线程是否繁忙，当前能否接受新的关键帧
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
// Check how many "close" points are being tracked and how many could be potentially created.
// 对于双目或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++;
else
nNonTrackedClose++;
}
}
}
// 如果跟踪到的近点数小于100，且没跟踪到大于70，则返回true
bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
// 决策是否需要插入关键帧
// Thresholds
// 设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧
float thRefRatio = 0.75f;
// 如果关键帧数量小于2，减小这个比例
if(nKFs<2)
thRefRatio = 0.4f;
// 单目情况设置设置为0.9
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;
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
// 满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
//Condition 1c: tracking is weak
// 在双目，RGB-D的情况下当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
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.
// 和参考帧相比当前跟踪到的点太少或者满足bNeedToInsertClose；同时跟踪到的内点还不能太少
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
// 满足c1a，c1b，c1c中的一个和c2
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
// 局部建图线程空闲就可以插入
if(bLocalMappingIdle)
{
return true;
}
else
{
// 发射一个中断BA的信号
mpLocalMapper->InterruptBA();
// 如果不是单目
if(mSensor!=System::MONOCULAR)
{
// 队列里不能阻塞太多关键帧
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
return true;
//队列中缓冲的关键帧数目太多,暂时不能插入
else
return false;
}
//对于单目情况,就直接无法插入关键帧了
//? 为什么这里对单目情况的处理不一样?
//回答：可能是单目关键帧相对比较密集
else
return false;
}
}
else
return false;
}
三、Tracking::CreateNewKeyFrame()
1. 函数讲解
这个函数是上一个函数得延申，如果上一个函数需要关键帧，那么这个函数就将当前帧构建为关键帧，因为新增了关键帧，那后面得帧完全可以以当前关键帧为当前参考关键帧（因为相对位移比上一个关键帧少），所以需要将当前关键帧设置为参考关键帧。最后一步就是，双目和RGBD情况时，我们因为有特征点的深度信息，完全可以把准确的特征点投影成地图点，这样可以为后面的位姿优化提供更多的约束条件，使误差更小，增加实时建图的精度。
2. 函数代码
/**
* @brief 创建新的关键帧
* 对于非单目的情况，同时创建新的MapPoints
*
* Step 1：将当前帧构造成关键帧
* Step 2：将当前关键帧设置为当前帧的参考关键帧
* Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
*/
void Tracking::CreateNewKeyFrame()
{
// 如果局部建图线程关闭了,就无法插入关键帧
if(!mpLocalMapper->SetNotStop(true))
return;
// 将当前帧创建为关键帧
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// 将当前关键帧设置为当前帧的参考关键帧
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;
// 双目和RGB的情况
if(mSensor!=System::MONOCULAR)
{
// 更新当前帧世界坐标
mCurrentFrame.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.
// 得到当前帧有深度值的特征点
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)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
if(!vDepthIdx.empty())
{
// 按照深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
int nPoints = 0;
// 从中找出不是地图点的包装为地图点
for(size_t j=0; j<vDepthIdx.size();j++)
{
int 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);
pNewMP->AddObservation(pKF,i);
pKF->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
nPoints++;
}
else
{
nPoints++;
}
// 停止新建地图点必须同时满足以下条件：
// 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
// 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
}
// 插入关键帧
mpLocalMapper->InsertKeyFrame(pKF);
// 插入好了，允许局部建图停止
mpLocalMapper->SetNotStop(false);
// 当前帧成为新的关键帧，更新
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
四、总结
这两个函数起到的是承上启下的作用，只有沈城关键帧，才会进入局部建图环节，是连接追踪线程和局部见图线程的函数。这两个函数中，第一个函数是重点，要了解什么时候需要关键帧，知道其中的原理，这个函数也就完全掌握了。到此追踪线程的主要函数就全部讲完了，接下来的文章将进入第二个线程—局部见图线程。如果对追踪线程有疑问或者发现问题，欢迎大家交流。
