ORB_SLAM2代碼技術路線
- 代碼入口
- Tracking主線程
- LocalMapping線程
- LocalClosing線程(閉環檢測)
代碼入口
ORB_SLAM2具有多種工作模式，總體可以分為ROS版本的和非ROS版本，由于本人更喜歡ROS版本，是以在此隻讨論ROS版本的代碼。事實上，ROS版本和非ROS版本隻是資料擷取方式不同，比如ROS的圖像是通過ROS話題的形式讀取的，而非ROS版本則是從本地讀取圖檔。除此之外，後續的處理代碼用的是同一套代碼。
首先找到可執行檔案所對應的源檔案，在Example檔案夾的ROS檔案夾下。分别是ros_mono.cc ros_rgbd.cc ros_stereo.cc。ROS版本的代碼又可以分成三種模式，分别是單目、RGBD和雙目版本。
代碼大緻分為三部分，追蹤，局部地圖和閉環，三部分都包含了位姿的計算，追蹤部分是計算兩幀之間的位姿，局部地圖相當于一個滑動視窗，計算從追蹤部分添加進來的一定數量内的關鍵幀之間的位姿資訊，而回環部分則是檢測是否有閉環的出現，并且進行全局的位姿優化。
我們從單目版本開始分析，找到main()函數。
首先建立一個SLAM對象：
------------->ros_mono.cc
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
ImageGrabber igb(&SLAM);
在System.cc中完成了SLAM系統的初始化：
------------->System.cc
// Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
//構造函數，用來初始化整個系統。
System(const string &strVocFile, //指定ORB字典檔案的路徑
const string &strSettingsFile, //指定配置檔案的路徑
const eSensor sensor, //指定所使用的傳感器類型
const bool bUseViewer = true); //指定是否使用可視化界面
這個函數中設定了相機類型，系統除了在主線程中進行運動追蹤工作外，會建立局部建圖線程、回環檢測線程和檢視器線程（就是可視化工具）。
分别如下：
------------->System.cc
//在本主線程中初始化追蹤線程
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracking(this, //現在還不是很明白為什麼這裡還需要一個this指針 TODO
mpVocabulary, //字典
mpFrameDrawer, //幀繪制器
mpMapDrawer, //地圖繪制器
mpMap, //地圖
mpKeyFrameDatabase, //關鍵幀地圖
strSettingsFile, //設定檔案路徑
mSensor); //傳感器類型iomanip
//初始化局部建圖線程并運作
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, //指定使iomanip
mSensor==MONOCULAR);
//運作這個局部建圖線程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //這個線程會調用的函數
mpLocalMapper); //這個調用函數的參數
//Initialize the Loop Closing thread and launchiomanip
mpLoopCloser = new LoopClosing(mpMap, //地圖
mpKeyFrameDatabase, //關鍵幀資料庫
mpVocabulary, //ORB字典
mSensor!=MONOCULAR); //目前的傳感器是否是單目
//運作這個回環檢測線程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //線程的主函數
mpLoopCloser); //該函數的參數
線程三個x函數就是最重要的函數，點進去可以檢視函數具體内容。
并且在類ImageGrabber中定義了回調函數。
------------->ros_mono.cc
class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
void GrabImage(const sensor_msgs::ImageConstPtr& msg);
ORB_SLAM2::System* mpSLAM;
};
定義ROS句柄，設定圖像消息，其他相機模式所訂閱的消息會不一樣，感興趣的小夥伴可以去看看。
------------->ros_mono.cc
ros::NodeHandle nodeHandler;
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);
Tracking主線程
接下來系統将不斷地進入回調函數中執行功能函數。這個回調函數有點像單片機的硬體中斷，隻要接收到圖像，就會喚起回調函數進行處理。
回調函數執行功能函數：
------------->ros_mono.cc
mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
進入該函數内部，首先是判斷系統是否為定位模式，如果是定位模式，則關閉局部地圖線程。
------------->Tracking.cc
//擷取相機位姿的估計結果，在這裡進行位姿估計
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
再點進去
------------->Tracking.cc
//擷取相機位姿的估計結果，在這裡進行位姿估計
//構造Frame
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 沒有成功初始化的前一個狀态就是NO_IMAGES_YET
mCurrentFrame = Frame(
mImGray,
timestamp,
mpIniORBextractor, //LNOTICE 這個使用的是初始化的時候的ORB特征點提取器
mpORBVocabulary,
mK,
mDistCoef,
mbf,
mThDepth);
else
mCurrentFrame = Frame(
mImGray,
timestamp,
mpORBextractorLeft, //NOTICE 當程式正常運作的時候使用的是正常的ORB特征點提取器
mpORBVocabulary,
mK,
mDistCoef,
mbf,
mThDepth);
進入Frame()方法内，該方法主要是對圖像進行特征點檢測，這裡不同的相機類型會有所差别，彈幕和RGBD提取特征點的方法一樣，RGBD會多一個深度指派，雙目會開兩個線程分别對兩張圖像同時處理
------------->Frame.cc
//圖像跟蹤
//單目和RGBD圖像進行提取特征點操作是一樣的
ExtractORB(0,imGray);
//-------------------------------------------分割線，下面是雙目的
// 同時對左右目提特征，還同時開了兩個線程
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
//對右目圖像提取ORB特征，參數含義同上
thread threadRight(&Frame::ExtractORB,this,1,imRight);
//等待兩張圖像特征點提取過程完成
threadLeft.join();
threadRight.join();
//将特征點配置設定到圖像網格中
//應該是文章中介紹的讓特征點均勻分布的具體實作
AssignFeaturesToGrid();
Frame()執行完之後，執行Track()，進入該函數，包含了位姿估計初始化和跟蹤
------------->Tracking.cc
//忽略部分代碼
// step 1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
//雙目初始化(RGBD相機也當做為雙目相機來看待)
StereoInitialization();
else
//單目初始化
MonocularInitialization();
}
如果初始化已經完成，下面進行追蹤，當然實際過程還有更複雜的情況，主要代碼就是下面這兩個函數
作者允許允許兩種模式，第一種是非純追蹤模式，第二種是純追蹤模式，在第二種模式中可以看作是定位模式，關閉局部地圖，不插入關鍵幀，我們隻讨論第一種模式
------------->Tracking.cc
//恒速模型方法
// 通過投影的方式在參考幀中找目前幀特征點的比對點
TrackWithMotionModel();
//參考關鍵幀方法
//通過BoW的方式在參考幀中找目前幀特征點的比對點
// 優化每個特征點都對應3D點重投影誤差即可得到位姿
TrackReferenceKeyFrame()
這兩個函數表示的是兩種求位姿的方法，首選第一個函數，該函數是在系統正常追蹤的時候采用的方法，如果正常追蹤失敗了，才用第二個函數。如果還沒進行初始化則進行重定位。以上兩個函數都是尋找比對的點進行位姿計算。分别如下：
------------->Tracking.cc
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
//and
int nmatches = matcher.SearchByBoW( mpReferenceKF,mCurrentFrame,vpMapPointMatches);
然後兩個函數都講比對點存儲到 mCurrentFrame類中。有了比對的點，就可以進行優化了，優化代碼如下，其中傳入mCurrentFrame類，這個函數是位姿估計的重頭戲，通過g2o優化器進行優化。
------------->Tracking.cc
//通過優化3D-2D的重投影誤差來獲得位姿
Optimizer::PoseOptimization(&mCurrentFrame);
點進這個函數，裡面就是g2o的優化套路，最後計算得到位姿。
------------->Optimizer.cc
// 得到優化後的目前幀的位姿
cv::Mat pose = Converter::toCvMat(SE3quat_recov);
pFrame->SetPose(pose);
LocalMapping線程
回顧前面講到的線程建立代碼中，在System.cc
------------->Optimizer.cc
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //這個線程會調用的函數
mpLocalMapper); //這個調用函數的參數
這個線程的實作函數在LocalMapping.cc檔案中
------------->LocalMapping.cc
void LocalMapping::Run()
{
// 檢視清單中是否有等待被插入的關鍵幀
if(CheckNewKeyFrames())
{
// 計算關鍵幀特征點的BoW映射，将關鍵幀插入地圖
ProcessNewKeyFrame();
// 剔除ProcessNewKeyFrame函數中引入的不合格MapPoints
MapPointCulling();
// 相機運動過程中與相鄰關鍵幀通過三角化恢複出一些MapPoints
CreateNewMapPoints();
// 檢查并融合目前關鍵幀與相鄰幀（兩級相鄰）重複的MapPoints
SearchInNeighbors();
進行局部地圖的BA
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// 檢測并剔除目前幀相鄰的關鍵幀中備援的關鍵幀
KeyFrameCulling();
// 将目前幀加入到閉環檢測隊列中
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
}
代碼中還有許多線程協同工作的細節，本文隻是理清楚代碼的粗略運作方式，是以不深究
LocalClosing線程(閉環檢測)
------------->LoopClosing.cc
void LoopClosing::Run()
{
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
//檢測是否有回環
if(DetectLoop())
{
// Compute similarity transformation [sR|t]---sim3比se3多了一個相對尺度s
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
//在此處進行回環的BA
CorrectLoop();
}
}
}
}
![Jericho Html Parser初探[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)