Kimera-RTABMap-SLAM-PioneerP3DX-SensorFusion

This repository includes work from my dissertation, involving experimental tests with 3D SLAM (Simultaneous Localization and Mapping) methods using a robot equipped with multiple sensors.

Overview

The sensing system used include:

• Robot: Pioneer P3DX
• Sensors:
  • 2D LIDAR: Hokuyo URG-04L
  • RGBD camera: Intel RealSense D435i
  • Stereo Camera: Mynt Eye S1030

Tools and Packages

1. rtabmap_ros
   Purpose: Real-time 3D SLAM and Localization
   Description: This ROS package provides the RTAB-Map framework, which enables RGB-D SLAM (Simultaneous Localization and Mapping) with real-time global loop closure detection. It can generate 3D point clouds of the environment and/or produce a 2D occupancy grid map for navigation. In this project, RTAB-Map was utilized as a ground truth localization estimator.

2. robot_localization
   Purpose: Nonlinear State Estimation using EKF
   Description: The ekf_robot_localization node in this package fuses data from multiple sensors (e.g., IMU, GPS, odometry) using an Extended Kalman Filter (EKF). This can improve the accuracy of the robot's localization by combining different sensor modalities.

3. hector_slam
   Purpose: 2D LIDAR SLAM
   Description: Hector SLAM is optimized for processing 2D LIDAR data, allowing for the creation of high-resolution 2D maps in real time without needing odometry information.

4. Kimera-VIO-ROS
   Purpose: Visual-Inertial Odometry (VIO)
   Description: A ROS wrapper for Kimera-VIO, which performs Visual-Inertial Odometry by camera images and IMU data to estimate a robot’s trajectory.

5. Kimera-RPGO
   Purpose: Pose Graph Optimization with Loop Closure
   Description: Kimera-RPGO refines the pose estimates generated by Kimera-VIO. It adds support for loop-closure detection and pose-graph optimization. Kimera-VIO acts simply as a frontend to detect loop closures and add odometry measurements, while Kimera-RPGO is used as a black box backend. Concerning the robust kernels/outlier rejection functionality, all of it is done in Kimera-RPGO itself.

6. Kimera-Semantics
   Purpose: Real-Time 3D Semantic Reconstruction
   Description: Kimera-Semantics builds a 3D semantic map from 2D RGB-D data. It enhances spatial awareness by labeling objects in the environment (e.g., walls, floors, furniture), which is helpful for tasks such as navigation or object interaction.

7. Kalibr
   Purpose: Calibration Tool for Sensors
   Description: Kalibr is a calibration toolkit for visual-inertial and multi-camera systems. It was used in this project to perform the necessary calibrations for SLAM systems, particularly for Kimera.

8. Allan Variance ROS
   Purpose: Noise Modeling for IMU Sensors
   Description: This package computes Allan variance, a method used to model the noise characteristics of inertial sensors (IMUs).

9. Kimera-Multi
   Purpose: Distributed SLAM for Multi-Robot Systems
   Description: This repository integrates multiple Kimera components, including Kimera-Semantics, Kimera-VIO, Kimera-Distributed, Kimera-Multi-LCD, Kimera-PGMO, DPGO, and DPGO-ROS.

10. mmsegmentation_ros
    Purpose: Semantic Segmentation using Deep Learning
    Description: This ROS package leverages the mmsegmentation toolbox from OpenMMLab for real-time semantic segmentation. In this project, the PSPNet network was employed for detailed semantic segmentation, providing pixel-level classifications of the environment.

Setup and Usage

1. Launching the Robot and Sensors

To launch the robot platform and the selected sensors, use the catkin_pioneer workspace. Make sure you have the sensor drivers installed. This command initializes the robot, the D435i camera, and the 2D LIDAR:

roslaunch robot robot.launch 

To control the robot via keyboard inputs, start the teleop node using the following command:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

Note: Grant necessary permissions to access the USB port for the robot:

sudo chmod 777 -R /dev/ttyUSB0

The Mynt Eye sensor requires ROS Melodic. To set up the Mynt Eye sensor, use Docker to install the Mynt Eye SDK drivers. Run the following commands to start the Docker container for the Mynt Eye SDK:

cd mynteye_docker/docker
docker compose up

2. Docker Setup for RTAB-Map and Robot localization

The RTAB-Map setup is managed via Docker to handle dependencies and configurations. To start the RTAB-Map Docker container, use:

cd rtabmap_docker/docker
docker compose up

This command launches RTAB-Map with additional parameters and manages the odometry fusion. Specifically, it starts the rtabmap_odom node, rtabmap_slam node, imu_filter_node and ekf_localization_node.

3. Hector SLAM for 2D LIDAR

For processing 2D LIDAR data with Hector SLAM, a script named pose_to_odom_hectorslam.py is available in the /Other_code directory. This script converts odometry data to pose and serves as input for robot localization package.

To run Hector SLAM, use:

roslaunch hector_slam_launch tutorial.launch

4. Kimera for single robots

This method includes several modules: Kimera-VIO, Kimera-RPGO, and Kimera-Semantics. Below are the steps to get these modules running on a single robot.

Running the Kimera Docker Container

The docker pull laugasp/kimera_workspace:latest is used to download the Docker image needed for the environment.

To get the single robot odometry working, ensure that: the topics are mapped correctly and that you are pointing to the right folder that has your robot's and sensor's configurations. Once configured, you can launch VIO on each robot, input stereo images and IMU data, and get a reasonable odometry estimate. Here's how to launch Kimera-VIO:

cd /home/catkin_ws/src/Kimera-VIO-ROS/launch
roslaunch kimera_vio_ros kimera_vio_ros_realsense_new.launch

Once you have the odometry (Poses 3D), you can activate the loop detection (loop closures) of the Kimera-RPGO module with these parameters:

 <arg name="use_lcd"                 default="true"/>
 <arg name="lcd_no_optimize"         default="false"/>
 <arg name="lcd_no_detection"        default="false"/>

Note: Other launch files are available, such as those provided by the Kimera authors for the Euroc dataset.

To change the calibration parameters, navigate to: cd /home/catkin_ws/src/Kimera-VIO/params

Enable Dense Depth Stereo estimation

To enable dense depth stereo estimation using OpenCV's StereoBM algorithm, you can set the parameter run_stereo_dense to 1 or true. For more info can be here.

In your workspace directory (/Kimera-VIO-ROS/cfg/calibr), place the .yaml files (e.g., similar to euroc_camchain.yaml) that you obtained from the calibration process.

Kimera-Semantics requires odometry data and a neural network for segmentation. For this, the mmsegmentation_ros package is adapted for class filtering and is based on the mmsegmentation framework. Kimera-Semantics can be fed with data from other odometry systems, such as Kimera-VIO, Kimera-RPGO, or RTAB-Map.

To run Kimera-Semantics, navigate to: cd /home/catkin_ws/src/Kimera-Semantics/kimera_semantics_ros

Make sure to input the correct sensor frames and topics. There is a folder with launch files and .csv files in the /cfg directory containing the correct semantic-color configurations. When using the kimera_semantics_node, the world_frame was specified as “odom” with the parameter <param name=“world_frame” value=“odom” />, but it is also possible to set it to “map”, depending on the desired reference.

To run Kimera-Semantics, use the following command (example of launching): roslaunch kimera_semantics_ros kimera_metric_realsense_new.launch

If not using Docker, installation of Kimera-VIO-ROS Guide:

Visit: Kimera-VIO-ROS Issue #180

It mentions that the used gtsam version is: git checkout c4184e192b4605303cc0b0d51129e470eb4b4ed1

If you encounter issues, try:

According to link, gtsam version commit: e5866799dff48239573cdd84964180867e50edd2

Acordind to link , gtsam version commit: 686e16aaae26c9a4f23d4af7f2d4a504125ec9c3

5. Docker with mmsegmentation ROS Container

To start the mmsegmentation_ros Docker container, the docker pull laugasp/mmsegmentation_workspace:latest is used to download the Docker image used.

The Docker environment is configured with the following components:

• PyTorch: 1.10.0
• CUDA: 11.3
• cuDNN: 8
• MMCV: 1.6.0

The following Python packages are installed in the Docker environment:

• mmcv-full: 1.6.0
• mmdet: 2.28.0
• mmengine: 0.10.4
• numpy: 1.22.0
• onnxruntime-gpu: 1.14.0
• opencv-python: 4.5.4
• torch: 1.10.0+cu113
• mmsegmentation: 0.25.0

The host system has the following characteristics:

• Driver NVIDIA: 470.256.02
• CUDA Version: 11.4

Semantic Segmentation with mmsegmentation_ros

The mmsegmentation_ros package contains the mmsegmentor.py script created by Jianheng Liu. This script performs semantic segmentation on images received from a ROS topic, applying a pre-trained segmentation model, and then publishing the processed results.

Key parameters loaded in the script include:

• config_path: Path to the model configuration file.
• checkpoint_path: Path to the pre-trained model weights.
• device: Specifies the CPU or GPU to be used for inference.
• palette: Color palette used for visualizing the segmentation maps.
• publish_rate: Rate at which the processed images are published.

Custom Script for Class Filtering

Building on top of the original script, a custom script mmsegmentor_yaml.py was added that allows loading a .yaml file to filter specific classes from the chosen dataset and select the desired color palette for segmentation. The yaml_file_path parameter has been added to the key parameters section. This can be found in ~/catkin_ws/src/mmsegmentation_ros/scripts. Note: The color format for output images is BGR.

The configuration files for the MMSegmentation model should be specified in the launch file.

Model Configurations and Datasets

In this experiment, the PSPNet network with an R-50-D8 backbone was used. It was trained on the ADE20K dataset, but the Cityscapes dataset was also tested.

You can find other models and results available for different datasets here.

To run the models, you will need both the config file and the model (checkpoint) file.

Then prepare the bag for playback, set sim_use_time to true and navigate to the directory containing the launch file and run it:

roslaunch mmsegmentor_test.launch

An alternative approach would be to utilise a different neural network that incorporates segmentation, or alternatively, any other ROS package. One such example would be DeepLab ROS.

6. Docker Evaluation and NVIDIA GPU Monitoring

Docker Container Usage Monitoring

In the /Other_code directory, there is a script to monitor Docker container usage (CPU and Memory) and generate plots in a PDF document.

To record the resource usage data (CPU and Memory) of your Docker container:

1. Edit the container name and the .bag file that will be played in the python run_docker_down_csv.py file.

   This will generate a CSV file with the recorded container data.

2. Once you have the CSV data, generate a plot and export it to a PDF file by running: python plot_docker_usage.py

   This will create a PDF file with CPU and memory usage plotted over time.

GPU Utilization Monitoring and Plot Generation

To monitor GPU and memory usage over time, the nvidia-smi command is used. The script will generate a plot that is saved as a PDF document:python plot_gpu_usage.py

7. Scripts for Converting Files to Different Formats

In the /Other_code folder, you will find the scripts conv_csv_tum.py and conv_txt_tum.py, which convert CSV files to TUM and TXT files to TUM, respectively.

Note: You need to adjust the input and output paths in each script according to your directory structure, in addition to the transformations that are to be applied.

8. Sensors Calibration

• RealSense D435i setup (Infrared)

Additionally, check out the following videos for more guidance:

• Visual-Inertial Sensor Calibration 1
• Visual-Inertial Sensor Calibration 2