The following are some key references about 3D state estimation, 3D motion, 3D pose graph optimization.

Solà, Joan, Jeremie Deray, and Dinesh Atchuthan. “A micro Lie theory for state estimation in robotics.” arXiv preprint arXiv:1812.01537 (2018).

In this paper, Solà et al. describe the most basic principles of the Lie theory, with the aim of conveying clear and useful ideas, and leave a significant corpus of the Lie theory behind. Even with this mutilation, the material included here has proven to be extremely useful in modern estimation algorithms for robotics, especially in the fields of SLAM, visual odometry, and the like. Alongside this micro Lie theory, they provide a chapter with a few application examples, and a vast reference of formulas for the major Lie groups used in robotics, including most Jacobian matrices and the way to easily manipulate them. They also present a new C++ template-only library implementing all the functionality described here.

A small c++11 header-only library for Lie theory. https://github.com/artivis/manif

Solà, Joan http://www.joansola.eu/

 

 

 

Jos´e Luis Blanco Claraco. A tutorial on SE(3) transformation parameterizations and on-manifold optimization. Technical report #012010. Last update: 18/03/2019.

An arbitrary rigid transformation in SE(3) can be separated into two parts, namely, a translation and a rigid rotation. This technical report reviews, under a unifying viewpoint, three common alternatives to representing the rotation part: sets of three (yaw-pitch-roll) Euler angles, orthogonal rotation matrices from SO(3) and quaternions. It will be described: (i) the equivalence between these representations and the formulas for transforming one to each other (in all cases considering the translational and rotational parts as a whole), (ii) how to compose poses with poses and poses with points in each representation and (iii) how the uncertainty of the poses (when modeled as Gaussian distributions) is affected by these transformations and compositions. Some brief notes are also given about the Jacobians required to implement least-squares optimization on manifolds, an very promising approach in recent engineering literature. The text reflects which MRPT C++ library1 functions implement each of the described algorithms. All formulas and their implementation have been thoroughly validated by means of unit testing and numerical estimation of the Jacobians.

Jose Luis Blanco https://w3.ual.es/personal/jlblanco/

 

 

 

Forster, Christian, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. “On-Manifold Preintegration for Real-Time Visual–Inertial Odometry.” IEEE Transactions on Robotics 33, no. 1 (2017): 1-21.

In this paper, Forster et al. present a preintegration theory that properly addresses the manifold structure of the rotation group. They formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the maximum a posteriori state estimation. Their theoretical development enables the computation of all necessary Jacobians for the optimization and a posteriori bias correction in analytic form. They also show that the preintegrated inertial measurement unit model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a structureless model for visual measurements, which avoids optimizing over the 3D points, further accelerating the computation.

Christian Forster https://www.cforster.ch/

Luca Carlone https://lucacarlone.mit.edu/

Frank Dellaert https://www.cc.gatech.edu/~dellaert/FrankDellaert/Frank_Dellaert/Frank_Dellaert.html

Davide Scaramuzza http://rpg.ifi.uzh.ch/people_scaramuzza.html

 

 

 

Grisetti, Giorgio, Rainer Kummerle, Cyrill Stachniss, and Wolfram Burgard. “A tutorial on graph-based SLAM.” IEEE Intelligent Transportation Systems Magazine 2, no. 4 (2010): 31-43.

One intuitive way of formulating SLAM is to use a graph whose nodes correspond to the poses of the robot at different points in time and whose edges represent constraints between the poses. The latter are obtained from observations of the environment or from movement actions carried out by the robot. Once such a graph is constructed, the map can be computed by finding the spatial configuration of the nodes that is mostly consistent with the measurements modeled by the edges. In this paper, they provide an introductory description to the graph-based SLAM problem. Furthermore, they discuss a state-of-the-art solution that is based on least-squares error minimization and exploits the structure of the SLAM problems during optimization.

 

 

 

Carlone, Luca, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. “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, 2015.

Pose graph optimization is the non-convex optimization problem underlying pose-based Simultaneous Localization and Mapping (SLAM). If robot orientations were known, pose graph optimization would be a linear leastsquares problem, whose solution can be computed efficiently and reliably. Since rotations are the actual reason why SLAM is a difficult problem, in this work they survey techniques for 3D rotation estimation. Rotation estimation has a rich history in three scientific communities: robotics, computer vision, and control theory. They review relevant contributions across these communities, assess their practical use in the SLAM domain, and benchmark their performance on representative SLAM problems. They show that the use of rotation estimation to bootstrap iterative pose graph solvers entails significant boost in convergence speed and robustness.

 

 

 

Grisetti, Giorgio, Cyrill Stachniss, and Wolfram Burgard. “Nonlinear constraint network optimization for efficient map learning.” IEEE Transactions on Intelligent Transportation Systems 10, no. 3 (2009): 428-439.

In this paper, the authors present a highly efficient maximum likelihood approach that is able to solve 3D as well as 2D problems. Their approach addresses the so-called graph-based formulation of the simultaneous localization and mapping (SLAM) and can be seen as an extension of Olson’s algorithm [27] towards non-flat environments. It applies a novel parameterization of the nodes of the graph that significantly improves the performance of the algorithm and can cope with arbitrary network topologies. The latter allows us to bound the complexity of the algorithm to the size of the mapped area and not to the length of the trajectory. Furthermore, their approach is able to appropriately distribute the roll, pitch and yaw error over a sequence of poses in 3D mapping problems. They implemented the technique and compared it to multiple other graph-based SLAM solutions. As they demonstrate in simulated and in real world experiments, their method converges faster than the other approaches and yields accurate maps of the environment.

 

 

 

Frank Dellaert. Factor Graphs and GTSAM: A Hands-on Introduction, Technical Report GT-RIM-CP&R-2012-002.

Factor graphs are graphical models (Koller and Friedman, 2009) that are well suited to modeling complex estimation problems, such as Simultaneous Localization and Mapping (SLAM) or Structure from Motion (SFM). You might be familiar with another often used graphical model, Bayes networks, which are directed acyclic graphs. A factor graph, however, is a bipartite graph consisting of factors connected to variables. The variables represent the unknown random variables in the estimation problem, whereas the factors represent probabilistic information on those variables, derived from measurements or prior knowledge. In the report Dellaert showed many examples from both robotics and vision. The GTSAM toolbox (GTSAM stands for “Georgia Tech Smoothing and Mapping”) toolbox is a BSD-licensed C++ library based on factor graphs, developed at the Georgia Institute of Technology by Dellaert, many of his students, and collaborators. It provides state of the art solutions to the SLAM and SFM problems, but can also be used to model and solve both simpler and more complex estimation problems. It also provides a MATLAB interface which allows for rapid prototype development, visualization, and user interaction. GTSAM exploits sparsity to be computationally efficient. Typically measurements only provide information on the relationship between a handful of variables, and hence the resulting factor graph will be sparsely connected. This is exploited by the algorithms implemented in GTSAM to reduce computational complexity. Even when graphs are too dense to be handled efficiently by direct methods, GTSAM provides iterative methods that are quite efficient regardless. You can download the latest version of GTSAM at http://tinyurl.com/gtsam.

 

Frank Dellaert. Lie Groups for Beginners. 2016 in GTSAM repository /doc/LieGroups.pdf

Frank Dellaert. “Derivatives and Differentials” 2016. in GTSAM repository /doc/math.pdf

 

 

 

Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.

The state of a robot is a set of quantities, such as position, orientation, and velocity, that, if known, fully describe that robot’s motion over time. Here Barfoot focuses entirely on the problem of estimating the state of a robot, putting aside the notion of control. Yes, control is essential, as we would like to make our robots behave in a certain way. But, the first step in doing so is often the process of determining the state. Moreover, the difficulty of state estimation is often underestimated for real-world problems, and thus it is important to put it on an equal footing with control. In this book, Barfoot introduces the classic estimation results for linear systems corrupted by Gaussian measurement noise. He then examines some of the extensions to nonlinear systems with non-Gaussian noise. In a departure from typical estimation texts, He takes a detailed look at how to tailor general estimation results to robots operating in three-dimensional space, advocating a particular approach to handling rotations.

Tim Barfoot http://asrl.utias.utoronto.ca/~tdb/

 

 

Ethan Eade. Lie Groups for 2D and 3D Transformations. 2018

This document derives useful formula for working with the Lie groups that represent transformations in 2D and 3D space. A Lie group is a topological group that is also a smooth manifold, with some other nice properties. Associated with every Lie group is a Lie algebra, which is a vector space discussed below. Importantly, a Lie group and its Lie algebra are intimately related, allowing calculations in one to be mapped usefully into the other.

Ethan Eade http://ethaneade.com/

 

 

Ethan Eade. Gauss-Newton / Levenberg-Marquardt Optimization. 2013

The document describes the principle of Gauss-Newton / Levenberg-Marquardt Optimization in 3D Graph Optimization. 

Ethan Eade http://ethaneade.com/

Henri P. Gavin. The Levenberg-Marquardt algorithm for nonlinear least squares curve-fitting problems. Duke University, January 10, 2019. 

 

 

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 

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

g2o code https://github.com/RainerKuemmerle/g2o 

 

 

 

Niko Sünderhauf. Vertigo: Versatile Extensions for Robust Inference using Graph Optimization. 2012.

Vertigo is an extension library for g2o and gtsam 2.0. It provides a C++ implementation of the switchable constraints described in
Sünderhauf 2012 a, b, c. This extension enables g2o or gtsam to solve pose graph SLAM problems in 2D and 3D despite a large number of false positive loop closure constraints. 

More info about Switchable Constraints for Robust SLAM, please visit the author’s website Switchable Constraints for Robust SLAM. 

Sünderhauf, Niko, and Peter Protzel. “Towards a robust back-end for pose graph slam.” In 2012 IEEE International Conference on Robotics and Automation, pp. 1254-1261. IEEE, 2012.

Sünderhauf, N., Protzel, P. (2012). Switchable Constraints for Robust Pose Graph SLAM. Proc. of IEEE International Conference on Intelligent Robots and Systems (IROS), Vilamoura, Portugal.

Sünderhauf, N. (2012). Robust Optimization for Simultaneous Localization and Mapping. PhD Thesis, Chemnitz University of Technology.

Sünderhauf, Niko, and Peter Protzel. “Switchable constraints vs. max-mixture models vs. RRR-A comparison of three approaches to robust pose graph SLAM.” In 2013 IEEE International Conference on Robotics and Automation, pp. 5198-5203. IEEE, 2013.

Niko Sünderhauf https://nikosuenderhauf.github.io/