graphslam 0.0.17

Graph SLAM solver in Python

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

This package implements a Graph SLAM solver in Python.

Features

• Optimize R^2, R^3, SE(2), and SE(3) datasets

• Analytic Jacobians

• Supports odometry and landmark edges

• Supports custom edge types (see tests/test_custom_edge.py for an example)

• Import and export .g2o files

Installation

pip install graphslam

Example Usage

SE(3) Dataset

>>> from graphslam.graph import Graph

>>> g = Graph.from_g2o("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.graph import Graph

>>> g = Graph.from_g2o("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       319950425.6477          43.488929
        2       124950341.8035          -0.609470
        3          338165.0770          -0.997294
        4             734.7343          -0.997827
        5             215.8405          -0.706233
        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)