Track() 是 Tracking.cc 下的一个函数，虽然它不是主函数，新人可能要找个10分钟才可以看到它。
但这个函数着实是武则天垂帘听政，是曹操装作持刀侍卫，是伴舞小罗罗都倒地后出现的Michal，是幕后大boss
void Tracking::Track()
{
cout << "mCurrentFrame.mnId: " << mCurrentFrame.mnId << endl;
cout << "mpMap KF num: " << mpMap->KeyFramesInMap() << endl;
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Different operation, according to whether the map is updated
bool bMapUpdated = false;
if(mpLocalMapper->GetMapUpdateFlagForTracking())
{
bMapUpdated = true;
mpLocalMapper->SetMapUpdateFlagInTracking(false);
}
if(mpLoopClosing->GetMapUpdateFlagForTracking())
{
bMapUpdated = true;
mpLoopClosing->SetMapUpdateFlagInTracking(false);
}
if(mCurrentFrame.mnId == mnLastRelocFrameId + 20)
{
bMapUpdated = true;
}
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else
{
// System is initialized. Track Frame.
bool bOK;
bOK = true;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
if(!mbOnlyTracking)
{
// 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
CheckReplacedInLastFrame();
// If Visual-Inertial is initialized
cout << "mpLocalMapper->GetVINSInited(): " << mpLocalMapper->GetVINSInited() << endl;
if(mpLocalMapper->GetVINSInited())
{
// 20 Frames after reloc, track with only vision
cout << "mbRelocBiasPrepare: " << mbRelocBiasPrepare << endl;
if(mbRelocBiasPrepare)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithIMU(bMapUpdated);
cout << "TrackWithIMU(bMapUpdated): " << bOK << " bMapUpdated:"<< bMapUpdated << endl;
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
}
// If Visual-Inertial not initialized, keep the same as pure-vslam
else
{
cout << "mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2: "
<<mVelocity.empty() <<" || "<< (mCurrentFrame.mnId<mnLastRelocFrameId+2) << endl;
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel();
cout <<"TrackWithMotionModel(): " << bOK << endl;
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
}
cout << "Track result: " << bOK << endl;
}
else
{
bOK = Relocalization();
cout << "Relocalization()..." << endl;
if(bOK) cout<<"Relocalized. id: "<<mCurrentFrame.mnId<<endl;
else cout<< "failed" <<endl;
}
}
else
{
// Localization Mode: Local Mapping is deactivated
cerr<<"Localization mode not supported yet"<<endl;
}
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
if(!mbOnlyTracking)
{
if(bOK)
{
bOK = TrackLocalMap();
cout << "TrackLocalMap(): " << bOK << endl;
}
}
else
{
// Localization Mode: Local Mapping is deactivated
cerr<<"Localization mode not supported yet"<<endl;
}
if(bOK)
{
mState = OK;
// Add Frames to re-compute IMU bias after reloc
if(mbRelocBiasPrepare)
{
mv20FramesReloc.push_back(mCurrentFrame);
// Before creating new keyframe
// Use 20 consecutive frames to re-compute IMU bias
if(mCurrentFrame.mnId == mnLastRelocFrameId+20-1)
{
NavState nscur;
RecomputeIMUBiasAndCurrentNavstate(nscur);
// Update NavState of CurrentFrame
mCurrentFrame.SetNavState(nscur);
// Clear flag and Frame buffer
mbRelocBiasPrepare = false;
mv20FramesReloc.clear();
// Release LocalMapping. To ensure to insert new keyframe.
mpLocalMapper->Release();
// Create new KeyFrame
mbCreateNewKFAfterReloc = true;
//Debug log
cout<<"NavState recomputed."<<endl;
cout<<"V:"<<mCurrentFrame.GetNavState().Get_V().transpose()<<endl;
cout<<"bg:"<<mCurrentFrame.GetNavState().Get_BiasGyr().transpose()<<endl;
cout<<"ba:"<<mCurrentFrame.GetNavState().Get_BiasAcc().transpose()<<endl;
cout<<"dbg:"<<mCurrentFrame.GetNavState().Get_dBias_Gyr().transpose()<<endl;
cout<<"dba:"<<mCurrentFrame.GetNavState().Get_dBias_Acc().transpose()<<endl;
}
}
}
else
{
mState=LOST;
// Clear Frame vectors for reloc bias computation
if(mv20FramesReloc.size()>0)
mv20FramesReloc.clear();
}
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.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();
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
//cout << "mCurrentFrame.mTcw" << endl << mCurrentFrame.mTcw << endl;
// cout << "mVelocity:" << endl << mVelocity << endl;
// Clean VO matches
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
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
if(NeedNewKeyFrame() || mbCreateNewKFAfterReloc)
cout << "CreateNewKeyFrame()"<< endl;
CreateNewKeyFrame();
// Clear flag
if(mbCreateNewKFAfterReloc)
mbCreateNewKFAfterReloc = false;
// 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.
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
// Clear First-Init flag
if(mpLocalMapper->GetFirstVINSInited())
{
mpLocalMapper->SetFirstVINSInited(false);
}
}
// Reset if the camera get lost soon after initialization
if(mState==LOST)
{
//if(mpMap->KeyFramesInMap()<=5)
if(!mpLocalMapper->GetVINSInited())
{
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.
if(!mCurrentFrame.mTcw.empty())
{
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);
}
}