From angular manifolds to the integer lattice: guaranteed orientation estimation with application to pose graph optimization


L. Carlone and A. Censi, "From angular manifolds to the integer lattice: guaranteed orientation estimation with application to pose graph optimization", IEEE Trans. on Robotics, (30)2: 475:492, 2014.




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 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.


Supplementary material

  • Source code:
    • Matlab code (.zip)
    • Instructions:
      •   Download and unzip the code
      •   Run the file "MAIN.m" (folder MOLE2D - matlab code/MOLE2D) in Matlab (tested in Matlab R2011a)
      •   Test different datasets and different choices of cycle basis matrix by changing the options at the beginning of MAIN.m
  • Datasets:
    • Input files of each dataset can be downloaded by clicking on the following images (TORO format). A description of the input data format and more datasets can be found here.




INTEL eg2o



MIT eg2o



M3500 eg2o



M3500a eg2o



M3500b eg2o



M3500c eg2o