经典基于外观的SLAM框架-RTABMAP

1. RTABMAP整体框架

  RTABMAP是采用优化算法的方式求解SLAM问题的SLAM框架，本赛题的定位参考输入信息只有RGB-D相机的图像信息，RTABMAP支持RGB-D视觉信息的输入，并且输出包括位姿、二维占据栅格地图（2D Occupancy）、三维占据地图（3D Occupancy）和点云地图，输出地图具有多样性，RTABMAP有分级的内存管理机制，能够对输入帧进行图结构建模和统筹规划，将建图部分的计算量简化到设备可以承受的范围内。
视觉传感器有两个优点，其一是定位的位姿具有鲁棒性，不易丢失；其二是感知到的信息量较大，与激光传感器相比得到的语义更加丰富。里程计能提供短期运动信息，这种信息可以给位姿估计提供预测信息，当环境特征缺失时，里程计提供的短期运动信息也可以参考处理。RTABMAP的系统框架图：

  在RTABMAP中，视觉传感器是必备的输入传感器，另外可以输入激光雷达传感器作为选配输入传感器，当然也可以将RGB-D的信息通过转换工具转换为激光点数据，一般这种方案存在于接入ros-navigation工具包使用，该工具包的必备输入是激光扫描输入，RTABMAP接入里程计的形式可以是轮式里程计、视觉里程计、激光里程计等，由于赛题条件的限制，我们采用视觉里程计，RTABMAP托管机器人坐标和地图坐标系的转换关系完成位姿估计，传感器数据会存入STM（Short-Term Memory），这段内存块被称为短期内存模块，由于RTABMAP采用图结构来组织地图，图结构包含节点和边，所以每一帧传感器数据的到来都会创建一个节点，该节点中存储的内容包括该帧对应的里程计位姿、该帧所有传感器的视觉观测数据以及在该帧上提取出的视觉单词（基于词袋模型的回环检测）和局部地图等。节点之间的连接边分为三种：

1. 相邻连接边。相邻连接边里程计能够直接获得相邻连节点之间的位姿变换关系。
2. 闭环连接边。闭环连接边基于视觉词袋的闭环检测和多视图几何计算出当前节点与闭环节点之间的位姿变换关系。
3. 相似连接边。相似连接边主要用于激光扫描数据相关的闭环检测，因为激光扫描设备对于巡视器本身的转向不敏感，更容易在闭环检测上出现问题。

  在闭环检测检测到闭环时，RTABMAP将所有的节点和所有的边进行全局优化，优化的过程通过图优化模块进行，同时会对内部的位姿漂移问题进行优化（主要是修正地图点到里程计的变换关系），经过修正后的节点中存储的局部地图通过拼接集成全局地图。

2.RTABMAP的内存管理机制

  由于视觉传感器的每一帧到来都会创建一个节点，那么建图规模达到很大之后，节点的数量也是同样非常大的，在此种情况下，对所有进行进行全局优化的计算将会十分的耗费资源甚至难以行进，为了保证优化和回环检测的实时性，RTABMAP采用了分级的内存管理机制。
  RTABMAP采用图结构维护地图结构，将所有的节点分为三类，即存储局部地图节点的STM、存储全局地图节点的工作内存WM（Working Memory）、存储短期与全局地图缺乏相关性的不重要节点的长期内存LTM（Long-Term Memory），节点的分类实例如下图所示，图中横向的箭头表征相邻连接边，竖向的箭头表征闭环连接边，灰色节点存储在STM中，白色节点存储在WM中，黑色节点处于LTM中，编号455节点外围有黑色外圈，表征当前巡视器位姿，其中每个节点都包含视觉传感器观测的数据、里程计的位姿信息和各连接边信息等。

  RGB-D的视觉传感器数据输入SM内存块，本内存块对传感器的观测数据进行数据降维、特征提取、位姿计算后，加入STM内存块，在STM内存块添加时序相邻的节点若相似度较高，则对两个节点进行权重更新的融合计算，当STM区满后，挑选最早加入STM的节点移出，并加入到WM进行后续的闭环检测搜索节点储备，WM中的闭环检测涉及词袋模型和贝叶斯滤波，视觉词袋用于计算两个节点之间的相似度，贝叶斯滤波器维护节点之间的相似性。当WM内存区满后，挑选最早进入WM区的节点移出，并加入到LTM中。内存管理机制的框图如下图所示。

3. 视觉里程计

  视觉里程计采用F2M（Frame-to-Map）实现，F2F（Frame-to-Frame）是利用图像帧到图像帧之间的特征点配准来进行位姿变换的计算，而F2M利用图像帧到地图之间的特征点配准进行位姿变换的配准，RTABMAP的视觉里程计系统框图如下图所示。

4. 局部地图

  在RTABMAP的STM中插入新节点后，系统会利用深度图像生成对应的局部地图，局部地图的参考坐标系为巡视器本身的坐标系，全局地图（最终经过局部地图拼接而成）的参考坐标系为世界坐标系，局部地图与全局地图通过机器人到世界坐标系的坐标变换完成。视觉里程计节点利用闭环检测和全局优化，以此种方式维护全局位姿，全局位姿的核心是地图到视觉里程计的坐标变换关系。局部地图的输出流程处理框图如下图所示。

5. 回环检测与图优化

  对于局部建图的累计误差，需要回环检测和全局优化的介入，视觉词袋模型和贝叶斯滤波器用于回环检测，视觉词袋模型属于视觉SLAM的常规模型和解决办法，其主要用于快速匹配相似度高的帧，而贝叶斯滤波器维护所有候选节点相似度的概率分布。设当前位姿节点为$L_t$，随机变量$S_t$表示WM中所有待检测的候选节点，随机变量$S_t=i$的概率表征$L_t$与$L_i$有回环的可能性，根据贝叶斯可得$S_t$更新的公式如下式所示。
$$P\left(S_t|L^t\right)=\eta P\left(L_t|S_t\right)\sum _{i=-1}^{t_n}P\left(S_t|S_{t-1}=i\right)P\left(S_{t-1}=i|L^{t-1}\right)$$
  其中$L=L_{-1},\dots ,L_t$表示t时刻WM中所有的节点，观测模型$P\left(L_t|S_t\right)$可以通过似然函数$\ell \left(S_t=j|L_t\right)=P\left(L_t|S_t=j\right)$计算。归一化的观测模型$P\left(S_t|L^t\right)$与阈值对比，若低于阈值则回环检测成功，否则取$P\left(S_t|L^t\right)$中概率取值最高的$S_t=i$对应的节点$L_i$选定为回环节点。最后将WM中所有的节点和约束边进行全局优化，同时对里程计位姿进行修正。

6. 代码工程实践

  RTABMAP的源码地址在https://github.com/introlab/rtabmap
  RTAB-Map（Real-Time Appearance-Based Mapping）是一个开源的RGB-D SLAM（Simultaneous Localization and Mapping）库，用于实时构建环境地图。它结合了视觉识别和图像姿态估计，并使用回环检测来优化地图的一致性。RTAB-Map的代码组织结构主要包括以下几个主要组件：

1. Core（核心）：这是RTAB-Map的核心模块，提供了地图构建、回环检测和姿态估计等功能。它实现了基于视觉特征的回环检测算法和基于图优化的姿态估计方法。

2. Database（数据库）：RTAB-Map使用数据库来存储和管理地图数据。数据库模块提供了对地图的读取、写入和查询功能。它可以将地图数据保存到硬盘上的SQLite数据库中，并且可以根据需要从数据库中加载地图。

3. GUI（图形用户界面）：RTAB-Map附带了一个基于Qt的图形用户界面，用于可视化地图构建的过程和结果。GUI模块提供了交互式地图浏览、参数设置和结果导出等功能。

4. Plugins（插件）：RTAB-Map支持插件扩展，可以通过插件机制添加额外的功能。例如，它提供了插件接口来支持不同类型的传感器、回环检测算法和地图优化算法。

5. Examples（示例）：RTAB-Map提供了一些示例代码，演示如何使用库中的不同功能。示例代码包括从RGB-D相机读取数据、构建地图、保存和加载地图等。

以RGB-D作为输入的程序参考示例：

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <rtabmap_ros/rgbd_sync.h>
#include <rtabmap_ros/rgbd_odometry.h>
#include <rtabmap_ros/rgbd_mapping.h>

void imageCallback(
    const sensor_msgs::ImageConstPtr& rgbMsg,
    const sensor_msgs::ImageConstPtr& depthMsg,
    const sensor_msgs::CameraInfoConstPtr& infoMsg)
{
    cv_bridge::CvImagePtr cvRgbPtr;
    cv_bridge::CvImagePtr cvDepthPtr;
    try
    {
        cvRgbPtr = cv_bridge::toCvCopy(rgbMsg, sensor_msgs::image_encodings::BGR8);
        cvDepthPtr = cv_bridge::toCvCopy(depthMsg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat rgbImage = cvRgbPtr->image;
    cv::Mat depthImage = cvDepthPtr->image;

    // Perform RGB-D SLAM with RTAB-Map

    // ...
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "rtabmap_example");
    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgbSub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depthSub(nh, "/camera/depth/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub(nh, "/camera/rgb/camera_info", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), rgbSub, depthSub, infoSub);
    sync.registerCallback(boost::bind(&imageCallback, _1, _2, _3));

    ros::spin();

    return 0;
}