ORB-SLAM2学习（一、System框架搭建）—RGBD篇

参考链接：https://blog.csdn.net/qq_30356613/article/details/76409367 注释详细

https://blog.csdn.net/u010128736/article/details/53169832 思路清晰

---

• 整体流程如图所示：     

orb-slam主要有三个线程组成：跟踪、Local Mapping（又称小图）、Loop Closing（又称大图）。

（1）跟踪模块：主要是通过提取每一帧的ORB特征，通过恒速模型、关键帧模型、重定位估计相机初始位姿，然后通过共视关系跟踪局部局部地图来优化相机位姿进行优化，最后确定当前帧是否作为关键帧插入地图中。

 （2）局部建图：主要是针对跟踪过程中产生的关键帧进行操作，包括把该关键帧插入到地图中，添加新的地图点，剔除冗余的关键帧、地图点，通过Local BA优化相机位姿和地图点。

 （3）回环检测：同样针对关键帧进行操作，主要是通过BoW模型判断当前关键帧是否产生回环，如果产生可能的回环则进行回环一致性检测，通过一致性检测之后认为运动已经产生回环，则计算Sim3变换进行回环矫正，并另起线程进行全局优化。

其中跟踪模块需要处理摄像头获取的每一帧图像，从而实时估计相机位姿，而局部建图和回环检测通常只针对关键帧进行操作，因此不需要实时运行。

• 主程序：

由于我们将使用其做室外场景的RGBD，所以主要查看rgbd_tub.cc或ros_rgbd.cc（基于ros平台实现）主程 序，因为orb-slam配置realsense d435i方法基于ros平台跑通的，这里首 先查看ros_rgbd.cc主程序，将关键步骤做成思维导图如下所示：

         主函数中使用ROS编程的风格将该SLAM系统作为ROS上的一个节点来运行在ROS上面，通过订阅Kinect/Realsense D435I发布的话题，来获取Kinect/Realsense D435I上采集的图像信息。

---

Tip 1：

message_filters是一个用于roscpp和rospy的实用程序库，类似于一个消息缓存，当消息到达消息过滤器的时候，可能不会立即输出，而是在稍后的时间点满足一定条件下输出。（比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在相同时间戳的每个源上接收到消息才能输出它们，即起到一个消息同步输出的效果）。

message_filters::Subscriber sub(nh, "my_topic", 1);

sub.registerCallback(myCallback);

等同于ros::Subscriber sub = nh.subscribe("my_topic", 1, myCallback);

告诉master我们要订阅my_topic（（此处对应的是彩色相机与深度相机发布的topic：/kinect2/hd/image_color_rect与/kinect2/hd/image_depth_rect，采用不同的相机运行orb-slam2代码，需要查询相机发布的主题，并更改此处对应的topic话题））话题上的消息，当有消息发布到这个话题时，ros就会调用myCallback（）函数，1为队列大小，以防我们处理消息的速度不够快，当缓存达到1条后，再有新消息到来就将开始丢弃先前接收的消息。

https://blog.csdn.net/chishuideyu/article/details/77479758

http://wiki.ros.org/cn/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

https://blog.csdn.net/wheelfjl/article/details/78653321   多传感器融合时间对齐，暂未看，后期融合IMU时可参考使用（基于ros基址的时间同步）

 

• 系统构建：

下面查看orb-slam的system构建，在system.cc中实现，这个代码是所有调用SLAM系统的主入口，在这里，我们将看到前面博客所说的ORB_SLAM的三大模块：Tracking、Mapping和LoopClosing。

/***********************************************************************************************************************************************

在看system.cc之前，我们要了解整个系统的输入和输出分别是什么（借鉴冯冰）：

（1）输入：摄像头（包括图像+时间码），这个通过上面的ros_rgbd.cc已循环获得，并对其了深度图像与彩色图像的时间；

（2）输出：轨迹（每帧图像）+地图（关键帧+map point）,其中轨迹及关键帧的选取在tracking线程进行，地图获取在map线程实现。

更为具体的输入如下：

（1）图像+时间码（传感器融合）：每帧图像都有一个时间码，后期与传感器融合比较重要，对一些真实轨迹对比等比较重要；

（2）相机内参（如Asus.yaml）（标定好的，具体的标定方法后面会单独开一篇帖子总结）——相机尽量是个定焦镜头；

（3）提供相关配置文件：包括特征描述子对应的词汇表（后端会还检测时使用，需了解针对我们应用的场景该词汇表是否需特殊训练）（~/Vocabulary/ORBvoc.txt）、内部算法设置的全局经验值（也在Asus.yaml等文件里面）等；

**********************************************************************************************************************************************/

我们下面来看system.cc中的接口：

 

System的构建：

 

  可以看到，类system的构造函数主要做了以下工作：
（1）检测传感器的种类，
（2）将配置文件和词典读入内存中，
（3）建立线程，分为四个，一个是主线程（tracking线程），第二个为局部建图线程（mptLocalMapping线程），第三个为回环检测线程（LoopClosing线程），第四个为实时显示线程（根据bUseViewer而定）
（4）各线程之间建立联系及通信

 

• Tracking的函数入口：

       我们在上面的ros——rgbd.cc中看到，  在main函数中节点的主要工作是不断回调成员函数ImageGrabber::GrabRGBD，而在该成员函数中除了验证一下传输数据是否正常并将ROS图片转变为cv::Mat格式外主要的工作量在不断调用SLAM->TrackRGBD(parameter)函数，这个函数就是主线程里调用tracking的函数入口。那么该函数的具体工作在什么呢?我们来看一下：

      

         该函数主要的工作：
（1）检测输入传感器
（2）核查运行模式是否改变（主要就是核查mbActivateLocalizationMode和mbDeactivateLocalizationMode）
（3）检查是否需要复位
（4）计算变换矩阵T(GrabImageRGBD() 函数来计算)这一步将是主要的工作量所在

我们来详细看一下GrabImageRGBD

以上便是system的入口操作。在进行追踪线程之前，首先对新的数据（单目相机下为rgb图片，双目相机下为两张rgb图片，rgbd相机下为rgb图片和深度图）进行处理，将数据转化为灰度图，并根据数据建立新帧数据（在构建新帧数据时对新帧数据进行一些预处理，比如提取图片特征点ORBextrctor并计算关键点（位置方向）和描述子构建金字塔模型，对新帧的特征点位置根据矫正矩阵进行矫正）。

  而GrabImageRGBD()函数的具体实现就是线程Tracking.cpp中的内容了，我们将在下一节中详细整理线程Tracking，有错误的地方期待指教。