Multi-Resolution 3D Mapping with Explicit Free Space Representation for Fast and Accurate Mobile Robot Motion Planning

TL;DR

The paper addresses the need for real-time, high-resolution 3D maps that support fast, collision-aware planning for mobile robots. It proposes a multi-resolution occupancy mapping framework based on a two-tier octree with log-odds occupancy, plus a map-to-camera allocation strategy that adaptively allocates resolution where needed. Key contributions include a conservative, depth-span–driven allocation and a weighted-mean log-odds fusion scheme, a scalable integration scale with hysteresis, and fast multi-resolution ray-casting and meshing for planning and tracking. The approach achieves competitive surface reconstruction accuracy while enabling real-time planning at centimetre-scale resolutions, without the heavy ESDF construction required by many existing methods, thereby facilitating tighter integration between mapping and planning in constrained robotic platforms.

Abstract

With the aim of bridging the gap between high quality reconstruction and mobile robot motion planning, we propose an efficient system that leverages the concept of adaptive-resolution volumetric mapping, which naturally integrates with the hierarchical decomposition of space in an octree data structure. Instead of a Truncated Signed Distance Function (TSDF), we adopt mapping of occupancy probabilities in log-odds representation, which allows to represent both surfaces, as well as the entire free, i.e. observed space, as opposed to unobserved space. We introduce a method for choosing resolution -- on the fly -- in real-time by means of a multi-scale max-min pooling of the input depth image. The notion of explicit free space mapping paired with the spatial hierarchy in the data structure, as well as map resolution, allows for collision queries, as needed for robot motion planning, at unprecedented speed. We quantitatively evaluate mapping accuracy, memory, runtime performance, and planning performance showing improvements over the state of the art, particularly in cases requiring high resolution maps.

Figures (11)

• Figure 1: The main goal of our system is to provide a navigable 3D occupancy map with a defined notion of observed free space in small to large scale scenarios, using adaptive resolution to constrain memory consumption. The figure shows the output of our mapping pipeline for the Cow and Lady RGB-D dataset Oleynikova2017 alongside a planned trajectory (red). The chosen voxel resolutions for free space encoding are shown for a map slice.
• Figure 2: Overview of our system: The different stages of the mapping pipeline are shown in grey, while the blue boxes show the steps in the allocation and updating procedure. The flow of information is illustrated by the arrows. The allocation and updating stages modify the map, while the rendering and planning stages utilise the map information.
• Figure 3: Overview of the data structure for a hypothetical 2D case. Data is represented in a two tier octree having voxel blocks in the lower levels. Depending on the distance based integration scale $s_c$ voxel blocks containing data up to different resolutions are allocated, resulting in important memory and time savings for cases where higher resolution is not needed. Note that in the given example the integration scale $s_c$ for blocks only containing free and unknown space has a lower bound of $s_c > 1$ (Section \ref{['section:Fusion']}).
• Figure 4: (a) Inverse sensor model for two measurements (1m, 5m) expressed as a function of the difference $d_r$ from a query point to the measured surface along the ray. Log-odd values in front of the surface are clipped at $l_{\min}$ reached at $\mu = 3 \sigma$ and grow linearly up to half the surface thickness $\tau(z)$. (b) Distance-dependent growth of two sensor uncertainty models (linear and quadratic) within the minimum and maximum sensor range $z_{\text{np}}$ and $z_{\text{fp}}$ and sensor uncertainty $\sigma_{\min}$ and $\sigma_{\max}$.
• Figure 5: Comparison of free space volume allocation in OFusion Vespa2018 (left) and our new adaptive-resolution strategy (right) in an environment with a wall and vertical pole. Blue corresponds to free space and white lines indicate allocated voxels on a map slice. While OFusion ignores occupancy variations inside a voxel's volume, leading to erroneous assumptions of free space behind the pole, our method naturally integrates the volume at the appropriate scale, allocating obstacles and boundaries to unknown space at fine resolution, while representing free space at the coarsest possible level.