I want to implement my own pose graph SLAM following [1]. Since my vehicle is moving in 3D-space i represent my pose using a 3D-translation vector and a quaternion for orientation. [1] tells me that it's necessary to adapt their algorithm 1 by using manifolds to project the poses into euclidean space. I also studied the approach of [2]. In section "IV.B. Nonlinear Systems" they write that their approach remains valid for nonlinear systems. I conclude that for their case it's not obligatory to make use of a manifold. But I don't understand how they avoid it. So my questions are:

  1. Is it correct that there is an alternative to manifolds?
  2. If yes, how does this alternative look like?

[1] Grisetti, G., Kummerle, R., Stachniss, C., & Burgard, W. (2010). A tutorial on graph-based SLAM. Intelligent Transportation Systems Magazine, IEEE, 2(4), 31-43.

[2] Kaess, M., Ranganathan, A., & Dellaert, F. (2008). iSAM: Incremental smoothing and mapping. Robotics, IEEE Transactions on, 24(6), 1365-1378.


1 Answer 1


You might want to have a look at [1] where the author uses iSAM for estimating - among other quantities - the position and attitude of a rigid body spinning in microgravity.

In section 4.5 p111 he mentions using a combination of {attitude error vector + reference quaternion} within the nodes of the pose graph :

*It is clear that any representations of attitude include nonlinear transformations and kinematics. This causes a problem for modelling and propagating probability distribution functions with Gaussian random variables, such as those typically used in Extended Kalman Filters or the iSAM system. It is well understood that the covariance matrix of a quaternion is rank deficient due to its normalization constraint. While there is active research in a number of estimation systems that do not use Gaussian random variables [83, 109, 127, 118, 119], a typical approach for dealing with this is to use three vector error parameterization and reset the quaternion (see [26, 70, 82, 134]), which is what will be used here since it fits well with the iSAM system for Gaussian random variables and has a history of good performance.

This error vector and reference quaternion approach can be applied to pose graph optimization methods such as iSAM. For each of the nodes that specify the vehicle’s 6DOF trajectory, the reference quaternion approach is mirrored. This means that at the vehicle’s state nodes for each timestep, both a four parameter reference quaternion and a three parameter attitude error is stored. Each time the optimization problem is re-linearized, a reset step is performed. This reset step transfers all of the attitude error into the reference quaternion."

In his particular case he uses Modified Rodriguez Parameters (MRP) but you could probably choose an alternative representation if you keep the idea of combining an error vector with a quaternion within the nodes.

[1] Tweddle, B. E., Computer Vision-Based Localization and Mapping of an Unknown, Uncooperative and Spinning Target for Spacecraft Proximity Operations, Ph.D. thesis, Massachusetts Institute of Technology, Department of Aeronautics and Astronautics, Cambridge, MA, 2013.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.