This repo contains a sliding-window optimization-based odometry system fusing visual, inertial and global pose measurements obtained by registering to a Digital Twin. It builds on top of the visual-inertial odometry algorithm SVO Pro.

If you use this code in an academic context, please cite the following RAL 2024 paper.

R. Merat (equal contribution), G. Cioffi (equal contribution), L. Bauersfeld, and D. Scaramuzza, "Drift-free Visual SLAM using Digital Twins," Robotics and Automation Letters (RA-L). 2024.

@InProceedings{merat2020drift,
  author={Merat, Roxane and Cioffi, Giovanni and Bauersfeld, Leonard and Scaramuzza, Davide},
  title={Drift-free Visual SLAM using Digital Twins},
  booktitle={Robotics and Automation Letters (RA-L)},
  organization={IEEE},
  year = {2024}
}

The code has been tested on

• Ubuntu 18.04 with ROS Melodic
• Ubuntu 20.04 with ROS Noetic

Install catkin tools and vcstools if you haven't done so before. Depending on your operating system, run

# For Ubuntu 18.04   Melodic
sudo apt-get install python-catkin-tools python-vcstool

or

# For Ubuntu 20.04   Noetic
sudo apt-get install python3-catkin-tools python3-vcstool python3-osrf-pycommon

Install system dependencies and dependencies for Ceres Solver

# system dep.
sudo apt-get install libglew-dev libopencv-dev libyaml-cpp-dev 
# Ceres dep.
sudo apt-get install libblas-dev liblapack-dev libsuitesparse-dev

Create a workspace and clone the code (ROS-DISTRO=melodic/noetic):

mkdir catkin_ws && cd catkin_ws
catkin config --init --mkdirs --extend /opt/ros/<ROS-DISTRO> --cmake-args -DCMAKE_BUILD_TYPE=Release
cd src
git clone [email protected]:uzh-rpg/rpg_svo_pro_with_digital_twins.git
vcs-import < ./rpg_svo_pro_with_digital_twins/dependencies.yaml
touch minkindr/minkindr_python/CATKIN_IGNORE
catkin build

Refer to SVO Pro.

To launch the programm on our custom dataset download the rosbag and the city twin files from here.

Place them correctly as specified in the configuration file, and run :

roslaunch svo_ros Oerlikon.launch

Make sure you have all the config files as requested in the launch file, and play your rosbag in another terminal. When using your own data, adjust the topic names in the launchfile.

This will launch :

• svo_ros with the global pose accepted as input to the optimizaqtion problem
• svo_registration, which will register the local point cloud to the global city twin.
• rviz

The VIO system used in this repo is based on SVO Pro. Check SVO Pro for the full list of aknowledgments.

The code is licensed under GPLv3. For commercial use, please contact the authors at [email protected] and [email protected].

Check SVO Pro for licenses of the external dependencies.