ORBSLAM3 运行流程 Track()函数
Track();函数是在TrackRGBD()函数里调用的
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 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 >(time_EndPreIMU - time_StartPreIMU).count();
vdIMUInteg_ms.push_back(timePreImu);
#endif
}
mbCreatedMap = false;
// Get Map Mutex -> Map cannot be changed
// 地图更新时加锁。保证地图不会发生变化
// 疑问:这样子会不会影响地图的实时更新?
// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
unique_lock 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.mnIdKeyFramesInMap()>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(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 vpMPsMM;
// 在追踪运动模型后发现的外点
vector 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; iIncreaseFound();
}
}
}
}
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 >(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 >(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; iObservations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
}
}
// Delete temporal MapPoints
// Step 9.3 清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
// 上个步骤中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
for(list::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 >(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(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
}