Robust and Efficient Volumetric Occupancy Mapping with an Application to Stereo Vision

A map of occupied and free space in a robot's environment is a common prerequisite for navigational tasks. Although the first methods for occupancy mapping relied on a 2D grid representation, 3D volumetric approaches are becoming increasingly popular. In this paper we present a new volumetric mapping approach that is based on the OctoMap method. We designed this method to be more robust against measurement errors, in particular against high temporally or spatially correlated errors usually received from a stereo vision system. For this purpose, we define a probability measure that a voxel is currently visible. An update of a voxel's occupancy probability then happens with respect to this visibility probability, allowing us to neglect measurements for voxels that are actually unobservable. Finally, we model the depth error of a stereo vision sensor, and take care of this error when performing a map update. By evaluation we show that our method produces maps with far less erroneous artifacts compared to OctoMap. Our maps also require less memory, and due to an optimized update reduction, our method is also faster than OctoMap when processing dense range measurement data.