RTAB-Map source code review

This article focuses on 2D laser mapping, divided into rtabmap-ros source code and rtabmap source code. One is the interface between rtabmap and ros, and the other is the core algorithm of rtabmap.

1. rtabmap-ros

rtabmap_ros-master\rtabmap_slam\src\CoreWrapper.cpp is the core wrapper of 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 & amp; globalDescriptor);

In the source code of RTAB-Map, the CoreWrapper::commonLaserScanCallback function is a callback function for processing laser scanning data.

This function is typically used to receive and process laser scan data from a laser sensor. The specific implementation may vary, but the general process is to convert the received laser scan data into the data structure required by RTAB-Map, and then pass it to RTAB-Map for processing.

In this callback function, some preprocessing operations may be performed, such as filtering, denoising or downsampling of laser data. Then, pass the processed data to the core algorithm of RTAB-Map for processing, such as building maps, positioning or loop detection, etc.

1.2 CoreWrapper::commonOdomCallback

CoreWrapper::commonOdomCallback(
        const nav_msgs::OdometryConstPtr & odomMsg,
        const rtabmap_msgs::UserDataConstPtr & userDataMsg,
        const rtabmap_msgs::OdomInfoConstPtr & amp; odomInfoMsg);

In the source code of RTAB-Map, the CoreWrapper::commonOdomCallback function is a callback function for processing odometer data.

This function is typically used to receive and process odometry data from an odometry sensor. It receives three parameters:

odomMsg: A parameter of nav_msgs::OdometryConstPtr type, which contains the ROS message of the odometer data.

userDataMsg: A parameter of type rtabmap_msgs::UserDataConstPtr, which contains the ROS message of user data. This parameter may be used in some specific application scenarios to associate the custom data provided by the user with the odometer data.

odomInfoMsg: A parameter of type rtabmap_msgs::OdomInfoConstPtr, which contains the ROS message of the odometer information, such as the covariance matrix of the odometer.

In this callback function, some preprocessing operations can be performed, such as filtering the odometer data, coordinate system conversion, etc. Then, pass the processed odometer data to the core algorithm of RTAB-Map for processing, such as map construction, positioning, etc.

1.3 CoreWrapper::process

void CoreWrapper::process(
const ros::Time & stamp,
SensorData & amp; data,
const Transform & odom,
const std::vector<float> & odomVelocityIn,
const std::string & odomFrameId,
const cv::Mat & odomCovariance,
const OdometryInfo & odomInfo,
double timeMsgConversion);

In the source code of RTAB-Map, the CoreWrapper::process function is the main processing function of RTAB-Map, which is used to execute the core algorithm process of RTAB-Map.

This function is usually called in a loop to continuously process sensor data, perform map building, positioning, loop closure detection, and other operations. Its main function is to drive the algorithm flow of RTAB-Map, and perform real-time map update and positioning according to the arrival of sensor data.

The exact implementation details will vary, but the general flow is as follows:

  1. Receive sensor data: Receive data from sensors such as laser sensors, cameras, or odometers, such as laser scan data, image data, or odometer data, etc.
  2. Data preprocessing: Perform some preprocessing on the received sensor data, such as filtering, denoising, coordinate system conversion and other operations.
  3. Call the core algorithm: pass the preprocessed sensor data to the core algorithm of RTAB-Map for processing. This includes map building, localization, loop detection, path planning, and more.
  4. Update the map: update the map according to the matching results of the sensor data, including adding new point clouds or image frames, adjusting the pose and topology of the map, etc.
  5. Publish results: publish the processing results in the form of ROS messages, such as map data, positioning results, loopback detection information, etc.
  6. Loop execution: Go back to step 1, wait for new sensor data to arrive, and continue processing.

2. rtabmap

2.1 Rtabmap::process

rtabmap-master\corelib\src\Rtabmap.cpp is the core code.

bool Rtabmap::process(
const SensorData & amp; data,
Transform odomPose,
const cv::Mat & odomCovariance,
const std::vector<float> & odomVelocity,
const std::map<std::string, float> & amp; externalStats);

The Rtabmap::process function is the core function in the RTAB-Map library, which is used to process sensor data and execute the SLAM (Simultaneous Localization and Mapping) algorithm.

Specifically, the Rtabmap::process function accepts sensor data (such as laser scans, RGB-D images, etc.) and odometry data, and passes them to the RTAB-Map algorithm for processing. It performs the following main steps:

  1. Data preprocessing: Preprocessing sensor data, such as denoising, filtering, downsampling, etc., to reduce data noise and redundancy.

  2. Feature extraction and descriptor calculation: Extract feature points or feature descriptors from sensor data for environment modeling and feature matching.

  3. Pose estimation: Estimate the pose and position of the robot using methods such as odometry data or feature matching.

  4. Map update: Based on the estimated pose and sensor data, update the map model, including adding new point clouds or image frames, optimizing the topology of the map, etc.

  5. Loop detection: In the process of building the map, the loop is found through the loop detection algorithm, and the pose estimation and map model are further optimized.

  6. Data output: Generate visualizations, publish updated maps, provide location estimates, etc.

2.2 Transform Memory::computeIcpTransform

rtabmap-master\corelib\src\Memory.cpp is responsible for implementing functions related to memory management.

Transform Memory::computeIcpTransform(
const Signature & fromS,
const Signature & toS,
Transform guess,
RegistrationInfo * info) ;

The Memory::computeIcpTransform() function is a function in Rtabmap, which is used to calculate the ICP (Iterative Closest Point) transformation matrix between two Signature objects.

The parameters of the function are as follows:

fromS: source Signature object, representing the starting frame.

toS: target Signature object, representing the target frame.

guess: The transformation matrix for the initial guess.

info: Used to store some information during the calculation process, such as the number of matching points, RMS (Root Mean Square) error, etc.

The function of the function is to calculate the transformation matrix that aligns the two point clouds as much as possible by matching the point cloud data in the two Signature objects. The commonly used matching algorithm is the ICP algorithm, which iteratively finds the optimal transformation matrix to minimize the overlap of point clouds.

It should be noted that this function is used to calculate the ICP transformation between two Signature objects, which is different from the global graph optimization process, and only calculates the matching relationship between two frames.

2.3 Transform Registration::compute Transformation

The code in rtabmap-master\corelib\src\Registration.cpp is responsible for implementing the core algorithm of point cloud matching and pose estimation, providing a variety of matching methods and optimization methods, as well as related auxiliary functions and data structures.

Transform Registration::computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess,
RegistrationInfo * infoOut) ;

The Registration::computeTransformation() function is a function in Rtabmap that computes a transformation matrix between two sensor data.

The parameters of the function are as follows:

from: The first sensor data entered.

to: The second sensor data entered.

guess: The transformation matrix for the initial guess.

infoOut: Used to store information during the registration process, such as internal points, matching time, etc.

The role of the function is to perform point cloud matching between the given two sensor data, and calculate the transformation matrix that makes the two point clouds align as much as possible. It can be used to compute motion estimation between adjacent frames, or for loop closure matching in loop closure detection.

The specific implementation details can be found in the corresponding Registration class, and different Registration classes may use different algorithms and strategies for point cloud matching. The function will use the appropriate algorithm and parameters to match according to the input parameters, and return the calculated transformation matrix. At the same time, some information during the registration process can be obtained through the infoOut parameter, such as the number of internal points, matching time, etc.

It should be noted that this function is used to calculate the transformation between two sensor data, not to perform global graph optimization. For global optimization, other functions or algorithms are usually used to handle it.

2.4 Transform RegistrationIcp::computeTransformationImpl

The code in rtabmap-master\corelib\src\RegistartionIcp.cpp implements the point cloud registration process based on the ICP algorithm, and provides functions such as matching parameter setting, iteration termination condition judgment, transformation calculation, etc. alignment and alignment.

Transform RegistrationIcp::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
RegistrationInfo & amp; info) ;

The RegistrationIcp::computeTransformationImpl function is a function in the RTAB-Map library, which is used to implement the ICP (Iterative Closest Point) algorithm for point cloud registration.

In RTAB-Map, the RegistrationIcp::computeTransformationImpl function is used to calculate the optimal transformation matrix between two point clouds. It takes two input point clouds, a target point cloud and a reference point cloud, and an initial transformation matrix as parameters. This function aligns the target point cloud with the reference point cloud through iterative optimization to optimize the correspondence between them.

Specifically, at each iteration, the function matches each point of the target point cloud with the closest point on the reference point cloud according to the current transformation matrix, and then by minimizing a distance metric such as point-to-point distance or point-to-plane distance) to optimize the transformation matrix. The iterative process is repeated until the convergence condition is met or the maximum number of iterations is reached.

The computeTransformationImpl function returns the optimized transformation matrix and the optimization result of the ICP algorithm (for example, whether it converges, the number of iterations, etc.).

By calling the computeTransformationImpl function, the target point cloud can be registered with the reference point cloud to obtain the optimal transformation relationship between them. This is useful for applications such as point cloud registration, 3D reconstruction, and environment modeling.

2.5 Transform icpPointToPlaneImpl

The code in rtabmap-master\corelib\src\util3d_registration.cpp provides the commonly used algorithms and methods in the point cloud registration process, which are used for point cloud matching, filtering, conversion and feature calculation.

Transform icpPointToPlaneImpl(
const typename pcl::PointCloud<PointNormalT>::ConstPtr & amp; cloud_source,
const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
int maximumIterations,
bool & has Converged,
pcl::PointCloud<PointNormalT> & cloud_source_registered,
float epsilon,
bool icp2D);

The icpPointToPlaneImpl function is a function in the RTAB-Map (Real-Time Appearance-Based Mapping) library, which is used to implement the point-to-plane ICP (Iterative Closest Point) algorithm for point cloud registration.

ICP is a commonly used point cloud registration algorithm for aligning two point clouds to optimize the relative transformation between them. In the point-to-plane version of ICP, each point of the target point cloud is matched with the closest point on the reference point cloud, and then the transformation matrix is optimized by minimizing the point-to-plane distance. This approach generally works better for point cloud registration with surface structures.

The icpPointToPlaneImpl function accepts two input point clouds, the target point cloud and the reference point cloud, and an initial transformation matrix as parameters. It aligns the target point cloud with the reference point cloud by continuously optimizing the transformation matrix according to the iterative steps of the ICP algorithm. This function will return the optimized transformation matrix and the optimization result of ICP (for example, whether it converges, the number of iterations, etc.).

By calling the icpPointToPlaneImpl function, the target point cloud can be registered with the reference point cloud to obtain the optimal transformation relationship between them. This is useful in many applications, such as robot navigation, SLAM (Simultaneous Localization and Mapping), and 3D reconstruction.