Rapid development of manifold-based graph optimization systems for multi-sensor calibration and SLAM

Non-linear optimization on constraint graphs has recently been applied very successfully in a variety of SLAM backends. We combine this technique with a principled way of handling non-Euclidean spaces, 3D orientations in particular, based on manifolds to build a generic and very flexible framework, the Manifold Toolkit for Matlab (MTKM). We show that MTKM makes it particularly easy to solve non-trivial multi-sensor calibration problems while remaining generic enough to handle a very different class of problems, namely SLAM, as well: After an introductory example on single camera calibration we apply MTKM to calibration of stereo vision and IMU w.r.t. the kinematic chain of a service robot, RGB-D and accelerometer calibration of a Microsoft Kinect, stereo calibration on a Nao soccer robot, and several SLAM benchmark data sets illustrating MTKM's versatility. MTKM and all presented examples are available as open source from http://openslam.org/MTK.html.