  Representing the environment is a fundamental task in enabling robots to act
autonomously in unknown environments. In this work, we present confidence-rich
mapping (CRM), a new algorithm for spatial grid-based mapping of the 3D
environment. CRM augments the occupancy level at each voxel by its confidence
value. By explicitly storing and evolving confidence values using the CRM
filter, CRM extends traditional grid mapping in three ways: first, it partially
maintains the probabilistic dependence among voxels. Second, it relaxes the
need for hand-engineering an inverse sensor model and proposes the concept of
sensor cause model that can be derived in a principled manner from the forward
sensor model. Third, and most importantly, it provides consistent confidence
values over the occupancy estimation that can be reliably used in collision
risk evaluation and motion planning. CRM runs online and enables mapping
environments where voxels might be partially occupied. We demonstrate the
performance of the method on various datasets and environments in simulation
and on physical systems. We show in real-world experiments that, in addition to
achieving maps that are more accurate than traditional methods, the proposed
filtering scheme demonstrates a much higher level of consistency between its
error and the reported confidence, hence, enabling a more reliable collision
risk evaluation for motion planning.
