/*
* @brief Main tracking function. It is independent of the input sensor.
*
* Tracking 线程
*/
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState 存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
// 上锁。保证地图不会发生变化
//? 疑问:这样子会不会影响地图的更新?
// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Step 1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
//双目RGBD相机的初始化共用一个函数
StereoInitialization();
else
//单目初始化
MonocularInitialization();
//更新帧绘制器中存储的最新状态
mpFrameDrawer->Update(this);
//这个状态量在上面的初始化函数中被更新
if(mState!=OK)
return;
}
// Step 2：跟踪
else
{
// System is initialized. Track Frame.
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
//SLAM模式
// 正常初始化成功
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
//由于追踪线程需要使用上一帧的信息,而局部建图线程则可能会对原有的地图点进行替换.在这里进行检查
CheckReplacedInLastFrame();
// step 2.1：跟踪上一帧或者参考帧或者重定位
// 运动模型是空的或刚完成重定位
// mCurrentFrame.mnId < mnLastRelocFrameId + 2 这个判断不应该有
// 应该只要 mVelocity 不为空，就优先选择 TrackWithMotionModel
// mnLastRelocFrameId 上一次重定位的那一帧
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
//对于上述两个条件我的理解:
//第一个条件,如果运动模型为空,那么就意味着之前已经..跟丢了
//第二个条件,就是如果当前帧紧紧地跟着在重定位的帧的后面,那么我们可以认为,通过在当前帧和发生重定位的帧(作为参考关键帧?)的基础上,
//我们可以恢复得到相机的位姿
// 将上一帧的位姿作为当前帧的初始位姿
// 通过BoW的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点都对应3D点重投影误差即可得到位姿
bOK = TrackReferenceKeyFrame();
}
else
{
// 根据恒速模型设定当前帧的初始位姿
// 通过投影的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点所对应3D点的投影误差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
// TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
// 最后通过优化得到优化后的位姿
//说白了就是根据恒速模型失败了.只能这样根据参考关键帧来了
bOK = TrackReferenceKeyFrame();
}
}
else
{
//如果正常的初始化不成功,那么就只能重定位了
// BOW搜索，PnP求解位姿
bOK = Relocalization();
}
}
else //如果现在处于定位模式
{
// Localization Mode: Local Mapping is deactivated
// 只进行跟踪tracking，局部地图不工作
// step 2.1：跟踪上一帧或者参考帧或者重定位
// tracking跟丢了, 那么就只能进行重定位了
if(mState==LOST)
{
bOK = Relocalization();
}
else //如果没有跟丢
{
// mbVO是mbOnlyTracking为true时的才有的一个变量
// mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常，
// mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏 - 233333
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
// mbVO为0则表明此帧匹配了很多的3D map点，非常好
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
// 这个地方是不是应该加上：
// if(!bOK)
// bOK = TrackReferenceKeyFrame();
//? 所以原作者为什么不加呢?
}
else
{
//如果恒速模型不被满足,那么就只能够通过参考关键帧来定位了
bOK = TrackReferenceKeyFrame();
}
}
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
// mbVO为1，则表明此帧匹配了很少的3D map点，少于10个，要跪的节奏，既做跟踪又做定位
//MM=Motion Model,通过运动模型进行跟踪的结果
bool bOKMM = false;
//通过重定位方法来跟踪的结果
bool bOKReloc = false;
//运动模型中构造的地图点
vector<MapPoint*> vpMPsMM;
//在追踪运动模型后发现的外点
vector<bool> vbOutMM;
//运动模型得到的位姿
cv::Mat TcwMM;
//当运动模型非空的时候,根据运动模型计算位姿
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
// 这三行没啥用？
//? 这三行中的内容会在上面的函数中被更新吗?
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
//使用重定位的方法来得到当前帧的位姿
bOKReloc = Relocalization();
// 重定位没有成功，但是如果跟踪成功
if(bOKMM && !bOKReloc)
{
// 这三行没啥用？
//? 这里为什么说没啥用呢?
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
//如果重定位本身要跪了
if(mbVO)
{
// 这段代码是不是有点多余？应该放到 TrackLocalMap 函数中统一做
// 更新当前帧的MapPoints被观测程度
for(int i =0; i<mCurrentFrame.N; i++)
{
//如果这个特征点形成了地图点,并且也不是外点的时候
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
//增加被观测的次数
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}//增加当前可视地图点的被观测次数
}//如果重定位要跪了
}//如果重定位没有成功,但是跟踪成功了
else if(bOKReloc)// 只要重定位成功整个跟踪过程正常进行（定位与跟踪，更相信重定位）
{
mbVO = false;
}
//有一个成功我们就认为执行成功了
bOK = bOKReloc || bOKMM;
}//上一次的结果告诉我们本帧要跪了
}//判断是否跟丢
}//判断是否处于定位模式
// 将最新的关键帧作为reference frame
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
// step 2.2：在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// NOTICE local map:当mpReferenceKF前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
//? 没有其他帧的地图点mpReferenceKF吗?
// 在步骤2.1中主要是两mpReferenceKF两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
// 然后将局部MapPointsmpReferenceKF和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
//如果前面的两两帧匹配过程进行得不好,这里也就直接放弃了
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
//根据上面的操作来判断是否追踪成功
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer 中的帧副本的信息
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
//只有在成功追踪时才考虑生成关键帧的问题
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// step 2.3：更新恒速运动模型 TrackWithMotionModel 中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
//? 这个是转换成为了相机相对世界坐标系的旋转?
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是 Tcl
}
else
//否则生成空矩阵
mVelocity = cv::Mat();
//相当于更新了地图绘制器
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
// step 2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
//?
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
// 排除 UpdateLastFrame 函数中为了跟踪增加的MapPoints
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// step 2.5：清除临时的MapPoints，这些MapPoints在 TrackWithMotionModel 的 UpdateLastFrame 函数里生成（仅双目和rgbd）
// 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
//? 为什么生成这些可以提高帧间跟踪效果?
//? 为什么单目就不能够生成地图点呢
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
//不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// step 2.6：检测并插入关键帧，对于双目会产生新的MapPoints
// NOTICE 在关键帧的时候生成地图点
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// step 2.7 删除那些在bundle adjustment中检测为outlier的3D map点
for(int i=0; i<mCurrentFrame.N;i++)
{
//这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟踪失败，并且relocation也没有搞定，只能重新Reset
if(mState==LOST)
{
//如果地图中的关键帧信息过少的话甚至直接重新进行初始化了
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
//确保已经设置了参考关键帧
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一帧的数据,当前帧变上一帧
mLastFrame = Frame(mCurrentFrame);
}
// Store frame pose information to retrieve the complete camera trajectory afterwards.
// step 3：记录位姿信息，用于轨迹复现
if(!mCurrentFrame.mTcw.empty())
{
// 计算相对姿态T_currentFrame_referenceKeyFrame
//这里的关键帧存储的位姿,表示的也是从参考关键帧的相机坐标系到世界坐标系的变换
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
//保存各种状态
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
// 如果跟踪失败，则相对位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}// Tracking
void Tracking::CheckReplacedInLastFrame()
/*
* @brief 检查上一帧中的MapPoints是否被替换
*
* Local Mapping线程可能会将关键帧中某些MapPoints进行替换，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints
* @see LocalMapping::SearchInNeighbors()
*/
//? 目测会发生替换,是因为合并三角化之后特别近的点吗?
void Tracking::CheckReplacedInLastFrame()
{
for(int i =0; i<mLastFrame.N; i++)
{
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
//如果这个地图点存在
if(pMP)
{
//获取其是否被替换,以及替换后的点
// 这也是程序不选择间这个地图点删除的原因，因为删除了就。。。段错误了
MapPoint* pRep = pMP->GetReplaced();
if(pRep)
{
//然后重设一下
mLastFrame.mvpMapPoints[i] = pRep;
}
}
}
}
MapPoint* MapPoint::GetReplaced()
{
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
return mpReplaced;
}
Tracking::TrackWithMotionModel()
/**
* @brief 根据匀速度模型对上一帧的MapPoints进行跟踪
*
* 1. 非单目情况，需要对上一帧产生一些新的MapPoints（临时） (因为传感器的原因，单目情况下仅仅凭借一帧没法生成可靠的地图点)
* 2. 将上一帧的MapPoints投影到当前帧的图像平面上，在投影的位置进行区域匹配 NOTICE 加快了匹配的速度
* 3. 根据匹配对估计当前帧的姿态
* 4. 根据姿态剔除误匹配
* @return 如果匹配数大于10，返回true
* @see V-B Initial Pose Estimation From Previous Frame
*/
bool Tracking::TrackWithMotionModel()
{
// 最小距离 < 0.9*次小距离 匹配成功
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
// Create "visual odometry" points
// step 1：更新上一帧的位姿；对于双目或rgbd摄像头，还会根据深度值为上一关键帧生成新的MapPoints
// （跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围）
// 在跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少
// 这个函数的功能就是补充增加RGBD和双目相机上一帧的MapPoints数
// 那么单目的时候呢? - 单目的时候就不加呗，单目的时候只计算上一帧相对于世界坐标系的位姿
// ？因为这里来看，计算的位姿是相对于参考关键帧的
UpdateLastFrame();
// 根据Const Velocity Model( NOTICE 认为这两帧之间的相对运动和之前两帧间相对运动相同)估计当前帧的位姿
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
//清空当前帧的地图点
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame
//这个阈值和下面的的跟踪有关,表示了匹配过程中的搜索半径
int th;
if(mSensor!=System::STEREO)
th=15;
else
th=7;
// step 2：根据匀速度模型进行对上一帧的MapPoints进行跟踪, 根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围
//我觉的这个才是使用恒速模型的根本目的
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search
// 如果跟踪的点少，则扩大搜索半径再来一次
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th
}
//如果就算是这样还是不能够获得足够的跟踪点,那么就认为运动跟踪失败了.
if(nmatches<20)
return false;
// Optimize frame pose with all matches
// step 3：优化位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// step 4：优化位姿后剔除outlier的mvpMapPoints,这个和前面相似
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
if(mCurrentFrame.mvbOutlier[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
//累加成功匹配到的地图点数目
nmatchesMap++;
}
}
if(mbOnlyTracking)
{
//如果在纯定位过程中追踪的地图点非常少,那么这里的 mbVO 标志就会置位
mbVO = nmatchesMap<10;
return nmatches>20;
}
return nmatchesMap>=10;
}
/**
* @brief 双目或rgbd摄像头根据深度值为上一帧产生新的MapPoints
*
* 在双目和rgbd情况下，选取一些深度小一些的点（可靠一些） \n
* 可以通过深度值产生一些新的MapPoints,用来补充当前视野中的地图点数目,这些新补充的地图点就被称之为"临时地图点""
*/
void Tracking::UpdateLastFrame()
{
// Update pose according to reference keyframe
// step 1：更新最近一帧的位姿
KeyFrame* pRef = mLastFrame.mpReferenceKF; //上一帧的参考KF
// ref_keyframe 到 lastframe的位姿
cv::Mat Tlr = mlRelativeFramePoses.back();
// 单目的时候，相当于只是完成了将上一帧的世界坐标系下的位姿计算出来
mLastFrame.SetPose(Tlr*pRef->GetPose()); // Tlr*Trw = Tlw l:last r:reference w:world
// 如果上一帧为关键帧，或者单目的情况，则退出
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
return;
// step 2：对于双目或rgbd摄像头，为上一帧临时生成新的MapPoints
// 注意这些MapPoints不加入到Map中，在tracking的最后会删除
// 跟踪过程中需要将将上一帧的MapPoints投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配
// Create "visual odometry" MapPoints
// We sort points according to their measured depth by the stereo/RGB-D sensor
// step 2.1：得到上一帧有深度值的特征点
//第一个元素是某个点的深度,第二个元素是对应的特征点id
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mLastFrame.N);
for(int i=0; i<mLastFrame.N;i++)
{
float z = mLastFrame.mvDepth[i];
if(z>0)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
//如果上一帧中没有有效深度的点,那么就直接退出了
if(vDepthIdx.empty())
return;
// step 2.2：按照深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
// We insert all close points (depth<mThDepth)
// If less than 100 close points, we insert the 100 closest ones.
// step 2.3：将距离比较近的点包装成MapPoints
int nPoints = 0;
for(size_t j=0; j<vDepthIdx.size();j++)
{
int i = vDepthIdx[j].second;
bool bCreateNew = false;
//如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
if(!pMP)
bCreateNew = true;
else if(pMP->Observations()<1) //? 从地图点被创建后就没有观测到,意味这是在上一帧中新添加的地图点吗
{
bCreateNew = true;
}
//如果需要创建新的临时地图点
if(bCreateNew)
{
// 这些生UpdateLastFrameints后并没有通过：
// a.AddMaUpdateLastFrame、
// b.AddObUpdateLastFrameion、
// c.CompuUpdateLastFrameinctiveDescriptors、
// d.UpdatUpdateLastFramelAndDepth添加属性，
// 这些MapPoint仅仅为了提高双目和RGBD的跟踪成功率 -- 我觉得可以这么说是因为在临时地图中增加了地图点，能够和局部地图一并进行定位工作
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(
x3D, //该点对应的空间点坐标
mpMap, //? 不明白为什么还要有这个参数
&mLastFrame, //存在这个特征点的帧(上一帧)
i); //特征点id
//? 上一帧在处理结束的时候,没有进行添加的操作吗?
mLastFrame.mvpMapPoints[i]=pNewMP; // 添加新的MapPoint
// 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
mlpTemporalPoints.push_back(pNewMP);
nPoints++;
}
else//如果不需要创建新的 临时地图点
{
nPoints++;
}
//当当前的点的深度已经超过了远点的阈值,并且已经这样处理了超过100个点的时候,说明就足够了
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
Tracking::TrackReferenceKeyFrame()
/*
* @brief 对参考关键帧的MapPoints进行跟踪
*
* 1. 计算当前帧的词包，将当前帧的特征点分到特定层的nodes上
* 2. 对属于同一node的描述子进行匹配
* 3. 根据匹配对估计当前帧的姿态
* 4. 根据姿态剔除误匹配
* @return 如果匹配数大于10，返回true
*/
bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
// step 1：将当前帧的描述子转化为BoW向量
mCurrentFrame.ComputeBoW();
// We perform first an ORB matching with the reference keyframe
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.7,true);
vector<MapPoint*> vpMapPointMatches;
// step 2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
//NOTICE 之前师兄说的，通过词袋模型加速匹配就是在这里哇
// 特征点的匹配关系由MapPoints进行维护
int nmatches = matcher.SearchByBoW(
mpReferenceKF, //参考关键帧
mCurrentFrame, //当前帧
vpMapPointMatches); //存储匹配关系
//这里的匹配数超过15才继续
if(nmatches<15)
return false;
// step 3:将上一帧的位姿态作为当前帧位姿的初始值
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
// step 4:通过优化3D-2D的重投影误差来获得位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// step 5：剔除优化后的outlier匹配点（MapPoints）
//之所以在优化之后才剔除外点，是因为在优化的过程中就有了对这些外点的标记
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
//如果对应到的某个特征点是外点
if(mCurrentFrame.mvbOutlier[i])
{
//清除它在当前帧中存在过的痕迹
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
//其实这里--也没有什么用了,因为后面也用不到它了
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
//匹配的内点计数++
nmatchesMap++;
}
}
return nmatchesMap>=10;
}
/*
* @brief 通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配
* @details 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） \n
* 对属于同一node的特征点通过描述子距离进行匹配 \n
* 根据匹配，更新vpMatches12 \n
* 通过距离阈值、比例阈值和角度投票进行剔除误匹配
* @param pKF1 KeyFrame1
* @param pKF2 KeyFrame2
* @param vpMatches12 pKF2中与pKF1匹配的MapPoint，null表示没有匹配
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
{
// 详细注释可参见：SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
const vector<cv::KeyPoint> &vKeysUn1 = pKF1->mvKeysUn;
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
const cv::Mat &Descriptors1 = pKF1->mDescriptors;
const vector<cv::KeyPoint> &vKeysUn2 = pKF2->mvKeysUn;
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
const cv::Mat &Descriptors2 = pKF2->mDescriptors;
// 保存匹配结果
vpMatches12 = vector<MapPoint*>(vpMapPoints1.size(),static_cast<MapPoint*>(NULL));
vector<bool> vbMatched2(vpMapPoints2.size(),false);
// 旋转直方图
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;
int nmatches = 0;
DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
while(f1it != f1end && f2it != f2end)
{
if(f1it->first == f2it->first)//步骤1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
{
// 步骤2：遍历KF中属于该node的特征点
for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
{
const size_t idx1 = f1it->second[i1];
MapPoint* pMP1 = vpMapPoints1[idx1];
if(!pMP1)
continue;
if(pMP1->isBad())
continue;
const cv::Mat &d1 = Descriptors1.row(idx1);
int bestDist1=256;
int bestIdx2 =-1 ;
int bestDist2=256;
// 步骤3：遍历KF2中属于该node的特征点，找到了最佳匹配点
for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
{
const size_t idx2 = f2it->second[i2];
MapPoint* pMP2 = vpMapPoints2[idx2];
// 如果已经有匹配的点，或者遍历到的特征点对应的地图点无效
if(vbMatched2[idx2] || !pMP2)
continue;
if(pMP2->isBad())
continue;
const cv::Mat &d2 = Descriptors2.row(idx2);
int dist = DescriptorDistance(d1,d2);
if(dist<bestDist1)
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdx2=idx2;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// 步骤4：根据阈值 和 角度投票剔除误匹配
// 详见SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)函数步骤4
if(bestDist1<TH_LOW)
{
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
vpMatches12[idx1]=vpMapPoints2[bestIdx2];
vbMatched2[bestIdx2]=true;
if(mbCheckOrientation)
{
float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(idx1);
}
nmatches++;
}
}
}
f1it++;
f2it++;
}
else if(f1it->first < f2it->first)
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else
{
f2it = vFeatVec2.lower_bound(f1it->first);
}
}
// 旋转检查
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMatches12[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
return nmatches;
}
Tracking::Relocalization()
/**
* @details 重定位过程
*
* Step 1：计算当前帧特征点的Bow映射
*
* Step 2：找到与当前帧相似的候选关键帧
*
* Step 3：通过BoW进行匹配
*
* Step 4：通过EPnP算法估计姿态
*
* Step 5：通过PoseOptimization对姿态进行优化求解
*
* Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
*/
bool Tracking::Relocalization()
{
// Compute Bag of Words Vector
// Step 1： 计算当前帧特征点的Bow映射
mCurrentFrame.ComputeBoW();
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
// Step 2：找到与当前帧相似的候选关键帧
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
//如果没有候选关键帧，则退出
if(vpCandidateKFs.empty())
return false;
const int nKFs = vpCandidateKFs.size();
// We perform first an ORB matching with each candidate
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.75,true);
//每个关键帧的解算器
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
//每个关键帧和当前帧中特征点的匹配关系
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
//放弃某个关键帧的标记
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
//有效的候选关键帧数目
int nCandidates=0;
//遍历所有的候选关键帧
for(int i=0; i<nKFs; i++)
{
KeyFrame* pKF = vpCandidateKFs[i];
if(pKF->isBad())
//? 前面的查找候选关键帧的时候,为什么不会检查一下这个呢? 为什么非要返回bad的关键帧呢? 关键帧为bad意味着什么呢?
//? 此外这个变量的初始值也不一定全部都是false吧
vbDiscarded[i] = true;
else
{
// Step 3：通过BoW进行匹配
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
//如果和当前帧的匹配数小于15,那么只能放弃这个关键帧
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else
{
// 初始化PnPsolver
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(
0.99, //用于计算RANSAC迭代次数理论值的概率
10, //最小内点数, NOTICE 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
300, //最大迭代次数
4, //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
0.5, //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
5.991); // 目测是自由度为2的卡方检验的阈值,作为内外点判定时的距离的baseline(程序中还会根据特征点所在的图层对这个阈值进行缩放的)
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}//遍历所有的候选关键帧,确定出满足进一步要求的候选关键帧并且为其创建pnp优化器
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
//? 这里的 P4P RANSAC 是啥意思啊 @lishuwei0424:我认为是Epnp，每次迭代需要4个点
//是否已经找到相匹配的关键帧的标志
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
//通过一系列骚操作,直到找到能够进行重定位的匹配上的关键帧
while(nCandidates>0 && !bMatch)
{
//遍历当前所有的候选关键帧
for(int i=0; i<nKFs; i++)
{
//如果刚才已经放弃了,那么这里也放弃了
if(vbDiscarded[i])
continue;
// Perform 5 Ransac Iterations
//内点标记
vector<bool> vbInliers;
//内点数
int nInliers;
// 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。
bool bNoMore;
// Step 4：通过EPnP算法估计姿态
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
// 如果这里的迭代已经尽力了。。。
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
Tcw.copyTo(mCurrentFrame.mTcw);
//成功被再次找到的地图点的集合,其实就是经过RANSAC之后的内点
set<MapPoint*> sFound;
const int np = vbInliers.size();
//遍历所有内点
for(int j=0; j<np; j++)
{
if(vbInliers[j])
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
// Step 5：通过PoseOptimization对姿态进行优化求解
//只优化位姿,不优化地图点的坐标;返回的是内点的数量
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// ? 如果优化之后的内点数目不多,注意这里是直接跳过了本次循环,但是却没有放弃当前的这个关键帧
if(nGood<10)
continue;
//删除外点对应的地图点
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
// If few inliers, search by projection in a coarse window and optimize again
// Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
// 前面的匹配关系是用词袋匹配过程得到的
if(nGood<50)
{
int nadditional =matcher2.SearchByProjection(
mCurrentFrame, //当前帧
vpCandidateKFs[i], //关键帧
sFound, //已经找到的地图点集合
10, //窗口阈值
100); //ORB描述子距离
//如果通过投影过程获得了比较多的特征点
if(nadditional+nGood>=50)
{
//根据投影匹配的结果，采用3D-2D pnp非线性优化求解
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
//如果这样依赖内点数还是比较少的话,就使用更小的窗口搜索投影点;由于相机位姿已经使用了更多的点进行了优化,所以可以认为使用更小的窗口搜索能够取得意料之内的效果
//我觉得可以理解为 垂死挣扎 2333
if(nGood>30 && nGood<50)
{
//重新进行搜索
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(
mCurrentFrame, //当前帧
vpCandidateKFs[i], //候选的关键帧
sFound, //已经找到的地图点
3, //新的窗口阈值
64); //ORB距离?
// Final optimization
if(nGood+nadditional>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
//更新地图点
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
//如果还是不能够满足就放弃了
}//如果地图点还是比较少的话
}//如果通过投影过程获得了比较多的特征点
}//如果内点较少,那么尝试通过投影关系再次进行匹配
// If the pose is supported by enough inliers stop ransacs and continue
//如果对于当前的关键帧已经有足够的内点(50个)了,那么就认为当前的这个关键帧已经和当前帧匹配上了
if(nGood>=50)
{
bMatch = true;
break;
}
}//遍历所有的候选关键帧
// ? 大哥，这里PnPSolver 可不能够保证一定能够得到相机位姿啊？怎么办？
}//一直运行,知道已经没有足够的关键帧,或者是已经有成功匹配上的关键帧
}
//折腾了这么久还是没有匹配上
if(!bMatch)
{
return false;
}
else
{
//如果匹配上了,说明当前帧重定位成功了(也就是在上面的优化过程中,当前帧已经拿到了属于自己的位姿).因此记录当前帧的id
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
}//重定位
Tracking::TrackLocalMap()
/**
* @brief 对Local Map的MapPoints进行跟踪
* Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
* Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪
* Step 3：更新局部所有MapPoints后对位姿再次优化
* Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
* Step 5：根据跟踪匹配数目及回环情况决定是否跟踪成功
* @return true 跟踪成功
* @return false 跟踪失败
*/
bool Tracking::TrackLocalMap()
{
// We have an estimation of the camera pose and some map points tracked in the frame.
// We retrieve the local map and try to find matches to points in the local map.
// Update Local KeyFrames and Local Points
// Step 1：更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPoints
UpdateLocalMap();
// Step 2：在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪
SearchLocalPoints();
// Optimize Pose
// 在这个函数之前，在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化，
// Step 3：更新局部所有MapPoints后对位姿再次优化
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
// Update MapPoints Statistics
// Step 4：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
// 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
if(!mCurrentFrame.mvbOutlier[i])
{
// 找到该点的帧数mnFound 加 1
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
//查看当前是否是在纯定位过程
if(!mbOnlyTracking)
{
// 该MapPoint被观测次数大于，就将mnMatchesInliers加1
//? 地图点被观测次数nObs 和 找到该点的帧数mnFound区别是什么?
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
// 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
mnMatchesInliers++;
}
//如果这个地图点是外点,并且当前相机输入还是双目的时候,就删除这个点
//?如果是其他类型的输入不管了?
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
// Decide if the tracking was succesful
// More restrictive if there was a relocalization recently
// Step 5：根据跟踪匹配数目及回环情况决定是否跟踪成功
//如果最近刚刚发生了重定位,那么至少跟踪上了50个点我们才认为是跟踪上了
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
return false;
//如果是正常的状态话只要跟踪的地图点大于30个我们就认为成功了
if(mnMatchesInliers<30)
return false;
else
return true;
}
/**
* @brief 更新局部地图 LocalMap
*
* 局部地图包括：共视关键帧、临近关键帧及其子父关键帧，由这些关键帧观测到的MapPoints
*/
void Tracking::UpdateLocalMap()
{
// This is for visualization
// 设置参考地图点用于绘图显示局部地图点（红色）
mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
// Update
// 更新局部关键帧和局部MapPoints
UpdateLocalKeyFrames();
UpdateLocalPoints();
}
/**
* @brief 对 Local MapPoints 进行跟踪
* 在局部地图中查找在当前帧视野范围内的点，将视野范围内的点和当前帧的特征点进行投影匹配
* Step 1：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索
* Step 2：将所有 局部MapPoints 投影到当前帧，判断是否在视野范围内
* Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配
*/
void Tracking::SearchLocalPoints()
{
// Do not search map points already matched
// Step 1：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索
// 因为当前的mvpMapPoints一定在当前帧的视野中
for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(pMP->isBad())
{
*vit = static_cast<MapPoint*>(NULL);
}
else
{
// 更新能观测到该点的帧数加1(被当前帧看到了)
pMP->IncreaseVisible();
// 标记该点被当前帧观测到
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
// 标记该点将来不被投影，因为已经匹配过(指的是使用恒速运动模型进行投影)
pMP->mbTrackInView = false;
}
}
}//遍历当前帧中所有的地图点
//准备进行投影匹配的点的数目
int nToMatch=0;
// Project points in frame and check its visibility
// Step 2：将所有 局部MapPoints 投影到当前帧，判断是否在视野范围内
for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
// 已经被当前帧观测到的MapPoint不再需要判断是否能被当前帧观测到
if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
continue;
//跳过局部地图中的坏点
if(pMP->isBad())
continue;
// Project (this fills MapPoint variables for matching)
// 判断LocalMapPoints中的点是否在在视野内
if(mCurrentFrame.isInFrustum(pMP,0.5))
{
// 观测到该点的帧数加1，该MapPoint在某些帧的视野范围内
pMP->IncreaseVisible();
// 只有在视野范围内的MapPoints才参与之后的投影匹配
nToMatch++;
}
}
// Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配
if(nToMatch>0)
{
ORBmatcher matcher(0.8);
int th = 1;
if(mSensor==System::RGBD) //RGBD相机输入的时候,搜索的阈值会变得稍微大一些
th=3;
// If the camera has been relocalised recently, perform a coarser search
// 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
th=5;
// 对视野范围内的MapPoints通过投影进行特征点匹配
matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
}
}
/*
* @brief 判断当前帧是否为关键帧
* @return true if needed
*/
bool Tracking::NeedNewKeyFrame()
{
// ?step 1：如果用户在界面上选择重定位，那么将不插入关键帧
// 由于插入关键帧过程中会生成MapPoint，因此用户选择重定位后地图上的点云和关键帧都不会再增加
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
// step 2：判断是否距离上一次插入关键帧的时间太短
// mCurrentFrame.mnId是当前帧的ID
// mnLastRelocFrameId是最近一次重定位帧的ID
// mMaxFrames等于图像输入的帧率
// 如果关键帧比较少，则考虑插入关键帧
// 或距离上一次重定位超过1s，则考虑插入关键帧
if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && //距离上一次重定位不超过1s
nKFs>mMaxFrames) //地图中的关键帧已经足���
return false;
// Tracked MapPoints in the reference keyframe
// step 3：得到参考关键帧跟踪到的MapPoints数量
// NOTICE 在 UpdateLocalKeyFrames 函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧 -- 一般的参考关键帧的选择原则
//地图点的最小观测次数
int nMinObs = 3;
if(nKFs<=2)
nMinObs=2;
//获取地图点的数目, which 参考帧观测的数目大于等于 nMinObs
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
// Local Mapping accept keyframes?
// step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();pan
// "total matches = matches to map + visual odometry matches"
// Visual odometry matches will become MapPoints if we insert a keyframe.
// This ratio measures how many MapPoints we could create if we insert a keyframe.
// step 5：对于双目或RGBD摄像头，统计 总的可以添加的MapPoints数量 和 跟踪到地图中的MapPoints数量
int nMap = 0; //现有地图中,可以被关键帧观测到的地图点数目
int nTotal= 0; //当前帧中可以添加到地图中的地图点数量
if(mSensor!=System::MONOCULAR)// 双目或rgbd
{
for(int i =0; i<mCurrentFrame.N; i++)
{
//如果是近点,并且这个特征点的深度合法,就可以被添加到地图中
if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
{
nTotal++;// 总的可以添加mappoints数
if(mCurrentFrame.mvpMapPoints[i])
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nMap++;// 被关键帧观测到的mappoints数，即观测到地图中的MapPoints数量
}
}
}
else
{
// There are no visual odometry matches in the monocular case
//? 提问:究竟什么才是 visual odometry matches ?
nMap=1;
nTotal=1;
}
//计算这个比例,当前帧中观测到的地图点数目和当前帧中总共的地图点数目之比.这个值越接近1越好,越接近0说明跟踪上的地图点太少,tracking is weak
const float ratioMap = (float)nMap/(float)(std::max(1,nTotal));
// step 6：决策是否需要插入关键帧
// Thresholds
// 设定inlier阈值，和之前帧特征点匹配的inlier比例
float thRefRatio = 0.75f;
if(nKFs<2)
thRefRatio = 0.4f;// 关键帧只有一帧，那么插入关键帧的阈值设置很低 //? 这句话不应该放在下面这句话的后面吗?
if(mSensor==System::MONOCULAR)
thRefRatio = 0.9f;//单目情况下插入关键帧的阈值很高
// MapPoints中和地图关联的比例阈值
float thMapRatio = 0.35f;
if(mnMatchesInliers>300)
thMapRatio = 0.20f;
// 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
// 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
const bool c1c = mSensor!=System::MONOCULAR && //只有在双目的时候才成立
(mnMatchesInliers<nRefMatches*0.25 || //和地图点匹配的数目非常少
ratioMap<0.3f) ; //地图点跟踪成功的比例非常小,要挂了
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
// 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || // 总的来说,还是参考关键帧观测到的地图点的数目太少,少于给定的阈值
ratioMap<thMapRatio) && // 追踪到的地图点的数目比例太少,少于阈值
mnMatchesInliers>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();
if(mSensor!=System::MONOCULAR)
{
// 队列里不能阻塞太多关键帧
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
//队列中的关键帧数目不是很多,可以插入
return true;
else
//队列中缓冲的关键帧数目太多,暂时不能插入
return false;
}
else
//对于单目情况,就直接无法插入关键帧了
//? 为什么这里对单目情况的处理不一样?
return false;
}
}
else
//不满足上面的条件,自然不能插入关键帧
return false;
}
/**
* @brief 创建新的关键帧
*
* 对于非单目的情况，同时创建新的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;
// 这段代码和 UpdateLastFrame 中的那一部分代码功能相同
// step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
if(mSensor!=System::MONOCULAR)
{
// 根据Tcw计算mRcw、mtcw和mRwc、mOw
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.
// step 3.1：得到当前帧深度小于阈值的特征点
// 创建新的MapPoint, depth < mThDepth
//第一个元素是深度,第二个元素是对应的特征点的id
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())
{
// step 3.2：按照深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
// step 3.3：将距离比较近的点包装成MapPoints
//处理的近点的个数
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);
// 这些添加属性的操作是每次创建MapPoint后都要做的
pNewMP->AddObservation(pKF,i);
pKF->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
nPoints++;
}
else
{
//? 这里?????
nPoints++;
}
// 这里决定了双目和rgbd摄像头时地图点云的稠密程度
// 但是仅仅为了让地图稠密直接改这些不太好，
// 因为这些MapPoints会参与之后整个slam过程
//当当前处理的点大于深度阈值或者已经处理的点超过阈值的时候,就不再进行了
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
}
//执行插入关键帧的操作,其实也是在列表中等待
mpLocalMapper->InsertKeyFrame(pKF);
//然后现在允许局部建图器停止了
mpLocalMapper->SetNotStop(false);
//当前帧成为新的关键帧
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
REFERENCES
ORB_SLAM2代码阅读(1)——系统入口: https://blog.csdn.net/u014709760/article/details/87922386
ORB_SLAM2代码阅读(2)——tracking线程 https://blog.csdn.net/u014709760/article/details/89465842#221_Tracking__16
![双目相机标定和orbslam2双目参数详解[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)