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

Authors: Julian Nubert (nubertj@ethz.ch) , Shehryar Khattak , Marco Hutter

Resources

[1] ICRA2022, Philadelphia

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:

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.
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:

graph_msf: The core library for the sensor fusion. This library is only dependant on Eigen and GTSAM.
graph_msf_ros: This package provides a basic class for using GraphMsf in ROS. It is dependant on GraphMsf and ROS.
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:

(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.
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.
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.

zhijunhan/graph_msf