How to Build a 3D Map with RTAB-VSLAM on Raspberry Pi 5

In this guide we build a 3D map of a room with RTAB-VSLAM on the MentorPi M1, then use that map for autonomous navigation.

What RTAB-VSLAM is

RTAB-VSLAM stands for real-time appearance-based visual simultaneous localization and mapping. It's an open-source library that creates 3D maps while tracking the robot's position.

Its special feature is memory management. Short-term memory matches current continuous data, and long-term memory detects loop closures, when the robot revisits somewhere it has seen before. This keeps loop closure detection inside a fixed time limit, which suits long-term mapping of large spaces.

It works from visual features:

1. Extract features from the camera images.
2. Match features across timestamps to detect loop closures.
3. Sort the data into short-term and long-term memory.
4. Use short-term memory to update position and build the map in real time.
5. When new data matches long-term memory, merge it to improve position accuracy and map quality.

Part 1: Build the 3D map

1. Start the robot and open its desktop over a VNC remote connection.
2. Open a terminal on the robot and disable the app auto-start service, so it doesn't conflict.
3. Launch the mapping service and wait for the ROS nodes to start, including the RTAB-VSLAM core and camera drivers.
4. On your virtual machine, launch RViz to see the 3D map build in real time.
5. Open another terminal on the robot and launch keyboard control.

ros2 launch slam rtabmap_slam.launch.py
ros2 launch slam rviz_rtabmap.launch.py
ros2 launch peripherals teleop_key_control.launch.py

Drive with W and S for forward and back, and hold A or D to turn left or right.

Tips for a better map

• Move slowly. The slower the robot, the smaller the odometry error.
• Start in front of a straight wall or inside an enclosed area.
• Do a full 360° scan of the space.
• In larger areas, finish a full loop before working on the details.

Check the map before you navigate

• Obstacle edges are clearly defined.
• The map matches the real room.
• There are no gray unmapped areas where the robot will drive.
• The map covers the whole navigation area.

The map saves automatically. Press Ctrl + C to close the mapping command, and the 3D map is saved to the system. Press Ctrl + C in each terminal to close everything.

Part 2: Navigate with the map

Launch navigation on the robot and the navigation view on your virtual machine. This loads the 3D map, starts localization and prepares the path planner.

ros2 launch navigation rtabmap_navigation.launch.py
ros2 launch navigation rviz_rtabmap_navigation.launch.py

Click 2D Nav Goal in RViz, then click your target on the map. Two paths appear. The line of blue squares is the straight-line distance to the goal. The dark blue line is the route the robot will really follow, planned around known obstacles, safety margins, the robot's size and its turning radius.

As it drives, the robot keeps comparing what it sees with the stored 3D map. If it meets an obstacle, old or new, the dark blue path updates and it goes around.

For bigger missions, use the Publish Point tool to add several waypoints. The robot visits each in order and plans a path between them.