NanoMap: Fast, Uncertainty-Aware Proximity Queries with Lazy Search over\n Local 3D Data

Abstract

We would like robots to be able to safely navigate at high speed, efficiently\nuse local 3D information, and robustly plan motions that consider pose\nuncertainty of measurements in a local map structure. This is hard to do with\npreviously existing mapping approaches, like occupancy grids, that are focused\non incrementally fusing 3D data into a common world frame. In particular, both\ntheir fragile sensitivity to state estimation errors and computational cost can\nbe limiting. We develop an alternative framework, NanoMap, which alleviates the\nneed for global map fusion and enables a motion planner to efficiently query\npose-uncertainty-aware local 3D geometric information. The key idea of NanoMap\nis to store a history of noisy relative pose transforms and search over a\ncorresponding set of depth sensor measurements for the minimum-uncertainty view\nof a queried point in space. This approach affords a variety of capabilities\nnot offered by traditional mapping techniques: (a) the pose uncertainty\nassociated with 3D data can be incorporated in motion planning, (b) poses can\nbe updated (i.e., from loop closures) with minimal computational effort, and\n(c) 3D data can be fused lazily for the purpose of planning. We provide an\nopen-source implementation of NanoMap, and analyze its capabilities and\ncomputational efficiency in simulation experiments. Finally, we demonstrate in\nhardware its effectiveness for fast 3D obstacle avoidance onboard a quadrotor\nflying up to 10 m/s.\n