ORB-SLAM2源码——Tracking.cc
这一部分的源码主要涉及的是跟踪线程相关的部分。
1.2. CreateNewKeyFrame 创建新的关键帧并新增真正地图点
当完成第二阶段跟踪后，清除这些临时地图点。
第二阶段跟踪结束后新建关键帧时，针对双目相机或RGB-D相机，找出当前帧中具有有效深度值且不是地图点的特征点，将其中较近的点作为当前帧的新的地图点。与函数Tracking::UpdateLastFrame()功能类似，不同之处是这里添加的是真正的地图点，是全局地图中新建地图点，用于跟踪。会添加地图点和关键帧的相互观测和属性信息，如最佳描述子、平均观测方向、观测距离范围，所以在每次创建MapPoint后都要进行添加属性的操作。
步骤：
- Step 1：将当前帧构造成关键帧
- Step 2：将当前关键帧设置为当前帧的参考关键帧
- Step 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
/**
* @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;
// 这段代码和 Tracking::UpdateLastFrame 中的那一部分代码功能相同
// Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作
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：得到当前帧有深度值的特征点（不一定是地图点）
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)
{
// 第一个元素是深度,第二个元素是对应的特征点的id
vDepthIdx.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++)
{
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++;
}
// 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;
}
2. 关键帧
2.1. 关键帧的定义：
关键帧是SLAM 中常用的一种选择图像帧的策略，一般称普通帧为非关键帧。
关键帧主要有如下几个作用：
- 关键帧可以降低信息冗余度。
- 关键帧可以降低计算负担，减少误差累积。
- 关键帧可以保证图像帧的质量。
2.2. 如何选择关键帧
选择关键帧主要从关键帧自身和关键帧与其他关键帧的关系两方面来考虑
- 关键帧自身的质量要好
- 关键帧与其他关键帧之间要保持合适的连接关系。要有一定的共视，但是重复度又不能太高，即存在约束又尽量减少信息的冗余
在ORB-SLAM2 中主要是通过关键帧和其他关键帧的关系来选择关键帧的。量化指标如下：
- 距离上一关键帧的帧数是否足够（时间）。（×）
- 距离最近关键帧的距离是否足够远（空间）。（×）
- 跟踪局部地图质量（共视特征点数目）。（√）
在 ORB-SLAM2中，结合以上几种指标选择关键帧。选择关键帧的策略是，在跟踪线程中，创建关键帧的约束条件比较宽松，但在后续的局部建图线程中会严格筛选，剔除冗余的关键帧。在大旋转、快速运动、纹理不足等恶劣情况下提高跟踪的鲁棒性，从而大大降低跟丢的概率。
2.3. 如何选择并创建关键帧
跟踪分为两个阶段，第一阶段跟踪和第二阶段跟踪。当完成第二阶段的跟踪（局部地图跟踪）后，就需要根据当前跟踪的状态来判断是否需要插人关键帧。
在什么情况下不需要插入关键帧：
- 在仅定位跟踪模式下不需要插入关键帧。因为在该模式下只有跟踪线程，没有局部建图和闭环线程。
- 如果局部建图线程正在被闭环线程使用，则不插入关键帧。
- 如果距离上一次重定位比较近（ 1s 以内)，则不插入关键帧。
- 如果地图中关键帧数目已经超出最大限制，则不插人关键帧。
而在什么情况下需要插入关键帧相对比较复杂，分条件1和条件2，条件1和条件2都分a、b、c三个条件。
- 条件1a：距离上次插入关键帧超过1s。认为时间比较久了。
- 条件1b：满足插人关键帧的最小间隔，并且局部建图线程处于空闲状态。
- 条件1c：在双目相机或RGB-D相机模式下，当前帧跟踪到的点比参考关键帧跟踪到的点不足1/4，或者跟踪到的近点太少且没有跟踪到的近点较多，两者满足其一即可，我们称为满足近点条件。
条件 1 成立只需要上面的三个小条件中的一个满足即可认为条件 1 成立。
- 条件 2a：和参考帧相比，当前帧跟踪到的点数目太少，小于阈值比例。这个比例在单目相机模式下是0.9，在双目相机或RGB-D相机模式下是0.75，在关键帧小于2帧时是0.4。这个比例越高，插入频率越高。
- 条件2b：满足近点条件，同条件1c。
- 条件2c：成功跟踪到的匹配内点数目大于15。
条件 2 成立需要条件2a和条件2b两个条件中满足一个，且同时满足条件2c，才能认为条件2成立。
插入关键帧有两个阶段：
- 阶段1：条件1、条件2同时成立。此时进入阶段2。
- 阶段2：
如果此时局部建图线程空闲，则插入关键帧
如果此时局部建图线程繁忙，则通知中断局部建图线程中的局部 BA 过程
如果是单目相机模式，则先不插人关键帧
如果是双目相机或RGB-D相机模式，并且局部建图线程中待处理的关键帧少于3帧，则插入关键帧，否则不插入。
为什么要强调近点?
在ORB-SLAM2中，近点的讨论仅针对双目相机或RGB-D 相机模式，它的定义为相机基线长度的40倍。在这个距离内，双目相机立体匹配恢复的深度值比较准，因为更远的点视差太小，三角化误差会大；RGB-D相机测量的深度值也在可靠范围内，因为量程有限。 近点可以恢复出比较准确的平移向量，但是远点只对估计旋转有用，对平移和尺度的估计都不准确。所以，当发现近点数目不足、远点增多时，就需要插人关键帧，保证正确跟踪，估计准确的位姿。
2.4. NeedNewKeyFrame 函数判断当前帧是否需要插入关键帧
步骤：
- Step 1：纯VO模式下不插入关键帧，如果局部地图被闭环检测使用，则不插入关键帧
- Step 2：如果距离上一次重定位比较近，或者关键帧数目超出最大限制，不插入关键帧
- Step 3：得到参考关键帧跟踪到的地图点数量
- Step 4：查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧
- Step 5：对于双目或RGBD摄像头，统计可以添加的有效地图点总数 和 跟踪到的地图点数量
- Step 6：决策是否需要插入关键帧
/**
* @brief 判断当前帧是否需要插入关键帧
* @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++;
else
nNonTrackedClose++;
}
}
}
// 双目或RGBD情况下：跟踪到的地图点中近点太少 同时 没有跟踪到的三维点太多，可以插入关键帧了
// 单目时，为false
bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
// Step 7：决策是否需要插入关键帧
// Thresholds
// 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倍还少，或者满足bNeedToInsertClose
const 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出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
//队列中的关键帧数目不是很多,可以插入
return true;
else
//队列中缓冲的关键帧数目太多,暂时不能插入
return false;
}
else
//对于单目情况,就直接无法插入关键帧了
//? 为什么这里对单目情况的处理不一样?
//回答：可能是单目关键帧相对比较密集
return false;
}
}
else
//不满足上面的条件,自然不能插入关键帧
return false;
}
3. 跟踪线程
跟踪线程一定得有
第一帧刚进来所以设为参考关键帧。所以在地图初始化之后，使用参考关键帧跟踪；在第二帧来之后，速度就已经差不多已知，那么可以假设速度恒定，进行恒速模型跟踪（恒速模型的速度很快），如果跟丢了那么会使用参考关键帧跟踪进行找回；如果参考关键帧找不回来，则使用重定位跟踪。
第一阶段（恒速模型跟踪、参考关键帧跟踪和重定位跟踪）的目的是跟的上，精度不高；第二阶段（局部地图跟踪）是对第一阶段的位姿再次进行优化，得到相对准确的位姿。
局部地图跟踪需要使用共视关系。共视关键帧。
/**
* @brief 跟踪过程，包括恒速模型跟踪、参考关键帧跟踪、局部地图跟踪
* track包含两部分：估计运动、跟踪局部地图
*
* Step 1：初始化
* Step 2：跟踪
* Step 3：记录位姿信息，用于轨迹复现
*/
void Tracking::Track()
{
if (bStepByStep)
{
std::cout << "Tracking: Waiting to the next step" << std::endl;
while(!mbStep && bStepByStep)
usleep(500);
mbStep = false;
}
// Step 1 如局部建图里认为IMU有问题，重置当前活跃地图
if(mpLocalMapper->mbBadImu)
{
cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;
mpSystem->ResetActiveMap();
return;
}
// 从Atlas中取出当前active的地图
Map* pCurrentMap = mpAtlas->GetCurrentMap();
if(!pCurrentMap)
{
cout << "ERROR: There is not an active map in the atlas" << endl;
}
// Step 2 处理时间戳异常的情况
if(mState!=NO_IMAGES_YET)
{
if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
{
// 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
unique_lock<mutex> lock(mMutexImuQueue);
// mlQueueImuData.clear();
// 创建新地图
CreateMapInAtlas();
return;
}
else if(mCurrentFrame.mTimeStamp>mLastFrame.mTimeStamp+1.0)
{
// cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp << endl;
// cout << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl;
// 如果当前图像时间戳和前一帧图像时间戳大于1s，说明时间戳明显跳变了，重置地图后直接返回
//根据是否是imu模式,进行imu的补偿
if(mpAtlas->isInertial())
{
// 如果当前地图imu成功初始化
if(mpAtlas->isImuInitialized())
{
cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
// IMU完成第3次初始化（在localmapping线程里）
if(!pCurrentMap->GetIniertialBA2())
{
// 如果当前子图中imu没有经过BA2，重置active地图，也就是之前的数据不要了
mpSystem->ResetActiveMap();
}
else
{
// 如果当前子图中imu进行了BA2，重新创建新的子图，保存当前地图
CreateMapInAtlas();
}
}
else
{
// 如果当前子图中imu还没有初始化，重置active地图
cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
mpSystem->ResetActiveMap();
}
return;
}
}
}
// Step 3 IMU模式下设置IMU的Bias参数,还要保证上一帧存在
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && mpLastKeyFrame)
mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias()); // 使用上一帧的bias作为当前帧的初值
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
mLastProcessedState=mState;
// Step 4 IMU模式且没有创建地图的情况下对IMU数据进行预积分
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && !mbCreatedMap)
{
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartPreIMU = std::chrono::steady_clock::now();
#endif
// IMU数据进行预积分
PreintegrateIMU();
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndPreIMU = std::chrono::steady_clock::now();
double timePreImu = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPreIMU - time_StartPreIMU).count();
vdIMUInteg_ms.push_back(timePreImu);
#endif
}
mbCreatedMap = false;
// Get Map Mutex -> Map cannot be changed
// 地图更新时加锁。保证地图不会发生变化
// 疑问:这样子会不会影响地图的实时更新?
// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);
mbMapUpdated = false;
// 判断地图id是否更新了
int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();
int nMapChangeIndex = pCurrentMap->GetLastMapChange();
if(nCurMapChangeIndex>nMapChangeIndex)
{
// 检测到地图更新了
pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
mbMapUpdated = true;
}
// Step 5 初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD)
{
// 双目RGBD相机的初始化共用一个函数
StereoInitialization();
}
else
{
// 单目初始化
MonocularInitialization();
}
//mpFrameDrawer->Update(this);
if(mState!=OK) // If rightly initialized, mState=OK
{
// 如果没有成功初始化，直接返回
mLastFrame = Frame(mCurrentFrame);
return;
}
if(mpAtlas->GetAllMaps().size() == 1)
{
// 如果当前地图是第一个地图，记录当前帧id为第一帧
mnFirstFrameId = mCurrentFrame.mnId;
}
}
else
{
// System is initialized. Track Frame.
// Step 6 系统成功初始化，下面是具体跟踪过程
bool bOK;
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartPosePred = std::chrono::steady_clock::now();
#endif
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式
// tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking
if(!mbOnlyTracking)
{
// State OK
// 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
// Step 6.1 检查并更新上一帧被替换的MapPoints
// 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查
CheckReplacedInLastFrame();
// Step 6.2 运动模型是空的并且imu未初始化或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪
// 第一个条件,如果运动模型为空并且imu未初始化,说明是刚开始第一帧跟踪，或者已经跟丢了。
// 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们用重定位帧来恢复位姿
// mnLastRelocFrameId 上一次重定位的那一帧
if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
Verbose::PrintMess("TRACK: Track with respect to the reference KF ", Verbose::VERBOSITY_DEBUG);
bOK = TrackReferenceKeyFrame();
}
else
{
Verbose::PrintMess("TRACK: Track with motion model", Verbose::VERBOSITY_DEBUG);
// 用恒速模型跟踪。所谓的恒速就是假设上上帧到上一帧的位姿=上一帧的位姿到当前帧位姿
// 根据恒速模型设定当前帧的初始位姿，用最近的普通帧来跟踪当前的普通帧
// 通过投影的方式在参考帧中找当前帧特征点的匹配点，优化每个特征点所对应3D点的投影误差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
bOK = TrackReferenceKeyFrame(); // 根据恒速模型失败了，只能根据参考关键帧来跟踪
}
// 新增了一个状态RECENTLY_LOST，主要是结合IMU看看能不能拽回来
// Step 6.3 如果经过跟踪参考关键帧、恒速模型跟踪都失败的话，并满足一定条件就要标记为RECENTLY_LOST或LOST
if (!bOK)
{
// 条件1：如果当前帧距离上次重定位成功不到1s
// mnFramesToResetIMU 表示经过多少帧后可以重置IMU，一般设置为和帧率相同，对应的时间是1s
// 条件2：单目+IMU 或者 双目+IMU模式
// 同时满足条件1，2，标记为LOST
if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&
(mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO || mSensor == System::IMU_RGBD))
{
mState = LOST;
}
else if(pCurrentMap->KeyFramesInMap()>10)
{
// cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
// 条件1：当前地图中关键帧数目较多（大于10）
// 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）或者非IMU模式
// 同时满足条件1，2，则将状态标记为RECENTLY_LOST，后面会结合IMU预测的位姿看看能不能拽回来
mState = RECENTLY_LOST;
// 记录丢失时间
mTimeStampLost = mCurrentFrame.mTimeStamp;
}
else
{
mState = LOST;
}
}
}
else // 跟踪不正常按照下面处理
{
// 如果是RECENTLY_LOST状态
if (mState == RECENTLY_LOST)
{
Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);
// bOK先置为true
bOK = true;
if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD))
{
// IMU模式下可以用IMU来预测位姿，看能否拽回来
// Step 6.4 如果当前地图中IMU已经成功初始化，就用IMU数据预测位姿
if(pCurrentMap->isImuInitialized())
PredictStateIMU();
else
bOK = false;
// 如果IMU模式下当前帧距离跟丢帧超过5s还没有找回（time_recently_lost默认为5s）
// 放弃了，将RECENTLY_LOST状态改为LOST
if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost)
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
else
{
// Step 6.5 纯视觉模式则进行重定位。主要是BOW搜索，EPnP求解位姿
// Relocalization
bOK = Relocalization();
//std::cout << "mCurrentFrame.mTimeStamp:" << to_string(mCurrentFrame.mTimeStamp) << std::endl;
//std::cout << "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl;
if(mCurrentFrame.mTimeStamp-mTimeStampLost>3.0f && !bOK)
{
// 纯视觉模式下重定位失败，状态为LOST
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
}
else if (mState == LOST) // 上一帧为最近丢失且重定位失败时
{
// Step 6.6 如果是LOST状态
// 开启一个新地图
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
if (pCurrentMap->KeyFramesInMap()<10)
{
// 当前地图中关键帧数目小于10，重置当前地图
mpSystem->ResetActiveMap();
Verbose::PrintMess("Reseting current map...", Verbose::VERBOSITY_NORMAL);
}else
CreateMapInAtlas(); // 当前地图中关键帧数目超过10，创建新地图
// 干掉上一个关键帧
if(mpLastKeyFrame)
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);
return;
}
}
}
else // 纯定位模式
{
// Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
// 只进行跟踪tracking，局部地图不工作
if(mState==LOST)
{
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
// Step 6.1 LOST状态进行重定位
bOK = Relocalization();
}
else
{
// mbVO是mbOnlyTracking为true时的才有的一个变量
// mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉)
// mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跟丢
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
// Step 6.2 如果跟踪状态正常，使用恒速模型或参考关键帧跟踪
if(mbVelocity)
{
// 优先使用恒速模型跟踪
bOK = TrackWithMotionModel();
}
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为true，表明此帧匹配了很少（小于10）的地图点，要跟丢的节奏，既做跟踪又做重定位
// MM=Motion Model,通过运动模型进行跟踪的结果
bool bOKMM = false;
// 通过重定位方法来跟踪的结果
bool bOKReloc = false;
// 运动模型中构造的地图点
vector<MapPoint*> vpMPsMM;
// 在追踪运动模型后发现的外点
vector<bool> vbOutMM;
// 运动模型得到的位姿
Sophus::SE3f TcwMM;
// Step 6.3 当运动模型有效的时候,根据运动模型计算位姿
if(mbVelocity)
{
bOKMM = TrackWithMotionModel();
// 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.GetPose();
}
// Step 6.4 使用重定位的方法来得到当前帧的位姿
bOKReloc = Relocalization();
// Step 6.5 根据前面的恒速模型、重定位结果来更新状态
if(bOKMM && !bOKReloc)
{
// 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
// 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数
if(mbVO)
{
// 更新当前帧的地图点被找到次数，注意跟观测次数区分，这是两个概念
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;
}
}
}
// 将最新的关键帧作为当前帧的参考关键帧
// mpReferenceKF先是上一时刻的参考关键帧，如果当前为新关键帧则变成当前关键帧，如果不是新的关键帧则先为上一帧的参考关键帧，而后经过更新局部关键帧重新确定
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndPosePred = std::chrono::steady_clock::now();
double timePosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPosePred - time_StartPosePred).count();
vdPosePred_ms.push_back(timePosePred);
#endif
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartLMTrack = std::chrono::steady_clock::now();
#endif
// Step 7 在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
// 在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
// 前面主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
// 然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
// If we have an initial estimation of the camera pose and matching. Track the local map.
if(!mbOnlyTracking)
{
if(bOK)
{
// 局部地图跟踪
bOK = TrackLocalMap();
}
if(!bOK)
cout << "Fail to track local map!" << endl;
}
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();
}
// 到此为止跟踪确定位姿阶段结束，下面开始做收尾工作和为下一帧做准备
// 查看到此为止时的两个状态变化
// bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true -->OK 1 跟踪局部地图成功
// \ \ \---局部地图跟踪失败---false
// \ \---当前帧跟踪失败---false
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true -->OK 2 重定位
// \ \---局部地图跟踪失败---false
// \---重定位失败---false
//
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK -->OK 1 跟踪局部地图成功
// \ \ \---局部地图跟踪失败---OK -->OK 3 正常跟踪
// \ \---当前帧跟踪失败---非OK
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---非OK
// \ \---局部地图跟踪失败---非OK
// \---重定位失败---非OK（传不到这里，因为直接return了）
// 由上图可知当前帧的状态OK的条件是跟踪局部地图成功，重定位或正常跟踪都可
// Step 8 根据上面的操作来判断是否追踪成功
if(bOK)
// 此时还OK才说明跟踪成功了
mState = OK;
else if (mState == OK) // 由上图可知只有当第一阶段跟踪成功，但第二阶段局部地图跟踪失败时执行
{
// 状态变为最近丢失
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
{
Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2())
{
// IMU模式下IMU没有成功初始化或者没有完成IMU BA，则重置当前地图
cout << "IMU is not or recently initialized. Reseting active map..." << endl;
mpSystem->ResetActiveMap();
}
mState=RECENTLY_LOST;
}
else
mState=RECENTLY_LOST; // 上一个版本这里直接判定丢失 LOST
// 被注释掉了，记录丢失时间
/*if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames)
{*/
mTimeStampLost = mCurrentFrame.mTimeStamp;
//}
}
// Save frame if recent relocalization, since they are used for IMU reset (as we are making copy, it shluld be once mCurrFrame is completely modified)
// 这段貌似没啥作用
if((mCurrentFrame.mnId<(mnLastRelocFrameId+mnFramesToResetIMU)) && (mCurrentFrame.mnId > mnFramesToResetIMU) &&
(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD) && pCurrentMap->isImuInitialized())
{
// TODO check this situation
Verbose::PrintMess("Saving pointer to frame. imu needs reset...", Verbose::VERBOSITY_NORMAL);
Frame* pF = new Frame(mCurrentFrame);
pF->mpPrevFrame = new Frame(mLastFrame);
// Load preintegration
pF->mpImuPreintegratedFrame = new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
}
// 下面代码没有用
if(pCurrentMap->isImuInitialized())
{
if(bOK)
{
// 当前帧距离上次重定位帧刚好等于1s，重置（还未实现 TODO）
if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
{
cout << "RESETING FRAME!!!" << endl;
ResetFrameIMU();
}
else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
mLastBias = mCurrentFrame.mImuBias; // 没啥用，后面会重新赋值后传给普通帧
}
}
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndLMTrack = std::chrono::steady_clock::now();
double timeLMTrack = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLMTrack - time_StartLMTrack).count();
vdLMTrack_ms.push_back(timeLMTrack);
#endif
// Update drawer
// 更新显示线程中的图像、特征点、地图点等信息
mpFrameDrawer->Update(this);
if(mCurrentFrame.isSet())
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
// 查看到此为止时的两个状态变化
// bOK的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---true
// \ \ \---局部地图跟踪失败---false
// \ \---当前帧跟踪失败---false
// \---上一帧跟踪失败---重定位成功---局部地图跟踪成功---true
// \ \---局部地图跟踪失败---false
// \---重定位失败---false
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK
// \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST）
// \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST)
// \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK
// \ \ \---局部地图跟踪失败---LOST
// \ \---重定位失败---LOST（传不到这里，因为直接return了）
// \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了）
// Step 9 如果跟踪成功 或 最近刚刚跟丢，更新速度，清除无效地图点，按需创建关键帧
if(bOK || mState==RECENTLY_LOST)
{
// Update motion model
// Step 9.1 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
if(mLastFrame.isSet() && mCurrentFrame.isSet())
{
Sophus::SE3f LastTwc = mLastFrame.GetPose().inverse();
// mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
mVelocity = mCurrentFrame.GetPose() * LastTwc;
mbVelocity = true;
}
else {
// 否则没有速度
mbVelocity = false;
}
// 使用IMU积分的位姿显示
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
// Clean VO matches
// Step 9.2 清除观测不到的地图点
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// Step 9.3 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
// 上个步骤中只是在当前帧中将这些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();
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_StartNewKF = std::chrono::steady_clock::now();
#endif
// 判断是否需要插入关键帧
bool bNeedKF = NeedNewKeyFrame();
// Check if we need to insert a new keyframe
// if(bNeedKF && bOK)
// Step 9.4 根据条件来判断是否插入关键帧
// 需要同时满足下面条件1和2
// 条件1：bNeedKF=true，需要插入关键帧
// 条件2：bOK=true跟踪成功 或 IMU模式下的RECENTLY_LOST模式且mInsertKFsLost为true
if(bNeedKF && (bOK || (mInsertKFsLost && mState==RECENTLY_LOST &&
(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD))))
CreateNewKeyFrame(); // 创建关键帧，对于双目或RGB-D会产生新的地图点
#ifdef REGISTER_TIMES
std::chrono::steady_clock::time_point time_EndNewKF = std::chrono::steady_clock::now();
double timeNewKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndNewKF - time_StartNewKF).count();
vdNewKF_ms.push_back(timeNewKF);
#endif
// 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. Only has effect if lastframe is tracked
// 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点
// 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉
// Step 9.5 删除那些在BA中检测为外点的地图点
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
// Step 10 如果第二阶段跟踪失败，跟踪状态为LOST
if(mState==LOST)
{
// 如果地图中关键帧小于10，重置当前地图，退出当前跟踪
if(pCurrentMap->KeyFramesInMap()<=10) // 上一个版本这里是5
{
mpSystem->ResetActiveMap();
return;
}
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
if (!pCurrentMap->isImuInitialized())
{
// 如果是IMU模式并且还未进行IMU初始化，重置当前地图，退出当前跟踪
Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
mpSystem->ResetActiveMap();
return;
}
// 如果地图中关键帧超过10 并且 纯视觉模式 或 虽然是IMU模式但是已经完成IMU初始化了，保存当前地图，创建新的地图
CreateMapInAtlas();
// 新增加了个return
return;
}
// 确保已经设置了参考关键帧
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 保存上一帧的数据,当前帧变上一帧
mLastFrame = Frame(mCurrentFrame);
}
// 查看到此为止
// mState的历史变化---上一帧跟踪成功---当前帧跟踪成功---局部地图跟踪成功---OK
// \ \ \---局部地图跟踪失败---非OK（IMU时为RECENTLY_LOST）
// \ \---当前帧跟踪失败---非OK(地图超过10个关键帧时 RECENTLY_LOST)
// \---上一帧跟踪失败(RECENTLY_LOST)---重定位成功---局部地图跟踪成功---OK
// \ \ \---局部地图跟踪失败---LOST
// \ \---重定位失败---LOST（传不到这里，因为直接return了）
// \--上一帧跟踪失败(LOST)--LOST（传不到这里，因为直接return了）
// last.记录位姿信息，用于轨迹复现
// Step 11 记录位姿信息，用于最后保存所有的轨迹
if(mState==OK || mState==RECENTLY_LOST)
{
// Store frame pose information to retrieve the complete camera trajectory afterwards.
// Step 11：记录位姿信息，用于最后保存所有的轨迹
if(mCurrentFrame.isSet())
{
// 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
Sophus::SE3f Tcr_ = mCurrentFrame.GetPose() * mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr_);
mlpReferences.push_back(mCurrentFrame.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);
}
}
#ifdef REGISTER_LOOP
if (Stop()) {
// Safe area to stop
while(isStopped())
{
usleep(3000);
}
}
#endif
}
3.1. 参考关键帧跟踪
参考关键帧跟踪就是将当前普通帧（位姿未知）和它对应的参考关键帧（位姿已知）进行特征匹配及优化，从而估计当前普通帧的位姿。
参考关键帧跟踪应用场景如下：
- 情况1。地图刚刚初始化之后，此时恒速模型中的速度为空。这时只能使用参考关键帧，也就是初始化的第1、2帧对当前帧进行跟踪。
- 情况2。恒速模型跟踪失败后，尝试用最近的参考关键帧跟踪当前普通帧。因为在恒速模型中估计的速度并不准确，可能会导致错误匹配，并且恒速模型只利用了前一帧的信息，信息量也有限，跟踪失败的可能性较大。而参考关键帧可能在局部建图线程中新匹配了更多的地图点，并且参考关键帧的位姿是经过多次优化的，更准确。
3.1.1. TrackReferenceKeyFrame 函数用参考关键帧的地图点来对当前普通帧进行跟踪
用参考关键帧的地图点来对当前普通帧进行跟踪
步骤
- Step 1：将当前普通帧的描述子转化为BoW向量
- Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
- Step 3 : 将上一帧的位姿态作为当前帧位姿的初始值
- Step 4 : 通过优化3D-2D的重投影误差来获得位姿
- Step 5：剔除优化后的匹配点中的外点
/*
* @return 如果匹配数超10，返回true
*/
bool Tracking::TrackReferenceKeyFrame()
{
// 注意函数返回值类型为 bool
// 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加速当前帧与参考帧之间的特征点匹配，得到成功匹配的特征点的数目 nmatches
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
// 匹配数目小于15，认为跟踪失败
if(nmatches<15)
{
cout << "TRACK_REF_KF: Less than 15 matches!!\n";
return false;
}
// Step 3:将上一帧的位姿态作为当前帧位姿的初始值
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.GetPose()); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
//mCurrentFrame.PrintPointDistribution();
// cout << " TrackReferenceKeyFrame mLastFrame.mTcw: " << mLastFrame.mTcw << endl;
// Step 4:通过优化3D-2D的重投影误差来获得位姿，mCurrentFrame为待优化的当前帧，返回值为内点的数目，同时可以得到当前帧中的特征点哪些是外点，哪些是内点。
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// Step 5：剔除优化后的匹配点中的外点
//之所以在优化之后才剔除外点，是因为在优化的过程中就有了对这些外点的标记
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
//if(i >= mCurrentFrame.Nleft) break;
if(mCurrentFrame.mvpMapPoints[i]) //首先得是地图点（mvpMapPoints中岑放的是每个关键点的相应立体坐标和深度。)
{
// 如果对应到的某个特征点是外点
if(mCurrentFrame.mvbOutlier[i])
{
// 清除它在当前帧中存在过的痕迹
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
if(i < mCurrentFrame.Nleft){
pMP->mbTrackInView = false;
}
else{
pMP->mbTrackInViewR = false;
}
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
// 匹配的内点计数++
nmatchesMap++;
}
}
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
return true;
else
return nmatchesMap>=10; // 跟踪成功的数目超过10才认为跟踪成功，否则跟踪失败
}
3.2. 恒速模型跟踪
两个图像帧之间一般只有几十毫秒的时间，在这么短的时间内，可以做合理的假设——在相邻帧间极短的时间内，相机处于匀速运动状态，可以用上一帧的位姿和速度估计当前帧的位姿。所以称为恒速模型跟踪。
参考关键帧跟踪比较复杂，恒速模型跟踪估计位姿实时性更高。使用参考关键帧跟踪成功后，就有了速度信息，就用恒速模型跟踪。
优势：跟踪只需要上一帧的信息。其次在双目和RGB-D模式生成临时地图点可以提高跟踪的成功率，提升跟踪的稳定性。但是缺点是过于理想化，在帧率较低且运动变化较大的场景中可能会跟踪丢失。
图中的 Fref 是关键帧，会在跟踪线程和局部建图线程中都用，因此关键帧的位姿和信息是一直在更新的，并且比较准确。因此使用较近的关键帧对上一帧进行恒速模型的位姿更新。
恒速模型跟踪中双目相机或RGB-D相机可以通过立体匹配或直接测量输出当前帧的深度图。在更新上一帧信息函数 UpdateLastFrame() 中，会针对双目相机或RGB-D相机为上一帧生成临时的地图点，这些临时的地图点主要用来增加跟踪稳定性，在跟踪结束后会丢弃掉。
3.2.1. UpdateLastFrame 函数更新上一帧位姿，在上一帧中生成临时地图点
第一阶段跟踪中的恒速模型跟踪中新增地图点。针对双目相机或RGB-D相机，找出上一帧中具有有效深度值且不是地图点的特征点，将其中较近的点作为上一帧新的临时地图点，并记录在向量mlpTemporalPoints中。mlpTemporalPoints.push_back(pNewMP);
注意：这里因为是临时地图点，其实是不满足作为地图点的条件，但是可以辅助做跟踪，所以没有添加地图点的相互观测和属性信息（最佳描述子、平均观测方向、观测距离范围）。之后在 Tracking::CreateNewKeyFrame 之前会全部删除。
/**
1. @brief 更新上一帧位姿，在上一帧中生成临时地图点
2. 单目情况：只计算了上一帧的世界坐标系位姿
3. 双目和rgbd情况：选取有有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性
*/
void Tracking::UpdateLastFrame()
{
// Update pose according to reference keyframe
// Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
// 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧
KeyFrame* pRef = mLastFrame.mpReferenceKF;
// ref_keyframe 到 lastframe的位姿变换
cv::Mat Tlr = mlRelativeFramePoses.back();
// 将上一帧的世界坐标系下的位姿计算出来
// l:last, r:reference, w:world
// Tlw = Tlr*Trw
mLastFrame.SetPose(Tlr*pRef->GetPose());
// 如果上一帧为关键帧，或者单目的情况，则退出
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
return;
// Step 2：对于双目或rgbd相机，为上一帧生成新的临时地图点
// 注意这些地图点只是用来跟踪，不加入到地图中，跟踪完后会删除
// 通过立体/RGB-D 传感器根据测量的深度对点进行排序
// Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
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第一个元素是某个点的深度,第二个元素是对应的特征点id
vDepthIdx.push_back(make_pair(z,i));
}
}
// 如果上一帧中没有有效深度的点,那么就直接退出
if(vDepthIdx.empty())
return;
// 按照深度从小到大排序
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.2：从中找出不是地图点的部分
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)
{
// Step 2.3：需要创建的点，包装为地图点。只是为了提高双目和RGBD的跟踪成功率，并没有添加复杂属性，因为后面会扔掉
// 反投影到世界坐标系中
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(
x3D, // 世界坐标系坐标
mpMap, // 跟踪的全局地图
&mLastFrame, // 存在这个特征点的帧(上一帧)
i); // 特征点id
// 加入上一帧的地图点中
mLastFrame.mvpMapPoints[i]=pNewMP;
// 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除，并未添加新的观测信息也不会更新。
mlpTemporalPoints.push_back(pNewMP);
nPoints++;
}
else
{
// 因为从近到远排序，记录其中不需要创建地图点的个数
nPoints++;
}
// Step 2.4：如果地图点质量不好，停止创建地图点
// 停止新增临时地图点必须同时满足以下条件：
// 1、当前的点的深度已经超过了设定的深度阈值（35倍基线）
// 2、nPoints已经超过100个点，说明距离比较远了，可能不准确，停掉退出
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
在创建新的关键帧之前会全部删掉新增的临时地图点。
mlpTemporalPoints.clear();
// Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点
if(NeedNewKeyFrame())
CreateNewKeyFrame();
3.2.2. TrackWithMotionModel 函数根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
步骤：
- Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点。根据之前估计的速度，用恒速模型得到当前帧的初始位姿。（代码中间关于 IMU 的部分可以跳过，用不上)
- Step 2：根据上一帧特征点对应地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再次尝试。
- Step 3：利用 3D-2D 投影关系，优化当前帧位姿
- Step 4：剔除地图点中外点，并清除其所有关系。
- 统计匹配成功的特征点数目，如果超过阈值，则认为跟踪成功。
/**
* @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
* @return 如果匹配数大于10，认为跟踪成功，返回true
*/
bool Tracking::TrackWithMotionModel()
{
// 最小距离 < 0.9*次小距离 匹配成功，检查旋转
ORBmatcher matcher(0.9,true);
// Create "visual odometry" points if in Localization Mode
// Step 1：根据参考关键帧更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
UpdateLastFrame();
// Step 2：根据IMU或者恒速模型得到当前帧的初始位姿。
if (mpAtlas->isImuInitialized() && (mCurrentFrame.mnId>mnLastRelocFrameId+mnFramesToResetIMU))
{
// Predict state with IMU if it is initialized and it doesnt need reset
// IMU完成初始化 并且 距离重定位挺久不需要重置IMU，用IMU来估计位姿，没有后面的这那那这的
PredictStateIMU();
return true;
}
else
{
// 根据之前估计的速度，用恒速模型得到当前帧的初始位姿。
mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose());
}
// 清空当前帧的地图点
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame
// 设置特征匹配过程中的搜索半径
int th;
if(mSensor==System::STEREO)
th=7;
else
th=15;
// Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR);
// If few matches, uses a wider window search
// 如果匹配点太少，则扩大搜索半径再来一次
if(nmatches<20)
{
Verbose::PrintMess("Not enough matches, wider window search!!", Verbose::VERBOSITY_NORMAL);
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR || mSensor==System::IMU_MONOCULAR);
Verbose::PrintMess("Matches with wider search: " + to_string(nmatches), Verbose::VERBOSITY_NORMAL);
}
// 如果还是不能够获得足够的匹配点，则认为跟踪失败。
// 但这里不同于ORB-SLAM2的方式
if(nmatches<20)
{
Verbose::PrintMess("Not enough matches!!", Verbose::VERBOSITY_NORMAL);
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
return true;
else
return false;
}
// Optimize frame pose with all matches
// Step 4：利用3D-2D投影关系，优化当前帧位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// Step 5：剔除地图点中外点
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;
if(i < mCurrentFrame.Nleft){
pMP->mbTrackInView = false;
}
else{
pMP->mbTrackInViewR = false;
}
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
// 累加成功匹配到的地图点数目
nmatchesMap++;
}
}
// 纯定位模式下：如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位
if(mbOnlyTracking)
{
mbVO = nmatchesMap<10;
return nmatches>20;
}
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO || mSensor == System::IMU_RGBD)
return true;
else
return nmatchesMap>=10; // 匹配超过10个点就认为跟踪成功
}
3.3. 重定位跟踪
核心是词袋粗匹配和 EPnP 算法精匹配。
使用前提是参考关键帧跟踪和恒速模型跟踪失效了。因为词袋是不需要依赖先验信息的。
其中避免暴力搜索，采用一种倒排索引。
3.3.1. 倒排索引（ Inverse Index ）
也称为逆向索引，是词袋模型中一个非常重要的概念。与之对应的操作叫作直接索引(Direct Index )，示意图和定义如下。
1．倒排索引
以单词为索引基础，存储有单词出现的所有图像的ID及对应的权重。倒排索引的优势是可以快速查询某个单词出现在哪些图像中，进而得到那些图像中有多少个共同的单词。这对判断图像的相似性非常有效。
涉及的函数是KeyFrameDatabase.cc 中的
- KeyFrameDatabase::add(KeyFrame *pKF) 函数 数据库有新的关键帧，根据关键帧的词袋向量，更新数据库的倒排索引。
- KeyFrameDatabase::erase(KeyFrame* pKF)函数 关键帧被删除后，更新数据库的倒排索引
3.3.2. 搜索重定位候选关键帧
搜索重定位候选关键帧相对比较复杂，其中用到了共视图、倒排索引，目的是从关键帧数据库中找出和当前帧最相似的候选关键帧组。
3.3.3. KeyFrameDatabase::DetectRelocalizationCandidates 函数在重定位中找到与该帧相似的候选关键帧组
步骤：
- Step 1. 找出和当前帧具有公共单词的所有关键帧
- Step 2. 只和具有共同单词较多的关键帧进行相似度计算
- Step 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
- Step 4. 只返回累计得分较高的组中分数最高的关键帧
/*
* @brief 在重定位中找到与该帧相似的候选关键帧组
* @param F 需要重定位的帧
* @return 相似的候选关键帧数组
*/
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
list<KeyFrame*> lKFsSharingWords;
// Search all keyframes that share a word with current frame
// Step 1：找出和当前帧具有公共单词(word)的所有关键帧
{
unique_lock<mutex> lock(mMutex);
// mBowVec 内部实际存储的是std::map<WordId, WordValue>
// WordId 和 WordValue 表示Word在叶子中的id 和权重
for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
{
// 根据倒排索引，提取所有包含该wordid的所有KeyFrame
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
{
KeyFrame* pKFi=*lit;
// pKFi->mnRelocQuery起标记作用，是为了防止重复选取
if(pKFi->mnRelocQuery!=F->mnId)
{
// pKFi还没有标记为F的重定位候选帧
pKFi->mnRelocWords=0;
pKFi->mnRelocQuery=F->mnId;
lKFsSharingWords.push_back(pKFi);
}
// 能找到 共同单词 则 mnRelocWords++
pKFi->mnRelocWords++;
}
}
}
// 如果和当前帧具有公共单词的关键帧数目为0，无法进行重定位，返回空
if(lKFsSharingWords.empty())
return vector<KeyFrame*>();
// Only compare against those keyframes that share enough words
// Step 2：统计上述关键帧中与当前帧F具有共同单词最多的单词数maxCommonWords，用来设定阈值1
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
if((*lit)->mnRelocWords>maxCommonWords)
maxCommonWords=(*lit)->mnRelocWords;
}
// 阈值1：最小公共单词数为最大公共单词数目的0.8倍
int minCommonWords = maxCommonWords*0.8f;
list<pair<float,KeyFrame*> > lScoreAndMatch;
int nscores=0;
// Compute similarity score.
// Step 3：遍历上述关键帧，挑选出共有单词数大于阈值1的及其和当前帧单词匹配得分存入lScoreAndMatch
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
// 当前帧F只和具有共同单词较多（大于minCommonWords）的关键帧进行比较
if(pKFi->mnRelocWords>minCommonWords)
{
nscores++; // 这个变量后面没有用到
// 用mBowVec来计算两者的相似度得分
float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
pKFi->mRelocScore=si;
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = 0;
// Lets now accumulate score by covisibility
// Step 4：计算lScoreAndMatch中每个关键帧的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值2
// 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧共视程度最高的前十个关键帧归为一组，计算累计得分
for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
{
KeyFrame* pKFi = it->second;
// 取出与关键帧pKFi共视程度最高的前10个关键帧
// GetBestCovisibilityKeyFrames 函数在 KeyFrame.cc 中，作用是得到与该关键帧连接的前N个最强共视关键帧(已按权值排序)，输出是满足权重条件的关键帧集合
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
// 该组最高分数
float bestScore = it->first;
// 该组累计得分
float accScore = bestScore;
// 该组最高分数对应的关键帧
KeyFrame* pBestKF = pKFi;
// 遍历共视关键帧，累计得分
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnRelocQuery!=F->mnId)
continue;
// 只有pKF2也在重定位候选帧中，才能贡献分数
accScore+=pKF2->mRelocScore;
// 统计得到组里分数最高的KeyFrame
if(pKF2->mRelocScore>bestScore)
{
pBestKF=pKF2;
bestScore = pKF2->mRelocScore;
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
// 记录所有组中最高的得分
if(accScore>bestAccScore)
bestAccScore=accScore;
}
// Return all those keyframes with a score higher than 0.75*bestScore
// Step 5：得到所有组中总得分大于阈值2的，组内得分最高的关键帧，作为候选关键帧组
//阈值2：最高得分的0.75倍
float minScoreToRetain = 0.75f*bestAccScore;
set<KeyFrame*> spAlreadyAddedKF;
vector<KeyFrame*> vpRelocCandidates;
vpRelocCandidates.reserve(lAccScoreAndMatch.size());
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
const float &si = it->first;
// 只返回累计得分大于阈值2的组中分数最高的关键帧
if(si>minScoreToRetain)
{
KeyFrame* pKFi = it->second;
// 判断该pKFi是否已经添加在队列中了
if(!spAlreadyAddedKF.count(pKFi))
{
vpRelocCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpRelocCandidates;
}
3.3.4. 重定位流程
3.3.5. Tracking::Relocalization 函数包含重定位过程
步骤：
- Step 1：计算当前帧特征点的词袋向量
- Step 2：找到与当前帧相似的候选关键帧，就是上面的 DetectRelocalizationCandidates 函数
- Step 3：通过BoW进行匹配，这个匹配比较粗糙。
- Step 4：通过EPnP算法估计姿态，在词袋基础上进行的精确匹配，前提是匹配点要足够。
- Step 5：通过PoseOptimization对姿态进行优化求解
- Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
/**
* @details 重定位过程
* @return true
* @return false
*/
bool Tracking::Relocalization()
{
// Compute Bag of Words Vector
// Step 1：计算当前帧特征点的词袋向量
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;
// Step 3：遍历所有的候选关键帧，通过词袋进行快速匹配，用匹配结果初始化PnP Solver
for(int i=0; i<nKFs; i++)
{
KeyFrame* pKF = vpCandidateKFs[i];
if(pKF->isBad())
vbDiscarded[i] = true;
else
{
// 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
// 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else
{
// 如果匹配数目够用，用匹配结果初始化EPnPsolver
// 为什么用EPnP? 因为计算复杂度低，精度高
// 用 SetRansacParameters 函数 设置RANSAC迭代的参数
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(
0.99, //用于计算RANSAC迭代次数理论值的概率
10, //最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
300, //最大迭代次数
4, //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
0.5, //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
// 这里的 P4P RANSAC是Epnp，每次迭代需要4个点
// 是否已经找到相匹配的关键帧的标志
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
// Step 4: 通过一系列操作,直到找到能够匹配上的关键帧
// 为什么搞这么复杂？答：是担心误闭环
while(nCandidates>0 && !bMatch)
{
//遍历当前所有的候选关键帧
for(int i=0; i<nKFs; i++)
{
// 忽略放弃的
if(vbDiscarded[i])
continue;
//内点标记
vector<bool> vbInliers;
//内点数
int nInliers;
// 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。
bool bNoMore;
// Step 4.1：通过EPnP算法估计姿态，迭代5次，首次计算出的位姿并不准确，因此需要进行优化。
PnPsolver* pSolver = vpPnPsolvers[i];
// 负责 EPnP的迭代计算，并进行内点的标记。vbInliers内点的标记,nInliers总共的内点数
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
// bNoMore 为true 表示已经超过了RANSAC最大迭代次数，就放弃当前关键帧
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
// Step 4.2：如果EPnP 计算出了位姿，对内点进行BA优化
Tcw.copyTo(mCurrentFrame.mTcw);
// EPnP 里RANSAC后的内点的集合
set<MapPoint*> sFound;
const int np = vbInliers.size();
//遍历所有内点
for(int j=0; j<np; j++)
{
// 判断第 j 个内点是否为 true。因为在上面的 iterate 迭代中已经对内点进行了标记，iterate 定义vbInliers元素时默认是false，如果是内点则vbInliers[j]会改为 true。
if(vbInliers[j])
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
// 只优化位姿,不优化地图点的坐标，返回的是内点的数量
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 4.3：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
// 前面的匹配关系是用词袋匹配过程得到的
if(nGood<50)
{
// 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配
int nadditional = matcher2.SearchByProjection(
mCurrentFrame, //当前帧
vpCandidateKFs[i], //关键帧
sFound, //已经找到的地图点集合，不会用于PNP
10, //窗口阈值，会乘以金字塔尺度
100); //匹配的ORB描述子距离应该小于这个阈值
// 如果通过投影过程新增了比较多的匹配特征点对
if(nadditional+nGood>=50)
{
// 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿
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
// Step 4.4：如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎
// 重新执行上一步 4.3的过程，只不过使用更小的搜索窗口
// 这里的位姿已经使用了更多的点进行了优化,应该更准，所以使用更小的窗口搜索
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, //已经找到的地图点，不会用于PNP
3, //新的窗口阈值，会乘以金字塔尺度
64); //匹配的ORB描述子距离应该小于这个阈值
// Final optimization
// 如果成功挽救回来，匹配数目达到要求，最后BA优化一下
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;
}
}
}//一直运行,知道已经没有足够的关键帧,或者是已经有成功匹配上的关键帧
}
// 折腾了这么久还是没有匹配上，重定位失败
if(!bMatch)
{
return false;
}
else
{
// 如果匹配上了,说明当前帧重定位成功了(当前帧已经有了自己的位姿)
// 记录成功重定位帧的id，防止短时间多次重定位
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
}
4. 局部地图跟踪
第一阶段主要保证跟得上，第二阶段主要保证跟的准。
局部地图跟踪主要目的是将当前帧的局部关键帧对应的局部地图点投影到该帧中，增加更多的匹配关系，对第一阶段的位姿再次优化，从而得到更准确的位姿。
局部地图为local map，局部建图为local mapping。
local map 局部地图来自局部关键帧对应的地图点，而局部关键帧包括当前普通帧的一级共视关键帧、二级共视关键帧及其子关键帧和父关键帧。local map 的目的是增加更多的投影匹配约束关系，仅优化当前帧的位姿，不优化局部关键帧，也不优化地图点。
local mapping 指局部建图线程，用来处理跟踪过程中建立的关键帧包括这些关键帧之间互相匹配生成新的可靠的地图点、一起优化当前关键帧及其共视关键帧的位姿和地图点。根据优化结果删除地图中不可靠的地图点、冗余的关键帧。local mapping 的目的是让已有的关键帧之间产生更多的联系，产生更多可靠的地图点，优化共视关键帧的位姿及其地图点，使得跟踪更稳定。
4.1. 局部关键帧
图11-8中当前帧F的局部关键帧为一级共视关键帧KF1、KF2，二级共视关键帧KF3、KF4、KF5、KF6，一级共视关键帧的父子关键帧KF7、KF8。父子关键帧指的是与当前一级共视关键帧共视程度最高的关键帧。一个关键帧只有一个父关键帧，但可以有多个子关键帧。
4.1.1. UpdateLocalKeyFrames 跟踪局部地图函数里，更新局部关键帧
步骤：
方法是遍历当前帧的地图点，将观测到这些地图点的关键帧和相邻的关键帧及其父子关键帧，作为mvpLocalKeyFrames
- Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
- Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧包括以下3种类型
2.1 类型1：能观测到当前帧地图点的关键帧，也称一级共视关键帧
2.2 类型2：一级共视关键帧的共视关键帧，称为二级共视关键帧
2.3 类型3：一级共视关键帧的子关键帧、父关键帧 - Step 3：更新当前帧的参考关键帧，与自己共视程度最高的关键帧作为参考关键帧
/**
* @brief 跟踪局部地图函数里，更新局部关键帧
*/
void Tracking::UpdateLocalKeyFrames()
{
// Each map point vote for the keyframes in which it has been observed
// Step 1：遍历当前帧的地图点，记录所有能观测到当前帧地图点的关键帧
map<KeyFrame*,int> keyframeCounter;
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP->isBad())
{
// KeyFrame为观测到该地图点的关键帧，size_t为该地图点在KeyFrame关键帧中的索引
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
// 由于一个地图点可以被多个关键帧观测到,因此对于每一次观测,都对观测到这个地图点的关键帧进行累计投票
for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
// 这里的操作非常精彩！
// map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
// it->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
// 所以最后keyframeCounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点，即共视程度
keyframeCounter[it->first]++;
}
else
{
mCurrentFrame.mvpMapPoints[i]=NULL;
}
}
}
// 没有当前帧没有共视关键帧，返回
if(keyframeCounter.empty())
return;
// 存储具有最多观测次数（max）的关键帧
int max=0;
KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
// Step 2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有3种类型
// 先清空局部关键帧
mvpLocalKeyFrames.clear();
// 先申请3倍内存，不够后面再加
mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
// All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
// Step 2.1 类型1：能观测到当前帧地图点的关键帧作为局部关键帧 （将邻居拉拢入伙）（一级共视关键帧），即图中的 KF1和 KF2
for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
{
KeyFrame* pKF = it->first;
// 如果设定为要删除的，跳过
if(pKF->isBad())
continue;
// 寻找具有最大观测数目的关键帧，因为keyframeCounter->second 表示的就是当前帧和每个KeyFrame(共视关键帧)的共视程度。
if(it->second>max)
{
max=it->second;
pKFmax=pKF;
}
// 添加到局部关键帧的列表里
mvpLocalKeyFrames.push_back(it->first);
// 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id
// 表示它已经是当前帧的局部关键帧了，可以防止重复添加局部关键帧
pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
}
// Include also some not-already-included keyframes that are neighbors to already-included keyframes
// Step 2.2 遍历一级共视关键帧，寻找更多的局部关键帧，即遍历图中的 KF1 和 KF2。
for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
{
// Limit the number of keyframes
// 处理的局部关键帧不超过80帧
if(mvpLocalKeyFrames.size()>80)
break;
KeyFrame* pKF = *itKF;
// 类型2:一级共视关键帧的共视（前10个）关键帧，称为二级共视关键帧（将邻居的邻居拉拢入伙）
// 如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧
const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
// vNeighs 是按照共视程度从大到小排列
for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
{
KeyFrame* pNeighKF = *itNeighKF;
if(!pNeighKF->isBad())
{
// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pNeighKF);
pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
// 因为只找共视程度最强的。因为 mvpLocalKeyFrames 会随着找共视关系不断被添加然后扩大。因此只找最好的可以节省时间和空间。
break;
}
}
}
// 类型3:将一级共视关键帧的子关键帧作为局部关键帧（将邻居的孩子们拉拢入伙）
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;
//也是只找共视程度最高的子关键帧。
break;
}
}
}
// 类型3:将一级共视关键帧的父关键帧作为局部关键帧（将邻居的父母们拉拢入伙）
KeyFrame* pParent = pKF->GetParent();
if(pParent)
{
// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
// 因为 父关键帧只有一个，所以直接 break就ok。
break;
}
}
}
// Step 3：更新当前帧的参考关键帧，与自己共视程度最高（观测次数最多）的关键帧作为参考关键帧。
if(pKFmax)
{
mpReferenceKF = pKFmax;
mCurrentFrame.mpReferenceKF = mpReferenceKF;
}
}
