ORB_SLAM2代碼閱讀(1）——系統(tǒng)簡介

• 1、說明
• 2、簡介
• 3、stereo_kitti.cc
• 4、SLAM系統(tǒng)文件（System.cc）
• • • 4.1 構造函數(shù)System()
    • 4.2 TrackStereo()
    • 4.3 SaveTrajectoryKITTI (const string &filename)
• 5.總結

1、說明

本文以～/ORB_SLAM2/Examples/Stereo/stereo_kitti.cc 為起點一步步閱讀ORB_SLAM2代碼。由于本人也是初學，閱讀代碼過程中難免出現(xiàn)錯誤，希望各位諒解。

2、簡介

ORB-SLAM是由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年發(fā)表在IEEE Transactions on Robotics。ORB-SLAM是一個基于特征點的實時單目SLAM系統(tǒng)，在大規(guī)模的、小規(guī)模的、室內(nèi)室外的環(huán)境都可以運行。該系統(tǒng)對劇烈運動也很魯棒，支持寬基線的閉環(huán)檢測和重定位，包括全自動初始化。該系統(tǒng)包含了所有SLAM系統(tǒng)共有的模塊：跟蹤（Tracking）、建圖（Mapping）、重定位（Relocalization）、閉環(huán)檢測（Loop closing）。由于ORB-SLAM系統(tǒng)是基于特征點的SLAM系統(tǒng)，故其能夠?qū)崟r計算出相機的軌線，并生成場景的稀疏三維重建結果。ORB-SLAM2在ORB-SLAM的基礎上，還支持標定后的雙目相機和RGB-D相機。

其系統(tǒng)結構圖如下圖所示：

可以看到ORB-SLAM主要分為三個線程進行，分別是Tracking、LocalMapping和LoopClosing。三個線程分別存放在對應的三個文件中，分別是Tracking.cpp、LocalMapping.cpp和LoopClosing.cpp文件中，很容易找到。

3、stereo_kitti.cc

stereo_kitti.cc 是針對KITTI數(shù)據(jù)集的雙目ORB_SLAM示例。該示例比較簡單，主要包括四部分：

1. 載入圖像 ：根據(jù)命令行輸入的參數(shù)，載入在相關文件夾下圖像和時間戳。LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);*注 ：該過程只是將圖像的名稱保存在相應的vector變量中。
2. 創(chuàng)建SLAM系統(tǒng)：根據(jù)命令行輸入的參數(shù)創(chuàng)建SLAM系統(tǒng)并進行系統(tǒng)初始化。ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);其中：argv[1]表示詞袋文件的路徑，argv[2]表示配置文件的路徑，ORB_SLAM2::System::STEREO表示創(chuàng)建SLAM系統(tǒng)為雙目SLAM系統(tǒng)，true表示進行可視化操作。
3. 傳入圖像進行追蹤 ：通過一個循環(huán)過程將圖像傳入SLAM系統(tǒng)中進行追蹤。SLAM.TrackStereo(imLeft,imRight,tframe);
4. 停止SLAM系統(tǒng)并保存軌跡：通過Shutdown()函數(shù)停止SLAM系統(tǒng)中的所有線程，利用SaveTrajectoryKITTI()函數(shù)來保存軌跡。

該示例主要內(nèi)容就是這三部分，其他內(nèi)容主要是相關變量的定義和追蹤時間的統(tǒng)計過程。

4、SLAM系統(tǒng)文件（System.cc）

ORB_SLAM2作者將整個系統(tǒng)進行了完整的封裝并定義了一個System類作為系統(tǒng)的入口。System類的類圖如下圖所示：

4.1 構造函數(shù)System()

在System類的構造函數(shù)中，主要執(zhí)行以下操作：

1. 判斷傳感器類型
2. 檢驗配置文件
3. 載入詞袋
4. 實例化相關成員變量：包括局部建圖線程、回環(huán)檢測線程和視圖線程的實例化。注:主線程即為追蹤線程！
5. 各個線程之間交叉設置指針

4.2 TrackStereo()

System類的構造函數(shù)將系統(tǒng)運行的所有條件都已經(jīng)準備就緒，等待圖片序列的傳入。在stereo_kitti.cc示例中通過SLAM.TrackStereo(imLeft,imRight,tframe)傳入圖片序列。因此，我們也選擇該函數(shù)作為切入點進行閱讀（TrackRGBD()、TrackMonocular()與TrackStereo()大同小異，不再贅述）。

cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)
{if(mSensor!=STEREO){cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;exit(-1);}   // Check mode change {unique_lock<mutex> lock(mMutexMode);  //在該聲明周期內(nèi)對 mMutexMode 進行上鎖操作，不允許其他線程修改定位模式if(mbActivateLocalizationMode)        //激活定位模式{mpLocalMapper->RequestStop();// Wait until Local Mapping has effectively stoppedwhile(!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;}}cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);unique_lock<mutex> lock2(mMutexState);mTrackingState = mpTracker->mState;//更新追蹤狀態(tài)mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//更新地圖點向量mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//更新關鍵點向量return Tcw;//返回相機坐標系到世界坐標系的變換矩陣
}

在TrackStereo()函數(shù)中主要執(zhí)行以下操作：

1. 首先檢驗傳感器類型。若傳感器類型錯誤則直接退出。
2. 檢驗定位模式的變化。若定位模式被激活，則關閉局部建圖功能，系統(tǒng)只進行定位。若定位模式被關閉，則之前建立的局部地圖被釋放（清除）。
3. 檢驗系統(tǒng)是否被重啟（有UI界面控制），若系統(tǒng)重啟，則重啟追蹤器。
4. 為追蹤器輸入圖像數(shù)據(jù)，返回世界坐標系到相機坐標系的變換矩陣。
5. 更新變量

4.3 SaveTrajectoryKITTI (const string &filename)

保存軌跡函數(shù)值得詳細說明一下。

void System::SaveTrajectoryKITTI(const string &filename)
{cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;if(mSensor==MONOCULAR){cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;return;}vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();//獲取所有的關鍵幀sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//對關鍵幀進行排序// Transform all keyframes so that the first keyframe is at the origin.// After a loop closure the first keyframe might not be at the origin.cv::Mat Two = vpKFs[0]->GetPoseInverse();ofstream f;f.open(filename.c_str());f << fixed;// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).// We need to get first the keyframe pose and then concatenate the relative transformation.// Frames not localized (tracking failure) are not saved.// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag// which is true when tracking failed (lbL).// 鏈表mlpReferences保存了所有圖像幀的參考關鍵幀// 鏈表mlRelativeFramePoses保存了所有圖像幀的相對于參考幀的姿態(tài)變換關系list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();list<double>::iterator lT = mpTracker->mlFrameTimes.begin();for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++){ORB_SLAM2::KeyFrame* pKF = *lRit;cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);while(pKF->isBad()){//  cout << "bad parent" << endl;Trw = Trw*pKF->mTcp;pKF = pKF->GetParent();}Trw = Trw*pKF->GetPose()*Two;//計算全局坐標系到參考關鍵幀坐標系變換關系cv::Mat Tcw = (*lit)*Trw;              //計算全局坐標系到相機坐標系的變換關系，注：Tcw是全局坐標系到相機坐標系的變換/*************************************************************************                       Tcw是全局坐標系到相機坐標系的變換*                      Tcw的逆為相機坐標系到全局坐標系的變換*                         變換矩陣的逆可由下式計算*                          T^-1 =[ R^T    -R^T*t   *                                   0^T      1   ]*  * **********************************************************************/cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//根據(jù)Tcw計算相機坐標系到全局坐標系的旋轉矩陣cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);         //根據(jù)Tcw計算相機坐標系到全局坐標系的平移向量//將旋轉矩陣和平移向量寫入文件f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1)  << " " << Rwc.at<float>(0,2) << " "  << twc.at<float>(0) << " " <<Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1)  << " " << Rwc.at<float>(1,2) << " "  << twc.at<float>(1) << " " <<Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1)  << " " << Rwc.at<float>(2,2) << " "  << twc.at<float>(2) << endl;}f.close();cout << endl << "trajectory saved!" << endl;
}

追蹤器中維護了兩個重要的鏈表——mlpReferences 和 mlRelativeFramePoses。其中mlpReferences保存了所有圖像幀的參考關鍵幀；mlRelativeFramePoses保存了圖像幀的相對于參考幀的姿態(tài)變換關系，即圖像與參考關鍵幀之間的變換矩陣。這正是由于這兩個鏈表的存在，我們才能將整個運動軌跡保存下來。

計算每一幀圖像相對于原點處的姿態(tài)的流程如下：

1. 從 mpMap 變量處獲取所有關鍵幀并對這些關鍵幀按ID號進行排序。
2. 獲取第一個關鍵幀的位姿信息。
3. 根據(jù)鏈表mlpReferences和mlRelativeFramePoses的數(shù)據(jù)，以遍歷的方式計算每一幀的姿態(tài)。在該過程中：
   1）.首先計算全局坐標系與參考關鍵幀之間的變換關系Trw。
   2）.計算當前幀相對于全局坐標系的變換關系Tcw。注：Tcw是全局坐標系到相機坐標系的變換，而最終要求的結果是相機坐標系到全局坐標系的變換關系。兩者互為逆運算。
   3）.根據(jù)Tcw計算相機坐標系到全局坐標系的旋轉矩陣Rwc和平移向量twc。

5.總結

作為整個ORB_SLAM2系統(tǒng)的入口,System類并不是很那理解，我對其中的內(nèi)容解釋的并不多，如果逐行閱讀代碼，實在是太費唇舌。我相信大家都能看明白，所以講的粗糙一點。下一篇博客一起閱讀關于追蹤部分的代碼，我盡力記錄的詳細些。如有錯誤或遺漏，歡迎留言！希望大家支持！

總結

以上是生活随笔為你收集整理的ORB_SLAM2代码阅读(1)——系统入口的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。