Improved Pose Graph Optimization for Planar Motions Using Riemannian Geometry on the Manifold of Dual Quaternions

07/31/2019 ∙ by Kailai Li, et al. ∙ KIT 0

We present a novel Riemannian approach for planar pose graph optimization problems. By formulating the cost function based on the Riemannian metric on the manifold of dual quaternions representing planar motions, the nonlinear structure of the SE(2) group is inherently considered. To solve the on-manifold least squares problem, a Riemannian Gauss-Newton method using the exponential retraction is applied. The proposed Riemannian pose graph optimizer (RPG-Opt) is further compared with currently popular optimization frameworks using public planar pose graph datasets. Evaluations show that the proposed method gives equivalently accurate results as the state-of-the-art frameworks and shows better convergence robustness under large uncertainties of odometry measurements.



There are no comments yet.


page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Pose graph optimization [1]

plays a fundamental role in robotic and computer vision tasks such as simultaneous localization and mapping (SLAM), structure from motion (SfM), etc. A pose graph is a graph with nodes representing robot poses and edges linking the nodes, between which odometry measurements are available. Pose graph optimization problems are typically formulated to maximize the likelihood of the observed odometry measurements of all the edges with constraints imposed by the group structure. They are non-convex with multiple local minima 

[2] . Mathematically speaking, the planar rigid motions belong to the special Euclidean group SE(2). The frequently used representation approach in the context of planar pose graph optimization is the two-dimensional rotation matrix plus the translation  . Consequently, the

-DoF planar rigid motions are overparameterized by six elements, which leads to three degrees of redundancy. The cost function for performing the maximum likelihood estimation (MLE) is normally formulated as the sum of individual rotational and translational error, which lacks consideration of the underlying group structure. This can be problematic when the rotation and translation errors are at different scales of uncertainty and a proper scaling ratio of the two error terms is not available. Moreover, for highly nonlinear and uncertain rigid motions, the structure-unaware formulations and conventional solvers for constrained optimization problems may have the risk of non-convergence and are prone to local minima.

The planar dual quaternions are an alternative representation approach for planar rigid motions and can be written as four-dimensional vectors with only one degree of redundancy 

[3, 4]. More importantly, planar dual quaternions form a Riemannian manifold [5, 6], which is equipped with the so-called Riemannian metric for measuring on-manifold uncertainty in accordance with the manifold structure. Furthermore, optimization approaches proposed on Riemannian manifolds, or equivalently Riemannian optimization approaches, have been gaining in popularity over the years [7]. Compared to conventional solvers for constrained optimization problems that use, e.g., the Lagrange multipliers, the family of Riemannian optimizers have shown better convergence efficiency and robustness as they exploit the geometric structure of the manifold.

Furthermore, recently there have been works dedicated to geometry-aware stochastic filters for pose estimation using the (dual) quaternion representation [8, 9]. In [10], the Bingham distribution is introduced to stochastically model the uncertainty of unit quaternions and a corresponding filtering approach is proposed for orientation estimation. In [11], a dual quaternion-based approach has been proposed for stochastic filtering of rigid body motions. To the best knowledge of the authors, there is no method proposed so far for planar pose graph optimization based on the dual quaternion representation.

In this paper, we propose the so-called Riemannian pose graph optimizer (RPG-Opt) for planar motions represented by dual quaternions. Here, odometry errors of edges are measured based on the Riemannian metric on the manifold of planar dual quaternions. Unlike conventional solvers for constrained optimization problems [12, 13], we apply the Riemannian Gauss-Newton approach on the manifold with exponential retraction for updates. Both the cost function formulation and optimization approach are geometry-aware and inherently consider the underlying nonlinear structure of the SE(2) group. For evaluation, we compare the proposed approach with state-of-the-art pose graph optimization frameworks (e.g., GTSAM [14], g2o [15] and iSAM [16]) based on public planar pose graph datasets under ordinary odometry noise level. We further synthesize new datasets with additionally larger odometry noise for the evaluation. Comparisons show that our approach gives equally accurate results as the state-of-the-art frameworks and gives better accuracy under large odometry uncertainties.

The remainder of the paper is as follows. In Sec. II, we introduce the dual quaternion parameterization for planar rigid motions and the geometric structure of the planar dual quaternion manifold. Sec. III introduces the proposed cost function and the on-manifold optimizer, the RPG-opt. We further evaluate the proposed approach regarding accuracy and convergence robustness in Sec. IV. The work is finally concluded in Sec. V.

Ii Planar Dual Quaternion Parameterization
and Manifold Structure

Ii-a Dual Quaternion-based Parameterization for
    Planar Rigid Motions

By convention [17], unit quaternions representing planar rotations are written as  ,with the unit vector indicating the -axis, around which a rotation of angle is performed. Every can be rotated to according to via

with denoting the Hamilton product [18] and the conjugate of  . Moreover, the planar rotation quaternions can be reformulated into the following vector form


which are located on the unit circle on the -plane. , their Hamilton product can also be represented as ordinary matrix-vector multiplication, namely , with


It is then trivial to confirm that both the left and right matrix representation belong to the two-dimensional rotation group SO(2), i.e., and  . The dual quaternion representation for planar rigid motions is defined as with denoting the dual number which satisfies and the translation. The corresponding vector form of planar dual quaternions can then be written as


with the real part defined as in (1) and the dual part


Therefore, the manifold of planar dual quaternions (denoted as ) is the Cartesian product of unit circle and the two-dimensional Euclidean space . The arithmetics of dual quaternions are the combination of the Hamilton product and the dual number theory. For instance,  , their product in the form of matrix-vector multiplication can be written as  , with

Similar to the rotation rule of unit quaternions, every can be transformed to via a planar rotation of followed by a translation according to


with being the conjugate of  . Here, is a diagonal matrix with the arguments placed at the diagonal entries. It should be noted that the vector form of dual quaternions for planar motions is under the coordinate of  , which is a reordering of the general quaternion coordinate  .

Ii-B Manifold Structure of Dual Quaternions
    Representing Planar Rigid Motions

The manifold of planar dual quaternions can be derived as the zero-level set of the following vector function defined in

where . This can be further used to derive the tangent plane at by calculating the null space of the Jacobian of evaluated at , i.e.,

, its orthogonal projection to can be obtained via , with being the projection matrix evaluated at  . As there only exists the unit norm constraint for the real part, the projection matrix can be derived as the following form [19]


with  . Note that the projection matrix is symmetric and idempotent, namely  .

The logarithm map of unit dual quaternions representing the SE(3) states can be obtained via the reparameterization into screw motions of according to the Lie algebra [17, 6]. Here, denotes the screw angle (identical to the rotation angle), the translation along the screw axis (identical to rotation axis) and

the screw moment. The logarithm map at the identity dual quaternion

(a quaternion representing zero rotation and zero translation) is given as

with  . The screw moment is computed with respect to the projected origin on the screw axis, namely  . Consequently, the logarithm map of dual quaternions representing planar rigid motions can be derived as a degenerate case of the spatial motions. Therefore, the projected point is the intersection of the screw axis with the -plane and can be computed according to [6] as

Considering that the translation along the screw axis is zero, we have the logarithm map of planar dual quaternion derived as

It can be further derived that

According to the definition of the dual part in (4), this can be further derived into a more concise form as

Therefore, the logarithm of planar dual quaternion can be written as


with and . Moreover, the logarithm map to the tangent plane at can be computed according to the parallel transport [6] on the manifold of planar dual quaternions as follows


Conversely, the exponential map at identity moves a point from the tangent plane at back to the manifold and can be derived as


Here, as derived in (7). Similar to the logarithm map, the exponential map for arbitrary tangent plane locations can be derived according to the parallel transport [6] with


Here, and we take out the last three indices of as its first element is zero according to (8).

Iii RPG-Opt: A Riemannian Pose Graph Optimizer

Iii-a Geometry-aware Cost Function Based on
    Riemannian Metric

The pose graph optimization problem is formulated as the maximum likelihood estimation given observed odometry. The optimized poses on the graph can then be obtained by minimizing the sum of the distance metrics through each edge of the graph, namely


with being the poses of graph nodes concatenated into one vector, where each pose is a planar dual quaternion. Here, denotes the uncertainty of the odometry measurement in the form of information matrix. The manifold for optimization is thus the Cartesian product of the planar dual quaternion manifold , which is also a Riemannian manifold [20]. The cost function


is thus a scalar function proposed on the manifold , namely  . Unlike the existing works [21, 22, 15], we propose a cost function based on the Riemannian metric [7, 23] on the manifold of planar dual quaternions, which inherently considers the geometric structure of the nonlinear manifold. Given the odometry measurement of the edge connecting two nodes and , the error term of the edge is defined as


such that the cost function for a single edge can be derived according to the Riemannian metric as


which is the Mahalanobis distance measured on the tangent plane at the identity planar dual quaternion  .

Iii-B Riemannian Gauss-Newton Method for On-manifold
    Planar Pose Graph Optimization

We apply a Riemannian Gauss-Newton approach [7] for solving the nonlinear least square problem formulated in (11). Here, a new iteration is obtained via the retraction [7] of the on-tangent-plane Newton step back to the manifold, namely


The iterative step results from the Riemanian gradient and the Riemannian Hessian via


with being the concatenated on-tangent-plane steps calculated for each nodes. Without loss of generality, we perform the following derivations based on the cost function of one single edge joining the two nodes and . For better readability, we ignore the iteration index  . The Riemannian gradient with respect to , can be computed by orthogonally projecting the ordinary gradient of onto the tangent plane at  [7] via  , with denoting the projection matrix to given in (6) . Here, denotes the classical gradient in the Euclidean space which can be derived as follows




Here, is the classical Jacobian of the error metric with respect to . Similarly, the Riemannian gradient at node can be calculated as


with denoting the classical Jacobian of with respect to  . Therefore, the Riemannian gradient of can be computed by traversing all the nodes through the graph with each entry computed as

The Riemannian Hessian can be computed by approximation [7] with entries derived as follows

with indicating the location of the block matrices in the Hessian matrix corresponding to the nodes and  . Finally, the iterative step can be obtained by solving the linear system

with denoting the on-tangent-plane iterative step concatenated through each node.

The retraction maps a point from the tangent plane back to the manifold, namely  , which is not unique. For instance, there have been different projection-like retractions proposed in [19]. We hereby use the exponential retraction for with each node undergoing the exponential map defined in (10) for update, namely

After the retraction back to the manifold, the cost function can then be linearized at the tangent plane at . The optimization is stopped until the norm of the Riemannian gradient becomes sufficiently small.

Iv Evaluation

We evaluate the proposed RPG-Opt based on publicly available planar pose graph datasets [2, 12] and compare it with three state-of-the-art pose graph optimization frameworks: g2o [1], GTSAM [14] and iSAM [16]. We choose to use Levenberg-Marquardt algorithm for iSAM and the Gauss-Newton method for GTSAM, respectively. In g2o, the optimization is performed using the Gauss-Newton algorithm.

For most of the publicly available pose graph datasets, the uncertainty of the odometry measurements is described by the information matrix for the state   with being the rotation angle and the translation. In order to incorporate the estimated uncertainty, the logarithm map derived in (7) can be reformulated into the following form


Here, and is the rotation angle and translation term of the planar dual quaternion  . The equation in (20) is essentially a nonlinear transformation of the state provided by the dataset. For each iteration step, we can assume that the odometry error is small, such that and  . Therefore, the information matrix from the raw dataset can be directly deployed as the on-tangent-plane information matrix in the metric of (20) . When the information matrices for odometry are not available, a typical way to formulate the cost function is to set  .

In the following evaluations, we first test the proposed approach based on datasets under ordinary odometry noise level using the identity and real information matrix, respectively. Secondly, we synthesize datasets with additionally added noise and perform the evaluation based on the ground truth.

Iv-a Evaluation Under Ordinary Odometry Noise Level

We first evaluate the proposed approach using datasets with ordinary level of odometry noise. As no ground truth is available, we use the cost function of g2o for comparison [15], namely


with being the ordinary rotation matrices and the translations. Here, the logarithm map of rotation matrices can be derived from Lie algebra as given in [24]. Table I shows the results for both identity and real information matrices. Our approach, the RPG-Opt, reaches comparably accurate results as state of the art.

Dataset * RPG-Opt10 g2o10 GTSAM iSAM

* The optimizations are done for both identity (denoted as ) and real information matrices from datasets (denoted as ). Here, g2o and RPG-Opt iteration steps are fixed to be  , whereas GTSAM and iSAM uses its default stopping criterion.
TABLE I: Comparison of RPG-Opt with g2o, GTSAM and iSAM using public datasets [12] based on the g2o cost function.

M3500a+ M3500b+ M3500c+ City10000a City10000b City10000c





Fig. 1: Evaluation using synthetic datasets based on ground truth. Here, the optimizations are initialized with odometry measurements. Results from GTSAM are not listed because of nonconvergence. The frameworks iSAM and g2o are prone to local minima. The iteration steps for RPG-Opt and g2o are fixed to be and iSAM is using its default stopping criteria. Though initialized directly with the odometry measurements, the proposed RPG-Opt shows the best accuracy and robustness against local minima under large uncertainty. Here, datasets with ‘a’ (e.g., M3500a+ or City10000a) in the name are added with equivalently scaled noise for rotations and translations. Datasets with ‘b’ in the names have different scaling for the uncertainties of rotation and translation. Datasets with ‘c’ in the names have nonequivalent and correlated translational and rotational noise for the odometry measurements. The RPE for rotation are in angular degree. The additional odometry noise is set up given the covariance , thus we have  .
RPG-Opt chord + RPG-Opt chord + g2o chord + GTSAM

* For the datasets ‘M3500a+’, ‘M3500b+’, ‘M3500c+’ and ‘City10000a’, the proposed approach directly initialized with odometry measurements achieves errors that are the same as the other approaches with the chordal initialization. Results from iSAM are not available, because external initlizations are not supported by the framework.
TABLE II: Summarized evaluation results for synthetic datasets with chordal relaxation-based initialization.
(a) Change of RPE for translational and rotational terms (in angular degree) for each iteration.
(b) Change of cost function values defined in (12) for each iteration.
Fig. 2: Convergence behavior of RPG-Opt and g2o for the dataset M3500c+.

M3500d+ City10000d
chord s
chord + GTSAM
chord + g2o
chord + RPG-Opt

Fig. 3: Sample visualization of optimization results with chordal relaxation-based initialization under large odometry noise. The full list for all the synthetic datasets can be found in Table II.

Iv-B Evaluation Based on Synthetic Datasets

We further synthesize two groups of datasets with known information matrices for the evaluation under large uncertainty of odometry measurements. For that, we add additional odometry noise to the datasets ‘M3500’ and ‘City10000’ [2]. Fig. 1 shows the results comparison based on the relative pose error (RPE) for both translations and rotations, which are denoted as and , respectively. Note that the optimizations are initialized directly from the odometry measurement. Results from GTSAM using the Gauss-Newton approach are not listed because of nonconvergence. For all the sequences, the g2o and iSAM framework can converge easily to the local minima. However, the proposed approach shows the best accuracy and robustness against local minima, though no special initialization is performed.

For pose graph optimization under large uncertainty of odometry measurements, it is typical to equip the optimizer with an additional initialization block for better convergence. Therefore, we incorporate an initialization method based on chordal relaxation [24]  in the evaluation. Fig. 3 visualizes evaluation results with chordal relaxation-based initialization using another two synthetic datasets based on the ‘M3500’ and ‘City10000’. In this case, the g2o shows significant improvement, achieving the same converged accuracy as the proposed approach. As a summary, Table II shows the optimization results with chordal initialization for all the synthetic datasets with respect to the ground truth. We additionally list the results with direct odometry initialization for the RPG-Opt to show the convergence robustness under large uncertainties of the proposed method .

Iv-C Convergence

We further compare the convergence behavior of the proposed approach with g2o as shown in Fig. 2. Here, the sequence ‘M3500c+’ is used and the RPE for both translation and rotations are plotted (Fig. 2-a). Fig. 2-b shows the convergence with respect to the proposed Riemannian metric in (12). The optimizations are initialized with odometry and the real information matrices are used. In both figures, the proposed approach shows faster convergence and better robustness against local minimum than g2o. This mainly results from the geometry-aware cost function formulation and the Riemannian Gauss-Newton of the proposed approach.

V Conclusion

In this work, we have proposed a Riemannian approach for planar pose graph optimization problems on the manifold of dual quaternions. Here, the cost function is built upon the Riemannian metric of the planar dual quaternion manifold, based on which a Riemannian Gauss-Newton method is applied using the exponential retraction. Both the on-manifold MLE formulation and optimization is geometry-aware, which inherently considers the underlying nonlinear structure of the SE(2) group. Evaluations using real-world and synthetic datasets have shown that the proposed approach gives equally accurate results under ordinary odometry noise and shows better accuracy and robustness under large uncertainty of odometry measurements than the state-of-the-art frameworks.

Based on the presented work, there is still much potential to exploit. The proposed Riemannian approach can be extended to the general dual quaternion-based spatial pose graph optimization [25, 26]. Also, numerous optimizers from the family of Riemannian optimization can be further applied for better convergence robustness and accuracy.


  • [1] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A Tutorial on Graph-based SLAM,” IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31–43, 2010.
  • [2] L. Carlone and A. Censi, “From Angular Manifolds to the Integer Lattice: Guaranteed Orientation Estimation with Application to Pose Graph Optimization,” IEEE Transactions on Robotics, vol. 30, no. 2, pp. 475–492, 2014.
  • [3] K. Li, G. Kurz, L. Bernreiter, and U. D. Hanebeck, “Nonlinear Progressive Filtering for SE(2) Estimation,” in Proceedings of the 21st International Conference on Information Fusion (Fusion 2018), Cambridge, United Kingdom, Jul. 2018.
  • [4] ——, “Simultaneous Localization and Mapping Using a Novel Dual Quaternion Particle Filter,” in Proceedings of the 21st International Conference on Information Fusion (Fusion 2018), Cambridge, United Kingdom, Jul. 2018.
  • [5] M. P. do Carmo, Riemannian Geometry.   Birkhäuser, 1992.
  • [6] B. Busam, T. Birdal, and N. Navab, “Camera Pose Filtering with Local Regression Geodesics on the Riemannian Manifold of Dual Quaternions,” in Proceedings of the 2017 IEEE International Conference on Computer Vision Workshop (ICCVW 2017), 2017, pp. 2436–2445.
  • [7] P.-A. Absil, R. Mahony, and R. Sepulchre, Optimization Algorithms on Matrix Manifolds.   Princeton University Press, 2009.
  • [8] K. Li, F. Pfaff, and U. D. Hanebeck, “Hyperspherical Deterministic Sampling Based on Riemannian Geometry for Improved Nonlinear Bingham Filtering,” in Proceedings of the 22nd International Conference on Information Fusion (Fusion 2019), Ottawa, Canada, Jul. 2019.
  • [9] K. Li, D. Frisch, B. Noack, and U. D. Hanebeck, “Geometry-Driven Deterministic Sampling for Nonlinear Bingham Filtering,” in Proceedings of the 2019 European Control Conference (ECC 2019), Naples, Italy, Jun. 2019.
  • [10] J. Glover and L. P. Kaelbling, “Tracking the Spin on a Ping Pong Ball with the Quaternion Bingham Filter,” in 2014 IEEE International Conference on Robotics and Automation (ICRA 2014), May 2014, pp. 4133–4140.
  • [11] R. A. Srivatsan, M. Xu, N. Zevallos, and H. Choset, “Probabilistic Pose Estimation Using a Bingham Distribution-based Linear Filter,” The International Journal of Robotics Research, June 2018.
  • [12] L. Carlone, R. Aragues, J. A. Castellanos, and B. Bona, “A fast and accurate approximation for planar pose graph optimization,” The International Journal of Robotics Research, vol. 33, no. 7, pp. 965–987, 2014.
  • [13] L. Carlone, D. M. Rosen, G. Calafiore, J. J. Leonard, and F. Dellaert, “Lagrangian Duality in 3D SLAM: Verification Techniques and Optimal Solutions,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2015), 2015, pp. 125–132.
  • [14] F. Dellaert, “Factor Graphs and GTSAM: A Hands-on Introduction,” Georgia Institute of Technology, Tech. Rep., 2012.
  • [15] R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g2o: A General Framework for Graph Optimization,” in Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA 2011), 2011, pp. 3607–3613.
  • [16] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental smoothing and mapping,” IEEE Trans. on Robotics (TRO), vol. 24, no. 6, pp. 1365–1378, Dec. 2008.
  • [17] J. Brookshire and S. Teller, “Extrinsic Calibration from Per-sensor Egomotion,” Robotics: Science and Systems VIII, pp. 504–512, 2013.
  • [18] W. R. Hamilton, “On Quaternions; or on a New System of Imaginaries in Algebra,” Philosophical Magazine Series 3, vol. 25, no. 163, pp. 10–13, 1844.
  • [19] P.-A. Absil and J. Malick, “Projection-like Retractions on Matrix Manifolds,” SIAM Journal on Optimization, vol. 22, no. 1, pp. 135–158, 2012.
  • [20] T. Birdal, U. Şimşekli, M. O. Eken, and S. Ilic, “Bayesian Pose Graph Optimization via Bingham Distributions and Tempered Geodesic MCMC,” arXiv preprint arXiv:1805.12279, 2018.
  • [21] M. Kaess and F. Dellaert, “Covariance Recovery from a Square Root Information Matrix for Data Association,” Journal of Robotics and Autonomous Systems (RAS), vol. 57, pp. 1198–1210, Dec. 2009.
  • [22] F. Dellaert and M. Kaess, “Square Root SAM: Simultaneous localization and mapping via square root information smoothing,” Intl. J. of Robotics Research (IJRR), vol. 25, no. 12, pp. 1181–1204, Dec. 2006.
  • [23]

    G. Arvanitidis, L. K. Hansen, and S. Hauberg, “A Locally Adaptive Normal Distribution,” in

    Advances in Neural Information Processing Systems (NIPS), jun 2016.
  • [24] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert, “Initialization Techniques for 3D SLAM: A Survey on Rotation Estimation and its Use in Pose Graph Optimization,” in Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA 2015), 2015, pp. 4597–4604.
  • [25] K. Li, F. Pfaff, and U. D. Hanebeck, “Geometry-Driven Stochastic Modeling of SE(3) States Based on Dual Quaternion Representation,” in Proceedings of the 2019 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2019), Taipei, Republic of China, May 2019.
  • [26] S. Bultmann, K. Li, and U. D. Hanebeck, “Stereo Visual SLAM Based on Unscented Dual Quaternion Filtering,” in Proceedings of the 22nd International Conference on Information Fusion (Fusion 2019), Ottawa, Canada, Jul. 2019.