從另一個角度看ORB-SLAM3——第0幀
原始碼：https://github.com/UZ-SLAMLab/ORB_SLAM3
上電時系統處於NO_IMAGES_YET狀態，如果這時第一張圖片，即第0幀，被系統讀取，它會經過哪些函式，會被系統如何處理呢？
1. 主函式
以mono_inertial_euroc為例，main()在mono_inertial_euroc.cc中。
1.1
系統首先讀取圖片路徑和時間戳，IMU測量值、引數和時間戳。
LoadImages(pathCam0, pathTimeStamps, vstrImageFilenames[seq], vTimestampsCam[seq]); LoadIMU(pathImu, vTimestampsImu[seq], vAcc[seq], vGyro[seq]);
1.2
用預訓練好的詞袋、yaml配置檔案初始化並啟動系統的4個執行緒，包括跟蹤mpTracker、建圖mpLocalMapper、迴環mpLoopCloser和視覺化mpViewer。
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
之後的工作就很直接了，系統需要將每一幀im、時間戳tframe和對應的IMU測量值vImuMeas傳遞到跟蹤執行緒。函式入口是：
SLAM.TrackMonocular(im,tframe,vImuMeas);
我們只考慮第0幀，來看看這個函式對第0幀做了什麼處理。
2. 跟蹤
2.1
首先檢查是否有純定位模式和系統重置被要求使能。
// Check mode change unique_lock<mutex> lock(mMutexMode); if(mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper->isStopped()) { usleep(1000); } mpTracker->InformOnlyTracking(true); mbActivateLocalizationMode = false; } if(mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); mbDeactivateLocalizationMode = false; } // Check reset unique_lock<mutex> lock(mMutexReset); if(mbReset) { mpTracker->Reset(); mbReset = false; mbResetActiveMap = false; } else if(mbResetActiveMap) { cout << "SYSTEM-> Reseting active map in monocular case" << endl; mpTracker->ResetActiveMap(); mbResetActiveMap = false; }
2.2
傳遞進來的IMU測量值被儲存在佇列mlQueueImuData中。
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
之後跟蹤傳遞進來的影象，計算它的位姿Tcw。因為這裡處理的是第0幀，Tcw應該是一個4維的單位矩陣。
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp,filename);
下面看看GrabImageMonocular()的實現細節，和第0幀處理無關的就直接略去。
3. GrabImageMonocular()
3.1
將第0幀轉成灰度圖。
cvtColor(mImGray,mImGray,cv::COLOR_RGB2GRAY);
此時系統狀態mState為NO_IMAGES_YET，用灰度圖建立當前幀mCurrentFrame。
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);
記錄下Id後，Track()開始跟蹤當前幀。Track()和系統所使用的感測器無關，無論是單目、雙目還是IMU方案，都是執行該函式來估計每一幀的位姿。
lastID = mCurrentFrame.mnId;
Track();
3.2 Track()
Track()對第0幀的處理很簡單。首先改變系統狀態為NOT_INITIALIZED，為讀取下一幀進行初始化做準備。
if(mState==NO_IMAGES_YET)
mState = NOT_INITIALIZED;
呼叫PreintegrateIMU()計算幀幀之間的預積分。因為第0幀沒有對應的IMU測量值傳遞進來，不滿足條件，mbImuPreintegrated置1，然後被返回。
if(mlQueueImuData.size() == 0)
{
Verbose::PrintMess("Not IMU data in mlQueueImuData!!", Verbose::VERBOSITY_NORMAL);
mCurrentFrame.setIntegrated();
return;
}
之後根據系統感測器型別進入MonocularInitialization()執行單目初始化。
3.3 MonocularInitialization()
在跟蹤執行緒初始化時，mpInitializer指標為空，條件判斷成功，進入主體設定用於單目初始化的參考幀。
if(!mpInitializer)
{
// Set Reference Frame
if(mCurrentFrame.mvKeys.size()>100)
{
mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
if(mpInitializer)
delete mpInitializer;
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
if (mSensor == System::IMU_MONOCULAR)
{
if(mpImuPreintegratedFromLastKF)
{
delete mpImuPreintegratedFromLastKF;
}
mpImuPreintegratedFromLastKF = new IMU::Preintegrated(IMU::Bias(),*mpImuCalib);
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
}
return;
}
}
mCurrentFrame.mvKeys.size()>100判斷當前幀提取的特徵點數是否大於100。如果滿足條件，設定初始幀mInitialFrame和最新幀mLastFrame均為當前幀，並將去畸變後的特徵點座標傳給mvbPrevMatched。delete mpInitializer這一步我沒看明白，外層條件語句已經判斷了mpInitializer為空指標，根本無法進入這一層的條件判斷，我把這兩行註釋後，程式執行似乎也沒出問題(?)。現在可以用當前幀重新設定mpInitializer了，fill()函式初始化mvIniMatches元素為-1，表示未進行特徵點匹配或匹配未成功。根據ORB-SLAM3的論文，系統會先跑2秒的純視覺，用來初始化IMU，包括bias、尺度、重力方向和前幾幀的速度，系統從第0幀開始累計預積分，使用yaml配置檔案給出的IMU引數設定第0幀的預積分，並給定bias初值為0。設定完成後，返回到上一層函式Track()。
3.4
最後一步，判斷初始化是否成功。到這裡我們只處理了第0幀，因此係統狀態仍舊是NOT_INITIALIZED。進入條件語句主體，設定最新幀為當前幀。當然這一步對於第0幀是多餘的，因為在MonocularInitialization()中，已經設定過一次最新幀了。
if(mState!=OK) // If rightly initialized, mState=OK
{
mLastFrame = Frame(mCurrentFrame);
return;
}
對第0幀的處理就全結束了，下次看看對第1幀及單目初始化是怎麼處理的。