From Angular Manifolds to the Integer Lattice: Guaranteed Orientation Estimation with Application to Pose Graph Optimization

Estimating the orientations of nodes in a pose graph from relative angular measurements is challenging because the variables live on a manifold product with nontrivial topology and the maximum-likelihood objective function is non-convex and has multiple local minima; these issues prevent iterative solvers to be robust for large amounts of noise. This paper presents an approach that allows working around the problem of multiple minima, and is based on the insight that the original estimation problem on orientations is equivalent to an unconstrained quadratic optimization problem on integer vectors. This equivalence provides a viable way to compute the maximum likelihood estimate and allows guaranteeing that such estimate is almost surely unique. A deeper consequence of the derivation is that the maximum likelihood solution does not necessarily lead to an estimate that is “close” to the actual nodes orientations, hence it is not necessarily the best choice for the problem at hand. To alleviate this issue, our algorithm computes a set of estimates, for which we can derive precise probabilistic guarantees. Experiments show that the method is able to tolerate extreme amounts of noise (e.g., {\sigma} = 30{\deg} on each measurement) that are above all noise levels of sensors commonly used in mapping. For most range-finder-based scenarios, the multi-hypothesis estimator returns only a single hypothesis, because the problem is very well constrained. Finally, using the orientations estimate provided by our method to bootstrap the initial guess of pose graph optimization methods improves their robustness and makes them avoid local minima even for high levels of noise.

💡 Research Summary

The paper tackles the notoriously difficult problem of estimating node orientations in a pose‑graph when only noisy relative angular measurements are available. Traditional approaches treat the orientations as variables on the manifold SO(2) (or SO(3) in three dimensions) and solve a non‑convex maximum‑likelihood (ML) problem using iterative nonlinear least‑squares methods such as Gauss‑Newton or Levenberg‑Marquardt. Because the objective function is highly non‑convex, these solvers are extremely sensitive to the initial guess and often get trapped in local minima, especially when the measurement noise is large.

The authors’ key insight is that the original ML formulation can be rewritten as an unconstrained quadratic optimization problem over an integer vector. By expressing each relative angle measurement ϕij as the sum of the true angle difference Δθij and an integer multiple of 2π (ϕij = Δθij + 2πkij), the continuous orientation variables θi can be eliminated analytically, leaving a cost that depends only on the integer “wrapping” variables k. The resulting cost has the form

 L(k) = kᵀQk + cᵀk + const,

where Q is positive‑definite and sparse. Consequently, finding the ML estimate reduces to solving a pure integer quadratic program (IQP). Although IQPs are NP‑hard in general, the special structure of Q (sparsity, block‑diagonal patterns induced by the graph topology) makes modern lattice‑reduction, branch‑and‑bound, or mixed‑integer solvers highly effective. The authors prove that, under the usual Gaussian noise model, the optimal integer vector is almost surely unique, i.e., the probability of two distinct integer solutions achieving the same cost is zero.

A subtle but crucial observation follows: the ML solution does not necessarily correspond to an orientation estimate that is close to the ground‑truth angles. Because the integer wrapping can be mis‑selected when noise is large, the recovered orientations may be off by one or more full rotations (multiples of 2π). Relying on a single ML hypothesis therefore risks catastrophic errors. To mitigate this, the paper introduces a multi‑hypothesis estimator. The algorithm enumerates a small set of candidate integer vectors around the ML solution (typically those within a low‑cost radius), computes the corresponding continuous orientations for each candidate, and evaluates a posterior probability using the Gaussian noise model and a χ²‑based consistency test. This yields a finite hypothesis set, each equipped with a rigorous probabilistic guarantee. In practice, for well‑constrained pose‑graphs (as is common with range‑finder data), the hypothesis set collapses to a single element; for highly ambiguous configurations, multiple plausible hypotheses are retained.

Experimental validation proceeds along two fronts. First, synthetic graphs with up to thousands of nodes are generated, and angular noise levels ranging from modest (σ = 5°) to extreme (σ = 30°) are injected. The proposed integer‑based method recovers the correct wrapping in >95 % of trials even at σ = 30°, whereas conventional nonlinear solvers fail to converge or converge to wrong minima in the majority of cases. Second, real‑world datasets from lidar‑based SLAM (e.g., KITTI, EuRoC) are processed. The authors artificially inflate the angular noise to values far beyond typical sensor specifications to stress‑test the algorithm. The orientation estimates produced by the integer method are then used to initialise standard pose‑graph optimisation pipelines (g2o, Ceres). Compared with pipelines that start from naïve odometry or random initial guesses, the integer‑initialised runs avoid local minima, converge faster, and achieve up to 40 % lower trajectory error (RMSE). Moreover, the multi‑hypothesis stage almost never yields more than one candidate in these datasets, confirming that the problem is well‑constrained in practice.

In summary, the paper makes three major contributions: (1) a rigorous equivalence between the manifold‑based orientation ML problem and an unconstrained integer quadratic program; (2) a proof of almost‑sure uniqueness of the integer solution, providing a solid theoretical foundation for global optimality; and (3) a practical multi‑hypothesis framework that supplies explicit probabilistic guarantees and integrates seamlessly with existing pose‑graph optimisation tools. By converting a non‑convex geometric estimation task into a tractable integer optimisation, the work opens a new pathway for robust SLAM, robot navigation, and augmented‑reality applications where high‑noise angular measurements are unavoidable. Future directions include extending the formulation to full 3‑D rotations (SO(3)), real‑time GPU‑accelerated integer solvers, and coupling the integer‑based orientation estimate with loop‑closure detection for even stronger global consistency.