RTAB-Map源码梳理
本文以2D激光建图为主，分为rtabmap-ros源码和rtabmap源码。一个是rtabmap与ros的接口，一个是rtabmap的核心算法。
1. rtabmap-ros
rtabmap_ros-master\rtabmap_slam\src\CoreWrapper.cpp是ratabmap-ros的核心封装器。
1.1 CoreWrapper::commonLaserScanCallback
CoreWrapper::commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const sensor_msgs::LaserScan& scan2dMsg,
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg,
const rtabmap_msgs::GlobalDescriptor & globalDescriptor);在RTAB-Map的源码中，CoreWrapper::commonLaserScanCallback函数是用于处理激光扫描数据的回调函数。
该函数通常用于接收和处理来自激光传感器的激光扫描数据。具体的实现可能会有所不同，但一般的流程是将接收到的激光扫描数据转换为RTAB-Map所需的数据结构，然后将其传递给RTAB-Map进行处理。
在该回调函数中，可能会进行一些预处理操作，例如对激光数据进行滤波、去噪或下采样等。然后，将处理后的数据传递给RTAB-Map的核心算法进行处理，例如构建地图、定位或回环检测等。
1.2 CoreWrapper::commonOdomCallback
CoreWrapper::commonOdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_msgs::UserDataConstPtr & userDataMsg,
const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg);在RTAB-Map的源码中，CoreWrapper::commonOdomCallback函数是用于处理里程计数据的回调函数。
该函数通常用于接收和处理来自里程计传感器的里程计数据。它接收三个参数：
odomMsg：nav_msgs::OdometryConstPtr类型的参数，包含了里程计数据的ROS消息。
userDataMsg：rtabmap_msgs::UserDataConstPtr类型的参数，包含了用户数据的ROS消息。这个参数在一些特定的应用场景下可能会用到，用于将用户提供的自定义数据与里程计数据关联起来。
odomInfoMsg：rtabmap_msgs::OdomInfoConstPtr类型的参数，包含了里程计信息的ROS消息，例如里程计的协方差矩阵等。
在该回调函数中，可以进行一些预处理操作，例如对里程计数据进行滤波、坐标系转换等。然后，将处理后的里程计数据传递给RTAB-Map的核心算法进行处理，例如地图构建、定位等。
1.3 CoreWrapper::process
void CoreWrapper::process(
const ros::Time & stamp,
SensorData & data,
const Transform & odom,
const std::vector & odomVelocityIn,
const std::string & odomFrameId,
const cv::Mat & odomCovariance,
const OdometryInfo & odomInfo,
double timeMsgConversion); 在RTAB-Map的源码中，CoreWrapper::process函数是RTAB-Map的主要处理函数，用于执行RTAB-Map的核心算法流程。
该函数通常在一个循环中被调用，用于不断地处理传感器数据、执行地图构建、定位、回环检测等操作。它的主要作用是驱动RTAB-Map的算法流程，并根据传感器数据的到来进行实时的地图更新和定位。
具体的实现细节会有所不同，但一般的流程如下：
- 接收传感器数据：从激光传感器、相机或里程计等传感器中接收数据，例如激光扫描数据、图像数据或里程计数据等。
- 数据预处理：对接收到的传感器数据进行一些预处理，例如滤波、去噪、坐标系转换等操作。
- 调用核心算法：将预处理后的传感器数据传递给RTAB-Map的核心算法进行处理。这包括地图构建、定位、回环检测、路径规划等。
- 更新地图：根据传感器数据的匹配结果更新地图，包括添加新的点云或图像帧、调整地图的位姿和拓扑结构等。
- 发布结果：将处理结果以ROS消息的形式发布出去，例如地图数据、定位结果、回环检测信息等。
- 循环执行：回到第1步，等待新的传感器数据到来，继续处理。
2. rtabmap
2.1 Rtabmap::process
rtabmap-master\corelib\src\Rtabmap.cpp是核心代码。
bool Rtabmap::process(
const SensorData & data,
Transform odomPose,
const cv::Mat & odomCovariance,
const std::vector & odomVelocity,
const std::map & externalStats); Rtabmap::process函数是RTAB-Map库中的核心函数，用于处理传感器数据并执行SLAM（Simultaneous Localization and Mapping）算法。
具体来说，Rtabmap::process函数接受传感器数据（如激光扫描、RGB-D图像等）和里程计数据，并将其传递给RTAB-Map算法进行处理。它执行以下主要步骤：
-
数据预处理：对传感器数据进行预处理，如去噪、滤波、降采样等，以减少数据噪声和冗余。
-
特征提取和描述子计算：从传感器数据中提取特征点或特征描述子，用于环境建模和特征匹配。
-
位姿估计：利用里程计数据或特征匹配等方法，估计机器人的姿态和位置。
-
地图更新：根据估计的位姿和传感器数据，更新地图模型，包括添加新的点云或图像帧，优化地图的拓扑结构等。
-
回环检测：在建立地图的过程中，通过回环检测算法寻找环路，进一步优化位姿估计和地图模型。
-
数据输出：生成可视化结果、发布更新后的地图、提供定位估计等。
2.2 Transform Memory::computeIcpTransform
rtabmap-master\corelib\src\Memory.cpp负责实现内存管理相关的功能。
Transform Memory::computeIcpTransform(
const Signature & fromS,
const Signature & toS,
Transform guess,
RegistrationInfo * info) ;Memory::computeIcpTransform() 函数是 Rtabmap 中的一个函数，用于计算两个 Signature 对象之间的 ICP（Iterative Closest Point）变换矩阵。
函数的参数如下：
fromS：源 Signature 对象，表示起始帧。
toS：目标 Signature 对象，表示目标帧。
guess：初始猜测的变换矩阵。
info：用于存储计算过程中的一些信息，如匹配的点数、RMS（Root Mean Square）误差等。
函数的作用是通过匹配两个 Signature 对象中的点云数据，计算出使得两个点云尽可能对齐的变换矩阵。常用的匹配算法是 ICP 算法，它通过迭代的方式寻找最优的变换矩阵，使得点云的重叠部分最小化。
需要注意的是，该函数是用于计算两个 Signature 对象之间的 ICP 变换，它不同于全局的图优化过程，仅仅针对两个帧之间的匹配关系进行计算。
2.3 Transform Registration::computeTransformation
rtabmap-master\corelib\src\Registration.cpp中的代码负责实现了点云匹配和位姿估计的核心算法，提供了多种匹配方式和优化方法，以及相关的辅助函数和数据结构。
Transform Registration::computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess,
RegistrationInfo * infoOut) ;Registration::computeTransformation() 函数是 Rtabmap 中的一个函数，用于计算两个传感器数据之间的变换矩阵。
函数的参数如下：
from：输入的第一个传感器数据。
to：输入的第二个传感器数据。
guess：初始猜测的变换矩阵。
infoOut：用于存储注册过程中的信息，如内点数、匹配时间等。
函数的作用是在给定的两个传感器数据之间进行点云匹配，并计算出使得这两个点云尽可能对齐的变换矩阵。它可以用于计算相邻帧之间的运动估计，或者用于回环检测中的闭环匹配。
具体的实现细节可以在相应的 Registration 类中查找，不同的 Registration 类可能使用不同的算法和策略来进行点云匹配。函数会根据输入的参数使用适当的算法和参数进行匹配，并返回计算得到的变换矩阵。同时，可以通过 infoOut 参数获取注册过程中的一些信息，如内点数、匹配时间等。
需要注意的是，该函数是用于计算两个传感器数据之间的变换，而不是进行全局的图优化。对于全局优化，通常会使用其他的函数或算法来处理。
2.4 Transform RegistrationIcp::computeTransformationImpl
rtabmap-master\corelib\src\RegistartionIcp.cpp中的代码实现了基于ICP算法的点云注册过程，提供了匹配参数设置、迭代终止条件判断、变换计算等功能，用于实现点云之间的配准和对齐。
Transform RegistrationIcp::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
RegistrationInfo & info) ;RegistrationIcp::computeTransformationImpl函数是RTAB-Map库中的一个函数，用于执行ICP（Iterative Closest Point）算法进行点云配准的实现。
在RTAB-Map中，RegistrationIcp::computeTransformationImpl函数用于计算两个点云之间的最优变换矩阵。它接受两个输入点云，即目标点云和参考点云，以及一个初始的变换矩阵作为参数。该函数通过迭代优化，将目标点云与参考点云对齐，使它们之间的对应关系最优化。
具体而言，该函数在每次迭代中，根据当前的变换矩阵，将目标点云的每个点与参考点云上的最近点进行匹配，然后通过最小化距离度量（如点到点距离或点到平面距离）来优化变换矩阵。迭代过程会重复执行，直到收敛条件满足或达到最大迭代次数。
computeTransformationImpl函数返回优化后的变换矩阵，以及ICP算法的优化结果（例如，是否收敛、迭代次数等）。
通过调用computeTransformationImpl函数，可以将目标点云与参考点云进行配准，以获得它们之间的最优变换关系。这对于点云配准、三维重建和环境建模等应用非常有用。
2.5 Transform icpPointToPlaneImpl
rtabmap-master\corelib\src\util3d_registration.cpp中的代码提供了点云注册过程中常用的算法和方法，用于点云的匹配、滤波、转换和特征计算等操作。
Transform icpPointToPlaneImpl(
const typename pcl::PointCloud::ConstPtr & cloud_source,
const typename pcl::PointCloud::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud & cloud_source_registered,
float epsilon,
bool icp2D); icpPointToPlaneImpl函数是RTAB-Map（Real-Time Appearance-Based Mapping）库中的一个函数，它是用于执行点云配准中基于点到平面的ICP（Iterative Closest Point）算法的实现。
ICP是一种常用的点云配准算法，用于将两个点云进行对齐，使它们之间的相对变换最优化。在ICP的点到平面版本中，目标点云的每个点与参考点云上的最近点进行匹配，然后通过最小化点到平面的距离来优化变换矩阵。这种方法通常对于具有表面结构的点云配准效果更好。
icpPointToPlaneImpl函数接受两个输入点云，即目标点云和参考点云，以及一个初始的变换矩阵作为参数。它根据ICP算法的迭代步骤，通过不断优化变换矩阵，使目标点云与参考点云对齐。该函数会返回优化后的变换矩阵，以及ICP的优化结果（例如，是否收敛、迭代次数等）。
通过调用icpPointToPlaneImpl函数，可以将目标点云与参考点云进行配准，以获得它们之间的最优变换关系。这在许多应用中都很有用，例如机器人导航、SLAM（Simultaneous Localization and Mapping）和三维重建等。