The self-localization capability is a crucial component for Unmanned Ground
Vehicles (UGV) in farming applications. Approaches based solely on visual cues
or on low-cost GPS are easily prone to fail in such scenarios. In this paper,
we present a robust and accurate 3D global pose estimation framework, designed
to take full advantage of heterogeneous sensory data. By modeling the pose
estimation problem as a pose graph optimization, our approach simultaneously
mitigates the cumulative drift introduced by motion estimation systems (wheel
odometry, visual odometry, ...), and the noise introduced by raw GPS readings.
Along with a suitable motion model, our system also integrates two additional
types of constraints: (i) a Digital Elevation Model and (ii) a Markov Random
Field assumption. We demonstrate how using these additional cues substantially
reduces the error along the altitude axis and, moreover, how this benefit
spreads to the other components of the state. We report exhaustive experiments
combining several sensor setups, showing accuracy improvements ranging from 37%
to 76% with respect to the exclusive use of a GPS sensor. We show that our
approach provides accurate results even if the GPS unexpectedly changes
positioning mode. The code of our system along with the acquired datasets are
released with this paper.Comment: Accepted for publication in IEEE Robotics and Automation Letters,
201
