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

Pose graph optimization from relative measurements is challenging because of the angular component of the poses: the variables live on a manifold product with nontrivial topology and the likelihood function is nonconvex and has many local minima. Because of these issues, iterative solvers are not robust to large amounts of noise. This paper describes a global estimation method, called multi-hypothesis orientation-from-lattice estimation in 2-D ( ${\ssr MOLE2D}$), for the estimation of the nodes’ orientation in a pose graph. We demonstrate that the original nonlinear optimization problem on the manifold product is equivalent to an unconstrained quadratic optimization problem on the integer lattice. Exploiting this insight, we show that, in general, the maximum likelihood estimate alone cannot be considered a reliable estimator. Therefore, ${\ssr MOLE2D}$ returns a set of point estimates, for which we can derive precise probabilistic guarantees. Experiments show that the method is able to tolerate extreme amounts of noise, far above all noise levels of sensors used in applications. Using ${\ssr MOLE2D}$’s output to bootstrap the initial guess of iterative pose graph optimization methods improves their robustness and makes them avoid local minima even for high levels of noise.