search

uoip / g2opy

Python binding of SLAM graph optimization framework g2o
680 stars 177 forks source link
binding bundle-adjustment g2o graph-optimization icp python slam

readme

g2opy

This is a python binding of graph optimization C++ framework g2o.

g2o is an open-source C++ framework for optimizing graph-based nonlinear error functions. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA.
A wide range of problems in robotics as well as in computer-vision involve the minimization of a non-linear error function that can be represented as a graph. Typical instances are simultaneous localization and mapping (SLAM) or bundle adjustment (BA). The overall goal in these problems is to find the configuration of parameters or state variables that maximally explain a set of measurements affected by Gaussian noise. g2o is an open-source C++ framework for such nonlinear least squares problems. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA.

Currently, this project doesn't support writing user-defined types in python, but the predefined types are enough to implement the most common algorithms, say PnP, ICP, Bundle Adjustment and Pose Graph Optimization in 2d or 3d scenarios. g2o's visualization part is not wrapped, if you want to visualize point clouds or graph, you can give pangolin a try, it's a python binding of C++ library Pangolin.

For convenience, some frequently used Eigen types (Quaternion, Rotation2d, Isometry3d, Isometry2d, AngleAxis) are packed into this library.
In the contrib folder, I collected some useful 3rd-party C++ code related to g2o, like robust pose graph optimization library vertigo, stereo sba and smooth estimate propagator from sptam.

Requirements

Installation

git clone https://github.com/uoip/g2opy.git
cd g2opy
mkdir build
cd build
cmake ..
make -j8
cd ..
python setup.py install

Tested under Ubuntu 16.04, Python 3.6+.

Get Started

The code snippets below show the core parts of BA and Pose Graph Optimization in a SLAM system.

Bundle Adjustment

import numpy
import g2o

class BundleAdjustment(g2o.SparseOptimizer):
    def __init__(self, ):
        super().__init__()
        solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3())
        solver = g2o.OptimizationAlgorithmLevenberg(solver)
        super().set_algorithm(solver)

    def optimize(self, max_iterations=10):
        super().initialize_optimization()
        super().optimize(max_iterations)

    def add_pose(self, pose_id, pose, cam, fixed=False):
        sbacam = g2o.SBACam(pose.orientation(), pose.position())
        sbacam.set_cam(cam.fx, cam.fy, cam.cx, cam.cy, cam.baseline)

        v_se3 = g2o.VertexCam()
        v_se3.set_id(pose_id * 2)   # internal id
        v_se3.set_estimate(sbacam)
        v_se3.set_fixed(fixed)
        super().add_vertex(v_se3) 

    def add_point(self, point_id, point, fixed=False, marginalized=True):
        v_p = g2o.VertexSBAPointXYZ()
        v_p.set_id(point_id * 2 + 1)
        v_p.set_estimate(point)
        v_p.set_marginalized(marginalized)
        v_p.set_fixed(fixed)
        super().add_vertex(v_p)

    def add_edge(self, point_id, pose_id, 
            measurement,
            information=np.identity(2),
            robust_kernel=g2o.RobustKernelHuber(np.sqrt(5.991))):   # 95% CI

        edge = g2o.EdgeProjectP2MC()
        edge.set_vertex(0, self.vertex(point_id * 2 + 1))
        edge.set_vertex(1, self.vertex(pose_id * 2))
        edge.set_measurement(measurement)   # projection
        edge.set_information(information)

        if robust_kernel is not None:
            edge.set_robust_kernel(robust_kernel)
        super().add_edge(edge)

    def get_pose(self, pose_id):
        return self.vertex(pose_id * 2).estimate()

    def get_point(self, point_id):
        return self.vertex(point_id * 2 + 1).estimate()

Pose Graph Optimization

import numpy
import g2o

class PoseGraphOptimization(g2o.SparseOptimizer):
    def __init__(self):
        super().__init__()
        solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
        solver = g2o.OptimizationAlgorithmLevenberg(solver)
        super().set_algorithm(solver)

    def optimize(self, max_iterations=20):
        super().initialize_optimization()
        super().optimize(max_iterations)

    def add_vertex(self, id, pose, fixed=False):
        v_se3 = g2o.VertexSE3()
        v_se3.set_id(id)
        v_se3.set_estimate(pose)
        v_se3.set_fixed(fixed)
        super().add_vertex(v_se3)

    def add_edge(self, vertices, measurement, 
            information=np.identity(6),
            robust_kernel=None):

        edge = g2o.EdgeSE3()
        for i, v in enumerate(vertices):
            if isinstance(v, int):
                v = self.vertex(v)
            edge.set_vertex(i, v)

        edge.set_measurement(measurement)  # relative pose
        edge.set_information(information)
        if robust_kernel is not None:
            edge.set_robust_kernel(robust_kernel)
        super().add_edge(edge)

    def get_pose(self, id):
        return self.vertex(id).estimate()

For more details, checkout python examples or project stereo_ptam.
Thanks to pybind11, g2opy works seamlessly between numpy and underlying Eigen.

Motivation

This project is my first step towards implementing complete SLAM system in python, and interacting with Deep Learning models.
Deep Learning is the hottest field in AI nowadays, it has greatly benefited many Robotics/Computer Vision tasks, like

SLAM, as a subfield of Robotics and Computer Vision, is one of the core modules of robots, MAV, autonomous driving, and augmented reality. The combination of SLAM and Deep Learning (and Deep Learning driving computer vision techniques) is very promising, actually, there are increasing work in this direction, e.g. CNN-SLAM, SfM-Net, DeepVO, DPC-Net, MapNet, SuperPoint.
Deep Learning community has developed many easy-to-use python libraries, like TensorFlow, PyTorch, Chainer, MXNet. These libraries make writing/training DL models easier, and in turn boost the development of the field itself. But in SLAM/Robotics fields, python is still underrated, most of the software stacks are writen for C/C++ users. Lacking of tools makes it inconvenient to interact with the booming Deep Learning comunity and python scientific computing ecosystem.
Hope this project can slightly relieve the situation.

TODO

License

Contact

If you have problems related to binding code/python interface/python examples of this project, you can report isseus, or email me (qihang@outlook.com).







g2o's README:

g2o - General Graph Optimization

Linux: Build Status Windows: Build status

g2o is an open-source C++ framework for optimizing graph-based nonlinear error functions. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA.

A wide range of problems in robotics as well as in computer-vision involve the minimization of a non-linear error function that can be represented as a graph. Typical instances are simultaneous localization and mapping (SLAM) or bundle adjustment (BA). The overall goal in these problems is to find the configuration of parameters or state variables that maximally explain a set of measurements affected by Gaussian noise. g2o is an open-source C++ framework for such nonlinear least squares problems. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA. g2o offers a performance comparable to implementations of state-of-the-art approaches for the specific problems (02/2011).

Papers Describing the Approach:

Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard g2o: A General Framework for Graph Optimization IEEE International Conference on Robotics and Automation (ICRA), 2011 http://ais.informatik.uni-freiburg.de/publications/papers/kuemmerle11icra.pdf

Documentation

A detailed description of how the library is structured and how to use and extend it can be found in /doc/g2o.pdf The API documentation can be generated as described in doc/doxygen/readme.txt

License

g2o is licensed under the BSD License. However, some libraries are available under different license terms. See below.

The following parts are licensed under LGPL3+:

The following parts are licensed under GPL3+:

Please note that some features of CHOLMOD (which may be used by g2o, see libsuitesparse below) are licensed under the GPL. To avoid that your binary has to be licensed under the GPL, you may have to re-compile CHOLMOD without including its GPL features. The CHOLMOD library distributed with, for example, Ubuntu or Debian includes the GPL features. The supernodal factorization is considered by g2o, if it is available.

Within the folder EXTERNAL we include software not written by us to guarantee easy compilation.

See the doc folder for the full text of the licenses.

g2o is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.

Requirements

Optional requirements

Mac OS X

If using Homebrew, then

brew install homebrew/science/g2o

will install g2o together with its required dependencies. In this case no manual compilation is necessary.

Compilation

Our primary development platform is Linux. Experimental support for Mac OS X, Android and Windows (MinGW or MSVC). We recommend a so-called out of source build which can be achieved by the following command sequence.

The binaries will be placed in bin and the libraries in lib which are both located in the top-level folder. If you are compiling on Windows, please download Eigen3 and extract it. Within cmake-gui set the variable G2O_EIGEN3_INCLUDE to that directory.

Cross-Compiling for Android

Acknowledgments

We thank the following contributors for providing patches:

Contact information

Rainer Kuemmerle kuemmerl@informatik.uni-freiburg.de
Giorgio Grisetti grisetti@dis.uniroma1.it
Hauke Strasdat strasdat@gmail.com
Kurt Konolige konolige@willowgarage.com
Wolfram Burgard burgard@informatik.uni-freiburg.de