HiPE : Hierarchical Initialization for Pose Graphs

HiPE is a hierarchical algorithm for pose graph initialization, namely, bootstrap the performances of a pose graph optimization algorithm by computing an initial configuration of the variables which is as close as possible to the optimal solution. HiPE first partitions the input into a set of sub-graphs and extract local spatial constraints between salient variables. The constraints are then combined in a coarse-grained graph, which represents an abstract topology of the problem. By exploiting a maximum likelihood estimation of this graph, HiPE can initialize the pose graph in a non-linear fashion. Further, the sparse structure of the coarse-grained representation enables an efficient initialization on large-scale problem instances without performance loses compared to existing methods.

A detailed description of the algorithm and an analysis of its performances can be found in our [paper - FILL ME]

Requirements

Ubuntu and ROS, currently we support:
- Ubuntu 18.04 with ROS melodic
- Ubuntu 20.04 with ROS noetic
srrg2 packages:
Miscellaneous deb packages:
- build-essential
- libeigen3-dev
- libsuitesparse-dev
- libgtest-dev
- libglfw3-dev
- libglu1-mesa-dev
- freeglut3-dev
Catkin tools (Optional):
- python3-catkin-tools
- python3-osrf-pycommon

Installation

Docker (recommanded)

A docker container is provided to run the code. You can check the instruction in the /docker folder.

Local Installation

Step 1: Install deb dependencies

sudo apt install build-essential libgtest-dev libglfw3-dev libglu1-mesa-dev python3-catkin-tools python3-osrf-pycommon libsuitesparse-dev freeglut3-dev libgtest-dev

Step 2: Create a ROS workspace with the srrg2 dependecies

$ mkdir -p ${HOME}/srrg2_ws/src
$ git clone https://github.com/srrg-sapienza/srrg2_cmake_modules.git ${HOME}/srrg2_ws/src/srrg_cmake_modules
$ git clone https://github.com/srrg-sapienza/srrg2_core.git ${HOME}/srrg2_ws/src/srrg2_core
$ git clone https://github.com/srrg-sapienza/srrg2_solver.git ${HOME}/srrg2_ws/src/srrg2_solver
$ git clone https://github.com/srrg-sapienza/srrg2_hipe.git ${HOME}/srrg2_ws/src/srrg2_hipe

Step 3: Initialize the workspace

$ cd ${HOME}/srrg2_ws/src && catkin_init_workspace

or if you use catkin-tools

$ cd ${HOME}/srrg2_ws/ && catkin init

Step 4: Build the srrg2_hipe package

$ catkin_make --only-pkg-with-deps srrg2_hipe

or if you use catkin-tools

$ catkin build srrg2_hipe

How to use

Prepare the datasets. We currently support input pose graphs in the boss format, see the srrg2_solver package. As most of the publicly available datasets are in g2o format, we provide a converter within the srrg2_solver package. To convert the datasets use:
```
$ rosrun srrg2_solver solver_app_graph_converter -i my_dataset.g2o -o my_dataset.boss
```
You can also convert back a boss file in g2o format using:
```
$ rosrun srrg2_solver solver_app_graph_converter -i my_dataset.boss -o my_dataset.g2o
```
Generate a configuration:
```
$ rosrun srrg2_hipe graph_optimizer -j
```
Which provide a parameter setting for the algorithm that usually works out-of-the-box in most scenarios. You can play around with the parameters by editing the hierarchical_solver.config file that is generated by the above command.

The default configuration is the one used in the experiments of the [paper - FILL ME] so that you can replicate the results on your machine.
Optimize the graph
```
$ rosrun srrg2_hipe graph_optimizer -i input_graph.boss -o output_graph.boss
```
Which will perform the HiPE initialization followed by a non-linear least square minimization and save the result in output_graph.boss. Further, the coarse-grained graph will be saved as hgraph_output_graph.boss.
Evaluate metrics. If you have available a ground truth graph, you can evaluate the Absolute Trajectory Error and Relative Error using:
```
$ rosrun srrg2_hipe compute_metrics -i graph.boss -gt ground_truth_graph.boss
```

Datasets

Within the repository we provide the datasets used for the comparative analysis in the [paper - FILL ME]. You can find those in the /data folder. Furthermore, we provide two executables to convert a dataset from the geodesic cost to the chordal cost and viceversa:

$ rosrun srrg2_hipe geodesic_to_chordal_converter -i geodesic_dataset.boss -o chordal_dataset.boss

$ rosrun srrg2_hipe chordal_to_geodesic_converter -i chordal_dataset.boss -o geodesic_dataset.boss

References

We are making this software freely available in the hope that it will be useful to others. If you use HiPE in your own work, please [cite our paper - FILL ME].

@article{Guadagnino2021HiPE,
title = {HiPE: Hierarchical Initialization for Pose Graphs},
author = {T. Guadagnino and L. Di Giammarino and G. Grisetti},
journal = {**FILL ME**},
volume = {38},
number = {2--3},
pages = {95--125},
month = mar,
year = {2019},
}

Copyright and License

The software is distributed under the terms of the Berkeley Software Distribution license (BSD) version 3. Please see the LICENSE file for more informations.

Contact: guadagnino@diag.uniroma1.it or frevo93@gmail.com

Contributors

Tiziano Guadagnino
Luca Di Giammarino
Giorgio Grisetti

mfkiwl/srrg2-hipe