ORB_SLAM3_ROS2

This repository is ROS2 Humble wrapping to use ORB_SLAM3

---

Demo Video

Prerequisites

• I have tested on below version.

  • Ubuntu 22.04
  • ROS2 Humble
  • OpenCV 4.5.4

• Build ORB_SLAM3

  • Go to this repo and follow build instruction.

• Install related ROS2 package

$ sudo apt install ros-$ROS_DISTRO-vision-opencv && sudo apt install ros-$ROS_DISTRO-message-filters

How to build

1. Clone repository to your ROS workspace

$ mkdir -p dev_ws/src
$ cd ~/dev_ws/src
$ git clone https://github.com/guyuehome/ORB_SLAM3_ROS2.git orbslam3_ros2

2. Change this line to your own python site-packages path

3. Change this line to your own ORB_SLAM3 path

Now, you are ready to build!

$ cd ~/dev_ws
$ colcon build --symlink-install --packages-select orbslam3

Troubleshootings

1. If you cannot find sophus/se3.hpp:
   Go to your ORB_SLAM3_ROOT_DIR and install sophus library.

$ cd ~/{ORB_SLAM3_ROOT_DIR}/Thirdparty/Sophus/build
$ sudo make install

2. Please compile with OpenCV 4.5.4 version.

How to use

1. Source the workspace

$ source ~/dev_ws/install/local_setup.bash

2. Run orbslam mode, which you want.
   This repository only support MONO, STEREO, RGBD, STEREO-INERTIAL mode now.
   You can find vocabulary file and config file in here. (e.g. orbslam3_ros2/vocabulary/ORBvoc.txt, orbslam3_ros2/config/monocular/TUM1.yaml for monocular SLAM).

• MONO mode

$ ros2 run orbslam3 mono PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE

e.g.

Terminal 1

$ sudo apt install ros-humble-v4l2-camera
$ ros2 run v4l2_camera v4l2_camera_node

Terminal 2

$ ros2 run orbslam3 mono ~/dev_ws/src/orbslam3_ros2/vocabulary/ORBvoc.txt  ~/dev_ws/src/orbslam3_ros2/config/monocular/EuRoC.yaml

• STEREO mode

$ ros2 run orbslam3 stereo PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY

• RGBD mode

$ ros2 run orbslam3 rgbd PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE

• STEREO-INERTIAL mode

$ ros2 run orbslam3 stereo-inertial PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY [BOOL_EQUALIZE]

Run with rosbag

To play ros1 bag file, you should install ros1 noetic & ros1 bridge.
Here is a link to demonstrate example of ros1-ros2 bridge procedure.
If you have ros1 noetic and ros1 bridge already, open your terminal and follow this:
(Shell A, B, C, D is all different terminal, e.g. stereo-inertial mode)

1. Download EuRoC Dataset (V1_02_medium.bag)

$ wget -P ~/Downloads http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_02_medium/V1_02_medium.bag

2. Launch Terminal
   (e.g. ROS1_INSTALL_PATH=/opt/ros/noetic, ROS2_INSTALL_PATH=/opt/ros/foxy)

#Shell A:
source ${ROS1_INSTALL_PATH}/setup.bash
roscore

#Shell B:
source ${ROS1_INSTALL_PATH}/setup.bash
source ${ROS2_INSTALL_PATH}/setup.bash
export ROS_MASTER_URI=http://localhost:11311
ros2 run ros1_bridge dynamic_bridge

#Shell C:
source ${ROS1_INSTALL_PATH}/setup.bash
rosbag play ~/Downloads/V1_02_medium.bag --pause /cam0/image_raw:=/camera/left /cam1/image_raw:=/camera/right /imu0:=/imu

#Shell D:
source ${ROS2_INSTALL_PATH}/setup.bash
ros2 run orbslam3 stereo-inertial PATH_TO_VOCABULARY PATH_TO_YAML_CONFIG_FILE BOOL_RECTIFY [BOOL_EQUALIZE]

3. Press spacebar in Shell C to resume bag file.

Acknowledgments

This repository is modified from this repository.
To add stereo-inertial mode and improve build difficulites.