Certifiably Optimal Rotation and Pose Estimation Based on the Cayley Map

TL;DR

This work addresses the challenge of obtaining globally optimal solutions for rotation and pose estimation under anisotropic noise modeled in Lie algebra coordinates. It introduces Cayley-map based noise models and formulates estimation problems as QCQPs that are relaxed to SDPs; strong duality provides a posteriori global optimality certificates for practical noise levels. The authors develop convex relaxations with carefully chosen redundant constraints for rotation averaging, pose averaging, discrete-time trajectories, and continuous-time trajectories (with a GP prior), and demonstrate that the SDP solutions are often rank-1, yielding globally optimal estimates. The approach enables certifiable global solutions using standard SDP solvers, offering a principled alternative to local optimization, with potential impact on robotics and computer vision tasks involving SE(3) state estimation. Future work discusses scaling these methods to larger problems, certifying local solvers, and exploring scalable global optimization strategies and covariance-aware refinements of Cayley-based distributions.

Abstract

We present novel, convex relaxations for rotation and pose estimation problems that can a posteriori guarantee global optimality for practical measurement noise levels. Some such relaxations exist in the literature for specific problem setups that assume the matrix von Mises-Fisher distribution (a.k.a., matrix Langevin distribution or chordal distance)for isotropic rotational uncertainty. However, another common way to represent uncertainty for rotations and poses is to define anisotropic noise in the associated Lie algebra. Starting from a noise model based on the Cayley map, we define our estimation problems, convert them to Quadratically Constrained Quadratic Programs (QCQPs), then relax them to Semidefinite Programs (SDPs), which can be solved using standard interior-point optimization methods; global optimality follows from Lagrangian strong duality. We first show how to carry out basic rotation and pose averaging. We then turn to the more complex problem of trajectory estimation, which involves many pose variables with both individual and inter-pose measurements (or motion priors). Our contribution is to formulate SDP relaxations for all these problems based on the Cayley map (including the identification of redundant constraints) and to show them working in practical settings. We hope our results can add to the catalogue of useful estimation problems whose solutions can be a posteriori guaranteed to be globally optimal.

Figures (13)

• Figure 1: Comparison of uncertainty on rotation angle, $\varphi$, for the exponential and Cayley maps, where the variances have been approximately matched (see Appendix \ref{['sec:distributions']} for further discussion of how this was done). (left) Standard deviation of rotational uncertainty is $\sigma = 0.2$ [rad]. (right) $\sigma = 0.5$ [rad]. The match is good in both cases with more divergence as rotational uncertainty increases.
• Figure 2: Rotation Averaging: A quantitative evaluation of the tightness of the rotation averaging problem with increasing measurement noise level, $\sigma$. At each noise level, we conducted $1000$ trials of averaging $10$ noisy rotations. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log SVR of at least $5$ to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplotsof the log SVR of the SDP solution show that the global solution remains highly rank 1 over a wide range of measurement noise values.
• Figure 3: Rotation Averaging: An example of noisy rotation averaging where the randomly initialized local solver (dotted) becomes trapped in a poor local minimum while the global solver (dashed) finds the correct global solution, which is closer to the groundtruth rotation (solid).
• Figure 4: Pose Averaging: Four examples of noisy pose averaging where the randomly initialized local solver (dotted) becomes trapped in a poor local minimum while the global solver (dashed) finds the correct solution, which is closer to the groundtruth pose (solid). The noisy pose measurements being averaged are shown in grey.
• Figure 5: Pose Averaging: A quantitative evaluation of the tightness of the pose averaging problem with increasing measurement noise level, $\sigma$. At each noise, we conducted $1000$ trials of averaging $10$ noisy poses. (left) We see that the local solver (randomly initialized) finds the global minimum with decreasing frequency (green) as the measurement noise is increased, while the SDP solver (blue) successfully produces rank-1 solutions (we consider log SVR of at least $5$ to be rank 1) to much higher noise levels. For completeness, we also show how frequently the local solver converges to any minimum (red). (right) Boxplots of the log SVR of the SDP solution show that the global solution remains highly rank 1 over a wide range of measurement noise values.