leggedrobotics / graph_msf

A graph-based multi-sensor fusion framework. It can be used to fuse various relative or absolute measurments with IMU readings in real-time.

Graph-MSF: Graph-based Multi-sensor Fusion for Consistent Localization and State Estimation

Authors: Julian Nubert ([email protected]) , Shehryar Khattak , Marco Hutter

Resources

[1] ICRA2022, Philadelphia

• project page.
• paper
• video

Overview

The presented framework aims for a flexible and fast fusion of multiple sensor modalities. The state estimate is published at imu frequency through IMU pre-integration with a multi-threaded implementation and book-keeping. The adding of the measurements and the optimization of the graph are performed in different threads. In contrast to classical filtering-based approaches this graph-based structure also allows for a simple incorporation of delayed sensor measurements up to the smoothingLag.

There are two intended use-cases:

1. Using the dual graph formulation as proosed in [1]. In this part of the implementation there are hard-coded components for this specific use-case.
2. A more general graph-based multi-sensor fusion. An example for fusing LiDAR odometry and IMU on the dataset of the ETH Zurich Robotic Summer School will follow shortly.

© IEEE

Disclaimer: The framework is still under development and will be updated, extended, and more generalized in the future.

Modules and Packages

This repository contains the following modules:

1. graph_msf: The core library for the sensor fusion. This library is only dependant on Eigen and GTSAM.
2. graph_msf_ros: This package provides a basic class for using GraphMsf in ROS. It is dependant on GraphMsf and ROS.
3. examples: Examples on how to use GraphMsf and GraphMsfRos.
   • ./examples/excavator_dual_graph from [1]. This is the implementation as presented in the paper.
   • Single-graph standalone fusion example following soon.

Installation

For the installation instructions please refer to the ./doc/installation.md.

Example Usage

Instructions on how to use and run the examples can be found in the ./examples/README.md.

Data

We provide some example datasets from our excavator HEAP. The data contains

• IMU measurements,
• LiDAR odometry from CompSLAM,
• Left and Right GNSS measurements,
• and an (arm-)filtered point cloud.

The datasets can be found here: Google Drive Link.

Custom Usage

For custom usage, such as the fusion of more sensor measurements, an own class with the desired functionality can be implemented. This class only has to inherit from the GraphMsfInterface base-class.

For usage three functionalities have to be implemented by the user:

1. (ROS)-callbacks or other code snippets can be used to add measurements through the given interface as specified here. Examples for this can be seen in the excavator_dual_graph, where ROS subscribers are used to add the measurements to the graph.
2. Furthermore, the purely virtual functions publishState needs to be implemented. This method is called after each arrival of an IMU measurement to publish the state in the desired format.
3. Lastly, readParams_ is called during initialization, and is needed to load extrinsic parameters inside the StaticTransforms required for coordinate transformations. Note that the StaticTransforms class can contain arbitrary extrinsic transformation pairs.

The measurements can be passed to the interface using the measurements specified in ./graph_msf/include/measurements. More measurements can be easily added for custom usage.

Paper

If you find this code useful, please consider citing

@inproceedings{nubert2022graph,
  title={Graph-based Multi-sensor Fusion for Consistent Localization of Autonomous Construction Robots},
  author={Nubert, Julian and Khattak, Shehryar and Hutter, Marco},
  booktitle={IEEE International Conference on Robotics and Automation (ICRA)},
  year={2022},
  organization={IEEE}
}

Acknowledgements

The authors thank Marco Tranzatto, Simon Kerscher, Dominic Jud, Lorenzo Terenzi, Timo Schoenegg and the remaining HEAP team for patiently testing parts of this framework during their experiments.

graph_msf's Issues

Estimated timeline of latest version

Waiting for the code

Hi, I really like the framework that you proposed.
And I am still waiting for the open-source code to do some researches.
Hope your can release it asap.
Appreciate your work again!

suitable for indoor scene?

Hello author,
Thanks for sharing the work! I roughly read the paper, if we deploy this dual graph into indoor scene for localization, is it suitable? I think the fallback graph of the paper is consider for gnss drop. In indoor environment, there is no gps, main graph always is active.

When will the new version be released？

Hi! In other issure, I see you reply that there is a new version coming soon, I am very interested in it, I want to ask when the new version will be released, or can I get a trial access? Thank you so much!

Regarding fusing a stereo inertial sensor for performing Visual SLAM

Hello @nubertj ! Thank you for this excellent work. I was a part of the ETH Robotics Summer school and I learnt a lot on using this code to map and localise.
I am currently working on my thesis on a similar topic at the moment and I wanted to ask if this Graph Msf can be used or modified to fuse stereo inertial cameras as well? Is it mandatory to have a LIDAR to use this SLAM method? I remember during the Summer school, since we tested a lot outdoors, the tracking camera gave problems and we disabled it.

My use case for my thesis is to perform indoors, is it possible that I can modify and add an implementation to fuse stereo inertial cameras?

Looking forward to your reply,
Aishwarya

Compslam

There is a launch file in graph_msf_ros for compslam_ros. But, I couldn't find your slam in the repo. I want to use your slam i.e., compslam. Is it available? If yes, where can I find it?

Out of Range exeception while optimizing graph: invalid key

Hi,

running example with the 2_driving_away_filtered.bag data, the ROS node crashes after a short while to an error "Out of Range exeception while optimizing graph: invalid key".

I've tried to increase the smootherLag parameter (default value: 1.5) in config/graph_config.yaml, but it does not seem to fix the issue completely.

I get a same type of error "Out of Range exeception while optimizing graph: Requested variable "x35180" is not in this VectorValues. This happens if the measurement delay is larger than the graph smootherLag..."

I thought to ask if you would have more insight on how to overtcome this problem.

what is the reason to empty the globalFactorsBufferPtr

when reading the source code, I found that there are always empty global factors in the "updateActiveGraphAndGetState" function. As for the action, I think the non-linear graph contains only very few factors such as the latest IMU factor or Gnss factor so the isam2 optimizer only consists of a few constraints. What is the reason to empty the buffer?

second example

I want to ask when the second example code will be released. Your work is so good that I can't wait to see it.

Pose and IMU fusion example?

Hi, and thank you for making this code available.

I need to add IMU data into existing pose data (xyz / xyzw) from a SLAM system.

The goal is to smooth and predict data when tracking is lost.

Is this code suitable for that purpose? Is there an example that might help me get started?

Thanks!

Releasing the code

Just came across this on youtube, I was hoping you could release the code as promised on the repo that it will be released in may 2022