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

Abstract

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., σ = 30 • on each measurement) that are above all noise levels of sensors commonly used in mapping. For most range-finder-based scenarios, the multihypothesis 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.

Bingham Distribution-Based Linear Filter for Online Pose Estimation

“…For example, Langevin distributions have been used for pose estimation [4,24]. Henebeck et al have recently developed a Bingham distribution-based recursive filtering approach for orientation estimation [9].…”

A Distributed Optimization Framework for Localization and Formation Control: Applications to Vision-Based Measurements

“…This approach was originated by Lu and Milios [49], and it has seen numerous contributions, with [47,42,36] being the most popular solutions. Other works build on these by considering on-line updates [38,20], multi-scale solvers [37,29], large problem sizes [46], or robustness [57,10]. As in the computer vision community, the main theme in these works is to use local optimization techniques while exploiting the specific structure of the problem to speed-up computations.…”

A convex relaxation for approximate global optimization in simultaneous localization and mapping

“…Problem (18) is thus a convex program. (2), where the set conv SO(2) is characterized explicitly by (20). The construction for SO(3) using (19) and (21) is analogous.…”

“…The convex hull of SO(n) is a spectrahedron for all n ∈ N. For the special cases n = 2 and n = 3, the corresponding spectrahedral descriptions are given explicitly by equations (20) and (21):…”