graphslam

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

>>> g.optimize()

>>> g.plot(vertex_markersize=1)

Output:

Iteration                chi^2        rel. change
---------                -----        -----------
        0           16720.0210
        1              45.6644          -0.997269
        2               1.2936          -0.971671
        3               1.2387          -0.042457
        4               1.2387          -0.000001

Original	Optimized

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       319916668.8138          43.484235
        2       124888469.1437          -0.609622
        3          338171.6169          -0.997292
        4             734.5693          -0.997828
        5             215.8405          -0.706167
        6             215.8405          -0.000000

Original	Optimized

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)