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 nonconvex 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 twodimensional rotation matrix plus the translation . Consequently, theDoF 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 structureunaware formulations and conventional solvers for constrained optimization problems may have the risk of nonconvergence and are prone to local minima.
The planar dual quaternions are an alternative representation approach for planar rigid motions and can be written as fourdimensional 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 socalled Riemannian metric for measuring onmanifold 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 geometryaware 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 quaternionbased 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 socalled Riemannian pose graph optimizer (RPGOpt) 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 GaussNewton approach on the manifold with exponential retraction for updates. Both the cost function formulation and optimization approach are geometryaware and inherently consider the underlying nonlinear structure of the SE(2) group. For evaluation, we compare the proposed approach with stateoftheart 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 stateoftheart 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 onmanifold optimizer, the RPGopt. 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
Iia Dual Quaternionbased 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
(1) 
which are located on the unit circle on the plane. , their Hamilton product can also be represented as ordinary matrixvector multiplication, namely , with
(2) 
It is then trivial to confirm that both the left and right matrix representation belong to the twodimensional 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
(3) 
with the real part defined as in (1) and the dual part
(4) 
Therefore, the manifold of planar dual quaternions (denoted as ) is the Cartesian product of unit circle and the twodimensional 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 matrixvector 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
(5) 
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 .
IiB Manifold Structure of Dual Quaternions
Representing Planar Rigid Motions
The manifold of planar dual quaternions can be derived as the zerolevel 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]
(6) 
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 aswith . 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
(7) 
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
(8) 
Conversely, the exponential map at identity moves a point from the tangent plane at back to the manifold and can be derived as
(9) 
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
(10) 
Here, and we take out the last three indices of as its first element is zero according to (8).
Iii RPGOpt: A Riemannian Pose Graph Optimizer
Iiia Geometryaware 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
(11) 
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
(12) 
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
(13) 
such that the cost function for a single edge can be derived according to the Riemannian metric as
(14)  
which is the Mahalanobis distance measured on the tangent plane at the identity planar dual quaternion .
IiiB Riemannian GaussNewton Method for Onmanifold
Planar Pose Graph Optimization
We apply a Riemannian GaussNewton approach [7] for solving the nonlinear least square problem formulated in (11). Here, a new iteration is obtained via the retraction [7] of the ontangentplane Newton step back to the manifold, namely
(15) 
The iterative step results from the Riemanian gradient and the Riemannian Hessian via
(16) 
with being the concatenated ontangentplane 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
(17) 
therefore
(18) 
Here, is the classical Jacobian of the error metric with respect to . Similarly, the Riemannian gradient at node can be calculated as
(19) 
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 ontangentplane 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 projectionlike 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 RPGOpt based on publicly available planar pose graph datasets [2, 12] and compare it with three stateoftheart pose graph optimization frameworks: g2o [1], GTSAM [14] and iSAM [16]. We choose to use LevenbergMarquardt algorithm for iSAM and the GaussNewton method for GTSAM, respectively. In g2o, the optimization is performed using the GaussNewton 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
(20)  
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 ontangentplane 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.
Iva 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
(21)  
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 RPGOpt, reaches comparably accurate results as state of the art.
Dataset  *  RPGOpt10  g2o10  GTSAM  iSAM 

CSAIL  
FR079  
FRH  
M3500  
MITb  
City10K  
M10K  
* The optimizations are done for both identity (denoted as ) and real information matrices from datasets (denoted as ). Here, g2o and RPGOpt iteration steps are fixed to be , whereas GTSAM and iSAM uses its default stopping criterion.
M3500a+
M3500b+
M3500c+
City10000a
City10000b
City10000c
odometry
iSAM
g2o
RPGOpt
RPGOpt  chord + RPGOpt  chord + g2o  chord + GTSAM  

Dataset*  
M3500a+  
M3500b+  
M3500c+  
M3500d+  
City10000a  
City10000b  
City10000c  
City10000d 
* 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.
IvB 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 GaussNewton 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 relaxationbased 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 RPGOpt to show the convergence robustness under large uncertainties of the proposed method .
IvC 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. 2a). Fig. 2b 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 geometryaware cost function formulation and the Riemannian GaussNewton 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 GaussNewton method is applied using the exponential retraction. Both the onmanifold MLE formulation and optimization is geometryaware, which inherently considers the underlying nonlinear structure of the SE(2) group. Evaluations using realworld 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 stateoftheart frameworks.
Based on the presented work, there is still much potential to exploit. The proposed Riemannian approach can be extended to the general dual quaternionbased 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.
References
 [1] G. Grisetti, R. Kummerle, C. Stachniss, and W. Burgard, “A Tutorial on Graphbased 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, “GeometryDriven 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 Distributionbased 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 Handson 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 Persensor 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, “Projectionlike 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, “GeometryDriven 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.
Comments
There are no comments yet.