Convex relaxations of SE(2) and SE(3) for visual pose estimation

Matanya B. Horowitz, Nikolai Matni, Joel W. Burdick

In the world of robotics and computer vision, "pose estimation"—figuring out exactly how an object is oriented and positioned in space—is a bread-and-butter problem. Whether it's a robot arm trying to grasp a mug or a self-driving car localizing itself, getting the pose right is critical. However, the math behind it is notoriously "bumpy," filled with local minima that can trap standard algorithms.

A paper from Caltech researchers Matanya B. Horowitz, Nikolai Matni, and Joel W. Burdick proposes a clever way out: Convex Relaxations of $SE(2)$ and $SE(3)$. By rethinking the geometry of transformations, they turn a difficult, non-convex search into a reliable convex optimization problem.

TL;DR

The authors transform the non-convex problem of rigid body pose estimation into a convex Semidefinite Program (SDP) by using the "convex hull" of the transformation groups $SE(2)$ and $SE(3)$. This approach provides global optimality guarantees and, when combined with $\ell_1$ regularization, becomes remarkably robust to sensor noise and outliers.

What Problem Does This Paper Address?

Traditional pose estimation usually involves matching features from a known model to observed sensor data (like a point cloud from a Kinect). Mathematically, this means finding a transformation in the Special Euclidean group, $SE(3)$, that minimizes the distance between points.

The catch? $SE(3)$ is a non-convex space. Standard tools like Iterated Closest Point (ICP) or Levenberg-Marquardt are "local search" methods. If you start with a bad initial guess, the algorithm might get stuck in a local minimum—essentially thinking the object is upside down or backwards because that's the best "local" fit it found. Furthermore, outliers (erroneous data points) can easily pull these algorithms off-track.

Methods

The researchers leverage tools from convex algebraic geometry. Instead of forcing the solution to lie strictly on the "surface" of the transformation group (which is non-convex), they allow it to lie anywhere within the convex hull of that group—a shape known as an orbitope.

Key Technical Steps:

  1. Spectrahedral Representation: They use Linear Matrix Inequalities (LMIs) to represent the convex hulls of $SO(2)$ and $SO(3)$ (the rotation parts). For $SO(3)$, this involves a $4 \times 4$ matrix representation derived from quaternions.
  2. Semidefinite Programming (SDP): 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.
  3. Robustness via $\ell_1$ Regularization: To handle outliers, they borrow a trick from compressed sensing. They add an $\ell_1$ penalty term (similar to LASSO) to the objective function. This "sparsity-inducing" term effectively identifies and ignores points that don't fit the model, treating them as explicit error terms ($Z_1$).

Datasets

The paper evaluates the method using a benchmark model:

  • The Stanford Bunny: A standard 3D mesh consisting of 944 points.
  • Synthetic Corruption: The authors generated test data by applying known transformations to the bunny and then corrupting the points with varying levels of Gaussian noise and artificial outliers (e.g., translating the bunny's ears to a different location to simulate "mis-correspondence").

Key Insights and Ideas

  • Exactness is Often Free: One might worry that "relaxing" a problem leads to an "approximate" answer. However, the authors prove that for a large class of problems (specifically when the projection matrix is orthogonal), the optimal solution to the relaxed convex problem is actually a member of the original group. In other words, the relaxation is exact.
  • Global Optimality: Unlike local search methods, convex optimization finds the global minimum. You don't need a "good" initial guess to get the right answer.
  • The Power of Sparsity: By splitting the error into "noise" (Gaussian) and "outliers" (sparse), the $\ell_1$ formulation can "reject" bad data points automatically without needing a separate RANSAC step.

Key Results

  • Superior Accuracy at High Noise: In head-to-head tests against Levenberg-Marquardt and Principal Component Analysis (PCA), the Orbitope method provided significantly better estimates as sensor noise increased.
  • Outlier Rejection: In a test where the bunny's ears were artificially moved (creating outliers), the robust Orbitope method achieved an error residual of 0.78, while PCA exploded to an error of 25.31.
  • Computational Trade-off: The method is slower than local search. On a 1.7GHz Macbook Air, Levenberg-Marquardt took ~25ms, while the Orbitope optimization took ~130ms. While slower, 130ms is still fast enough for many real-time robotics applications.

Limitations and Future Work

  • Speed: While 130ms is respectable, it is an order of magnitude slower than the fastest local methods. The authors suggest that using specialized solvers like ADMM (Alternating Direction Method of Multipliers) could significantly speed this up.
  • Loss of Exactness with Robustness: The mathematical guarantee of "exactness" (the solution being a valid rotation) is technically lost when the $\ell_1$ robust term is added. While numerical experiments show it still usually works, a formal proof for the robust case remains an open challenge.
  • Heuristic Projections: If the solver returns a result slightly outside the valid transformation group, the authors use a Singular Value Decomposition (SVD) based projection to "snap" it back to the nearest valid pose. The performance of this heuristic needs more quantification.

Summary updated

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 as input. 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 indicates 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.
BibTeX
@inproceedings{horowitz2014convex,
  author = {Matanya B. Horowitz and Nikolai Matni and Joel W. Burdick},
  title = {{Convex relaxations of SE(2) and SE(3) for visual pose estimation}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2014},
  url = {https://openreview.net/forum?id=FJF4SyX-DDI},
}

Loading related content...

Figure