Abstract: This paper proposes a new method for rigid body pose estimation based on spectrahedral representations of the tautological orbitopes of $SE(2)$ and $SE(3)$. The approach can use dense point cloud data from stereo vision or an RGB-D sensor (such as the Microsoft Kinect), as well as visual appearance data. The method is a convex relaxation of the classical pose estimation problem, and is based on explicit linear matrix inequality (LMI) representations for the convex hulls of $SE(2)$ and $SE(3)$. Given these representations, the relaxed pose estimation problem can be framed as a robust least squares problem with the optimization variable constrained to these convex sets. Although this formulation is a relaxation of the original problem, numerical experiments indicate that it is indeed exact - i.e. its solution is a member of $SE(2)$ or $SE(3)$ - in many interesting settings. We additionally show that this method is guaranteed to be exact for a large class of pose estimation problems.
Introduction
In robotics and computer vision, pose estimation—determining the orientation and position of an object in space—is a fundamental challenge. Traditional approaches, such as Iterated Closest Point (ICP) or Levenberg-Marquardt (LM), rely on local search techniques. While effective in controlled environments, these methods are prone to getting trapped in local minima and are sensitive to outliers and sensor noise. This paper introduces a novel approach using convex relaxations of the Special Euclidean groups $SE(2)$ and $SE(3)$ to provide global optimality and enhanced robustness.
Methodology
The core innovation lies in leveraging convex algebraic geometry to transform the non-convex pose estimation problem into a convex Semidefinite Program (SDP).
Spectrahedral Representation and Orbitopes
Instead of searching over the non-convex surface of the transformation group, the authors optimize over its convex hull, known as an orbitope. They utilize Linear Matrix Inequalities (LMIs) to represent the convex hulls of $SO(2)$ and $SO(3)$. For $SO(3)$, a $4 \times 4$ matrix representation derived from quaternions is employed. By relaxing the constraints to these convex sets, the pose estimation becomes an SDP, which can be solved efficiently using off-the-shelf convex solvers.
Robustness via $\ell_1$ Regularization
To address outliers, the formulation incorporates an $\ell_1$ penalty term, a technique borrowed from compressed sensing. By splitting the error into Gaussian noise and sparse outliers, the optimization can effectively "ignore" data points that do not conform to the rigid body model. This sparsity-inducing approach eliminates the need for separate outlier rejection steps like RANSAC.
Experimental Evaluation
The proposed method was evaluated using the Stanford Bunny model (944 points) under synthetic corruption, including Gaussian noise and artificial outliers.
Performance under Noise
The Orbitope method demonstrates superior accuracy compared to PCA and Levenberg-Marquardt as sensor noise increases. While all methods perform similarly at very low noise levels, the convex relaxation maintains a significantly lower Sum of Squared Error (SSE) in high-noise regimes.
Robustness to Outliers
A critical test involved artificially displacing the bunny's ears to simulate mis-correspondence. The robust Orbitope method successfully rejected these outliers, achieving an error residual of 0.78. In contrast, PCA was significantly pulled off-track, resulting in an error of 25.31.
Discussion and Conclusion
A significant finding is that the convex relaxation is often exact. For a large class of problems (specifically when the projection matrix is orthogonal), the optimal solution to the relaxed problem is guaranteed to be a valid member of the original transformation group.
While the Orbitope method is computationally more intensive than local search (e.g., 130ms vs 25ms on a standard laptop), it offers global optimality and robustness that local methods lack. Future work focuses on accelerating the solver using techniques like ADMM and formalizing exactness proofs for the robust $\ell_1$ case.
Summary updated