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

This package implements a Graph SLAM solver in Python.

• 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

pip install graphslam

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

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

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.

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)