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

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

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

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

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

2. 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 so that you can replicate the results on your machine.

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

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

Within the repository we provide the datasets used for the comparative analysis in the paper. 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

We are making this software freely available in the hope that it will be useful to others. If you use HiPE in your own research work, please cite:

@ARTICLE{hipe2022guadagnino,
  author={Guadagnino, Tiziano and Giammarino, Luca Di and Grisetti, Giorgio},
  journal={IEEE Robotics and Automation Letters}, 
  title={HiPE: Hierarchical Initialization for Pose Graphs}, 
  year={2022},
  volume={7},
  number={1},
  pages={287-294},
  doi={10.1109/LRA.2021.3125046}}

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: [email protected] or [email protected]

• Tiziano Guadagnino
• Luca Di Giammarino
• Giorgio Grisetti