6DOF pose estimation using 3D sensors

Pose estimation is an important capability for mobile agents. A wide variety of solutions have been proposed, but work in the literature has focused primarily on solutions for robots whose mobility is restricted to the ground plane. In this work we present a framework for 6DOF pose estimation. Normally the increased computational cost associated with this higher dimensional space makes pose estimation intractable. The approach presented here addresses the computational issues associated with the higher dimensional problem by decoupling orientation estimation from position estimation. Assuming that orientation can be estimated separately from position allows efficient methods to be used for the (unimodal) orientation estimate, while more sophisticated methods are used for the position estimate. Although similar to Rao-Blackwellization, the approach is essentially reversed. Results on real and simulated datasets and a comparison with a naive 6DOF filter are presented.