graphslam Documentation

https://travis-ci.com/JeffLIrion/python-graphslam.svg?branch=master https://coveralls.io/repos/github/JeffLIrion/python-graphslam/badge.svg?branch=master

Documentation for this package can be found at https://python-graphslam.readthedocs.io/.

This package implements a Graph SLAM solver in Python.

Features

  • Optimize \(\mathbb{R}^2\), \(\mathbb{R}^3\), \(SE(2)\), and \(SE(3)\) datasets

  • Analytic Jacobians

  • Supports odometry edges

  • Import and export .g2o files for \(SE(2)\) and \(SE(3)\) datasets

Installation

pip install graphslam

Example Usage

SE(3) Dataset

>>> from graphslam.load import load_g2o_se3

>>> g = load_g2o_se3("data/parking-garage.g2o")  # https://lucacarlone.mit.edu/datasets/

>>> g.plot(vertex_markersize=1)

>>> g.calc_chi2()

16720.020602489112

>>> g.optimize()

>>> g.plot(vertex_markersize=1)

Output:

Iteration                chi^2        rel. change
---------                -----        -----------
        0           16720.0206
        1              26.5495          -0.998412
        2               1.2712          -0.952119
        3               1.2402          -0.024439
        4               1.2396          -0.000456
        5               1.2395          -0.000091

Original

Optimized

_images/parking-garage.png _images/parking-garage-optimized.png

SE(2) Dataset

>>> from graphslam.load import load_g2o_se2

>>> g = load_g2o_se2("data/input_INTEL.g2o")  # https://lucacarlone.mit.edu/datasets/

>>> g.plot()

>>> g.calc_chi2()

7191686.382493544

>>> g.optimize()

>>> g.plot()

Output:

Iteration                chi^2        rel. change
---------                -----        -----------
        0         7191686.3825
        1       319915276.1284          43.484042
        2       124894535.1749          -0.609601
        3          338185.8171          -0.997292
        4             734.5142          -0.997828
        5             215.8405          -0.706145
        6             215.8405          -0.000000

Original

Optimized

_images/input_INTEL.png _images/input_INTEL-optimized.png

References and Acknowledgments

  1. Grisetti, G., Kummerle, R., Stachniss, C. and Burgard, W., 2010. A tutorial on graph-based SLAM. IEEE Intelligent Transportation Systems Magazine, 2(4), pp.31-43.

  2. Blanco, J.L., 2010. A tutorial on SE(3) transformation parameterizations and on-manifold optimization. University of Malaga, Tech. Rep, 3.

  3. Carlone, L., Tron, R., Daniilidis, K. and Dellaert, F., 2015, May. Initialization techniques for 3D SLAM: a survey on rotation estimation and its use in pose graph optimization. In 2015 IEEE international conference on robotics and automation (ICRA) (pp. 4597-4604). IEEE.

  4. Carlone, L. and Censi, A., 2014. From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization. IEEE Transactions on Robotics, 30(2), pp.475-492.

Thanks to Luca Larlone for allowing inclusion of the Intel and parking garage datasets in this repo.

Live Coding Graph SLAM in Python

If you’re interested, you can watch as I coded this up.

  1. Live coding Graph SLAM in Python (Part 1)

  2. Live coding Graph SLAM in Python (Part 2)

  3. Live coding Graph SLAM in Python (Part 3)

  4. Live coding Graph SLAM in Python (Part 4)

  5. Live coding Graph SLAM in Python (Part 5)

Indices and tables