void Tracking::Track()
{
#ifdef SAVE_TIMES
mTime_PreIntIMU = 0;
mTime_PosePred = 0;
mTime_LocalMapTrack = 0;
mTime_NewKF_Dec = 0;
#endif
if (bStepByStep)
{
while(!mbStep)
usleep(500);
mbStep = false;
}
if(mpLocalMapper->mbBadImu)
{
cout << "TRACK: Reset map because local mapper set the bad imu flag " << endl;
mpSystem->ResetActiveMap();// IMU数据有问题则重置地图
return;
}
Map* pCurrentMap = mpAtlas->GetCurrentMap();//找地图，没有就创建
if(mState!=NO_IMAGES_YET)
{
if(mLastFrame.mTimeStamp>mCurrentFrame.mTimeStamp)
{
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 << "id last: " << mLastFrame.mnId << " id curr: " << mCurrentFrame.mnId << endl;
if(mpAtlas->isInertial())
{
if(mpAtlas->isImuInitialized())
{
cout << "Timestamp jump detected. State set to LOST. Reseting IMU integration..." << endl;
if(!pCurrentMap->GetIniertialBA2())
{
mpSystem->ResetActiveMap();
}
else
{
CreateMapInAtlas();
}
}
else
{
cout << "Timestamp jump detected, before IMU initialization. Reseting..." << endl;
mpSystem->ResetActiveMap();
}
}
return;
}
}
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && mpLastKeyFrame)
mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
mLastProcessedState=mState;
if ((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO) && !mbCreatedMap)
{
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
#endif
PreintegrateIMU();
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
mTime_PreIntIMU = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t1 - t0).count();
#endif
}
mbCreatedMap = false;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(pCurrentMap->mMutexMapUpdate);
mbMapUpdated = false;
int nCurMapChangeIndex = pCurrentMap->GetMapChangeIndex();
int nMapChangeIndex = pCurrentMap->GetLastMapChange();
if(nCurMapChangeIndex>nMapChangeIndex)
{
// cout << "Map update detected" << endl;
pCurrentMap->SetLastMapChange(nCurMapChangeIndex);
mbMapUpdated = true;
}
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO)
StereoInitialization();//初始化成功后，mState状态置为ok
else
{
MonocularInitialization();
}
mpFrameDrawer->Update(this);
if(mState!=OK) // If rightly initialized, mState=OK
{
mLastFrame = Frame(mCurrentFrame);//如果没有初始化成功则会一直刷新帧，直到可以初始化成功
return;
}
if(mpAtlas->GetAllMaps().size() == 1)
{
mnFirstFrameId = mCurrentFrame.mnId;
}
}
else
{
// System is initialized. Track Frame.
bool bOK; // bOK为临时变量，bOK==true现在tracking状态正常,能够及时的反应现在tracking的状态
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
if(!mbOnlyTracking)// 建图加定位追踪
{
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point timeStartPosePredict = std::chrono::steady_clock::now();
#endif
// State OK
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
//lastframe中可以看到的mappoint替换为lastframe储存的备胎mappoint点mpReplaced，也就是更新mappoint
//检查并更新上一帧被替换的MapPoints,更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
//如果运动模型是空的 && IMU 未初始化||当前帧ID与重定位帧ID之间相差大于2,刚刚完成重定位
//跟踪上一帧或者参考帧或者重定位
if((mVelocity.empty() && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
//Verbose::PrintMess("TRACK: Track with respect to the reference KF ", Verbose::VERBOSITY_DEBUG);
/**
* 将参考帧关键帧的位姿作为当前帧的初始位姿进行跟踪；
* 匹配参考帧关键帧中有对应mappoint的特征点与当前帧特征点，通过dbow加速匹配；
* 优化3D点重投影误差，得到更精确的位姿以及剔除错误的特征点匹配；
* @return 如果匹配数大于10，返回true
*/
bOK = TrackReferenceKeyFrame();
}
else
{
/**
* 根据恒速模型设定当前帧的初始位姿,通过投影的方式在参考帧中找当前帧特征点的匹配点,
* 优化每个特征点所对应3D点的投影误差即可得到位姿
*/
//Verbose::PrintMess("TRACK: Track with motion model", Verbose::VERBOSITY_DEBUG);
bOK = TrackWithMotionModel();
if(!bOK)
//恒速模型失败了再跟踪参考关键帧
bOK = TrackReferenceKeyFrame();
}
if (!bOK)
{ //如果是VIO模式，刚刚进行过重定位就tracking丢失了
if ( mCurrentFrame.mnId<=(mnLastRelocFrameId+mnFramesToResetIMU) &&
(mSensor==System::IMU_MONOCULAR || mSensor==System::IMU_STEREO))
{
mState = LOST;//直接设置为丢失
}
else if(pCurrentMap->KeyFramesInMap()>10)//如果当前地图中的关键帧数量大于10
{
cout << "KF in map: " << pCurrentMap->KeyFramesInMap() << endl;
mState = RECENTLY_LOST;//设置为最近的丢失，进入下面的最近丢失
mTimeStampLost = mCurrentFrame.mTimeStamp;//记录丢失的时间
//mCurrentFrame.SetPose(mLastFrame.mTcw);
}
else
{
mState = LOST;// LOST其他情况直接丢失
}
}
}
else
{
if (mState == RECENTLY_LOST)//如果是最近丢失
{
Verbose::PrintMess("Lost for a short time", Verbose::VERBOSITY_NORMAL);
bOK = true;
if((mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO))
{
if(pCurrentMap->isImuInitialized())
PredictStateIMU();//利用IMU进行跟踪，最多跟踪5s
else
bOK = false;
if (mCurrentFrame.mTimeStamp-mTimeStampLost>time_recently_lost)//time_recently_lost 的值为5s，如果时间大于5s就设置为丢失
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
else
{
// TODO fix relocalization
//BOW搜索候选关键帧，PnP求解位姿
//重定位,BOW搜索，PnP求解位姿
bOK = Relocalization();//上面的IF还没有解决，就开始重定位，再不行就设置丢失
if(!bOK)
{
mState = LOST;
Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
bOK=false;
}
}
}
else if (mState == LOST)//如果丢失了
{
Verbose::PrintMess("A new map is started...", Verbose::VERBOSITY_NORMAL);
if (pCurrentMap->KeyFramesInMap()<10)//查看地图中关键帧的数目，如果关键帧数量小于10，就重置地图
{
mpSystem->ResetActiveMap();
cout << "Reseting current map..." << endl;
}else
CreateMapInAtlas();//再不行就重建地图
if(mpLastKeyFrame)
mpLastKeyFrame = static_cast<KeyFrame*>(NULL);//删除mpLastKeyFrame从头开始
Verbose::PrintMess("done", Verbose::VERBOSITY_NORMAL);
return;
}
}
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point timeEndPosePredict = std::chrono::steady_clock::now();
mTime_PosePred = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(timeEndPosePredict - timeStartPosePredict).count();
#endif
}
else
{
// Localization Mode: Local Mapping is deactivated (TODO Not available in inertial mode)
if(mState==LOST)
{
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
bOK = Relocalization();
}
else
{
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
if(!mVelocity.empty())
{
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.
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)
{
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，如果当前帧不是新的关键帧，就用上一时刻的参考关键帧，否则参考关键帧就是它自己
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
/**************************************************************************************************************************************************
***************************************************************************************************************************************************
**************************************************************************************************************************************************
// If we have an initial estimation of the camera pose and matching. Track the local map.
* 在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
* local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系
* 在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
* 然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
*
*
* 前端线程首先通过匀速运动模型，跟踪临近关键帧，或是重定位的方法对当前帧的位姿做了初始估计。
* 接下来，因为当前帧无论是通过上述三种方法的哪一种，基本都是可以看到已存地图中的一些点，同时自然是会和一部分关键帧产生共视关系的。
* 观测地图点的信息存在了mCurrentFrame.mvpMapPoints里面，然后对于这些点有共视的关键帧，通过MapPoint的GetObservations()就可以轻松获取。
* 虽然有共视，但之前的追踪效果是有限的，当前帧和这些帧的共视关系还并未全部找出来。
* 此外，这些帧周围的一些关键帧，也是非常有可能和当前关键帧存在共视的。
* 如果我们能够获取更多的共视关系，便可用poseBA进一步优化当前位姿。
* 因为即使是前台线程算出的初始值，也是非常重要的，无论这一帧是不是关键帧。
* 先说是关键帧，插入较为准确的关键帧是直接影响全局轨迹精度的。
* 要不是关键帧呢，在trackmotionmodel模式下，普通帧的轨迹也是派上用场的。
* 所以作者在这里用心良苦，选择去追踪一个被称为localmap的东西，根本目的就是优化当前帧位姿，一切为了精度。 *
*
**************************************************************************************************************************************************
**************************************************************************************************************************************************
***************************************************************************************************************************************************/
if(!mbOnlyTracking)
{
if(bOK)//跟踪成功的状态（前面的关卡都过了，终于跟踪成功了）
{
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point time_StartTrackLocalMap = std::chrono::steady_clock::now();
#endif
bOK = TrackLocalMap();//bOK 转为跟踪局部地图是否OK的标志, 跟踪局部地图，优化当前帧位姿，进一步提高精度
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point time_EndTrackLocalMap = std::chrono::steady_clock::now();
mTime_LocalMapTrack = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndTrackLocalMap - time_StartTrackLocalMap).count();
#endif
}
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.
// mbVO值为true表示有很少的匹配点和地图中的MapPoint相匹配，我们无法检索本地地图因此此时不调用TrackLocalMap函数。
// 一旦系统重新定位相机位置，我们将再次使用局部地图
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)//局部地图跟踪成功，mState置为ok
mState = OK;
else if (mState == OK)//局部地图跟踪失败，但是局部地图跟踪之前的位姿是成功的
{
if (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
{
Verbose::PrintMess("Track lost for less than one second...", Verbose::VERBOSITY_NORMAL);
if(!pCurrentMap->isImuInitialized() || !pCurrentMap->GetIniertialBA2())
{
cout << "IMU is not or recently initialized. Reseting active map..." << endl;
mpSystem->ResetActiveMap();
}
//IMU模式下置为 最近的丢失
mState=RECENTLY_LOST;
}
else
mState=LOST; // visual to 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)) && 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)
{
if(mCurrentFrame.mnId==(mnLastRelocFrameId+mnFramesToResetIMU))
{
cout << "RESETING FRAME!!!" << endl;
ResetFrameIMU();
}
else if(mCurrentFrame.mnId>(mnLastRelocFrameId+30))
mLastBias = mCurrentFrame.mImuBias;
}
}
// Update drawer
mpFrameDrawer->Update(this);
if(!mCurrentFrame.mTcw.empty())
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
if(bOK || mState==RECENTLY_LOST)
{
// Update motion model
if(!mLastFrame.mTcw.empty() && !mCurrentFrame.mTcw.empty())
{
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;
}
else
mVelocity = cv::Mat();
if(mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO)
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
//如果该MapPoint没有被其他帧观察到，则将该MapPoint设置为NULL,也就是去掉
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints 删除暂时的MapPoint
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
mlpTemporalPoints.clear();
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point timeStartNewKF = std::chrono::steady_clock::now();
#endif
bool bNeedKF = NeedNewKeyFrame();
#ifdef SAVE_TIMES
std::chrono::steady_clock::time_point timeEndNewKF = std::chrono::steady_clock::now();
mTime_NewKF_Dec = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(timeEndNewKF - timeStartNewKF).count();
#endif
// Check if we need to insert a new keyframe
// 判断是否需要插入关键帧
if(bNeedKF && (bOK|| (mState==RECENTLY_LOST && (mSensor == System::IMU_MONOCULAR || mSensor == System::IMU_STEREO))))
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. Only has effect if lastframe is tracked
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
//如果初始化后不久相机丢失，则进行重置
if(mState==LOST)
{
if(pCurrentMap->KeyFramesInMap()<=5)
{
mpSystem->ResetActiveMap();
return;
}
if ((mSensor == System::IMU_MONOCULAR) || (mSensor == System::IMU_STEREO))
if (!pCurrentMap->isImuInitialized())
{
Verbose::PrintMess("Track lost before IMU initialisation, reseting...", Verbose::VERBOSITY_QUIET);
mpSystem->ResetActiveMap();
return;
}
CreateMapInAtlas();
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
//Tracking完成的时候，记录当前帧为上一帧
mLastFrame = Frame(mCurrentFrame);
}
//3：存储帧的位姿信息，稍后用来重现完整的相机运动轨迹
if(mState==OK || mState==RECENTLY_LOST)
{
// Store frame pose information to retrieve the complete camera trajectory afterwards.
if(!mCurrentFrame.mTcw.empty())
{
cv::Mat Tcr = mCurrentFrame.mTcw*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);
}
}
}
3456
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
