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=)