Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments
Authors: Afonso E. Carvalho, David Portugal, Paulo Peixoto, George Kantor
Organizations: Institute of Systems and Robotics, University of Coimbra, Coimbra, Portugal · Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA
Real-time volumetric mapping for a planning robot is dominated by the cost of integrating and storing information at the finest resolution, yet only a small fraction of any map ever lies on or near a path the planner actually considers. We exploit this with a tiered-resolution mapping mechanism that integrates LiDAR into a single map structure at two resolutions simultaneously: fine inside a corridor around the robot's current plan, and coarse everywhere else. As opposed to traditional navigation pipelines where path planning uses the mapping result and provides no feedback, we propose a feedback loop where planning also informs mapping to dynamically integrate information at different leaf resolutions. Across two real-world teleoperated datasets, replayed and scored offline against an independent full-resolution map, the tiered maps are 13-15x smaller and require 6-8x less integration CPU time than a uniform fine map, while producing paths at a similar cost per meter as the ones generated from fine resolution maps on both scenarios. A uniform coarse map reaches the same size savings but its path quality collapses (+210% cost per meter) on the harder scenario, because it over-discretizes terrain the robot must traverse. Tiering approximates the coarse map's cheapness without its failure mode.
Figures & tables
Fig. 1 : Part of the volumetric map generated with fine resolution near the path (3m corridor), and coarse resolution otherwise. The path is colored magenta, and the voxels are colored by height.
Fig. 2 : The proposed architecture. Dashed lines represent the modules that would be added to perform actuation, and green modules represent the feedback loop described.
Fig. 3 : The CURTmini platform in the two experimental environments.
Fig. 4 : Recorded trajectories and the five goal poses (planning probes) issued per scenario.
Metric
Scen.
Fine
Coarse
Tiered
vs. fine
Nodes
S1
2,744,307
67,351
201,632
− 92.7%
Nodes
S2
2,340,180
57,827
154,834
− 93.4%
Memory (MB)
S1
135.7
3.60
10.29
− 92.4%
Memory (MB)
S2
116.6
3.01
7.86
− 93.3%
Map CPU (%)
S1
219.3
22.9
35.4
− 83.8%
Map CPU (%)
S2
288.0
20.1
35.8
− 87.6%
TABLE I : Map cost: tiered vs. uniform maps. All differences vs. uniform_fine have 95% CIs excluding zero.
Fig. 5 : Tiered tracks close to uniform coarse in map size while matching uniform fine in path quality on the harder scenario; uniform coarse collapses.
Arm
Scen.
Cost/m
vs. fine
Exact
Optim.
uniform_fine
S1
3.39
–
0.632
1.009
uniform_coarse
S1
3.59
+5.7% ns
0.176
1.211
tiered
S1
4.09
+20.6% ns
0.568
1.211
tiered_hrd1p5
S1
4.10
+20.8% ns
0.536
1.252
tiered_hrd6p0
S1
4.07
+19.9% ns
0.544
1.143
range_r6p0
S1
3.78
+11.3% ns
0.352
1.228
TABLE II : Route quality (25/25 coverage, every arm). The ”vs. fine” column is the paired 95% CI difference.
Hierarchical, multi-resolution volumetric mapping approaches are widely used to represent large and complex environments as they can efficiently capture their occupancy and connectivity information. Yet widely used path planning methods such as sampling and trajectory optimization do not exploit this explicit connectivity information, and search-based methods such as A* suffer from scalability issues in large-scale high-resolution maps. In many applications, Euclidean shortest paths form the underpinning of the navigation system. For such applications, any-angle planning methods, which find optimal paths by connecting corners of obstacles with straight-line segments, provide a simple and efficient solution. In this paper, we present a method that has the optimality and completeness properties of any-angle planners while overcoming computational tractability issues common to search-based methods by exploiting multi-resolution representations. Extensive experiments on real and synthetic environments demonstrate the proposed approach's solution quality and speed, outperforming even sampling-based methods. The framework is open-sourced to allow the robotics and planning community to build on our research.
Victor Reijgwart, Cesar Cadena, Roland Siegwart +1
Mapping is essential in robotics and autonomous systems because it provides the spatial foundation for path planning. Efficient mapping enables planning algorithms to generate reliable paths while ensuring safety and adapting in real time to complex environments. Fixed-resolution mapping methods often produce overly conservative obstacle representations that lead to suboptimal paths or planning failures in cluttered scenes. To address this issue, we introduce Parallel OctoMapping (POMP), an efficient OctoMap-based mapping technique that maximizes available free space and supports multi-threaded computation. To the best of our knowledge, POMP is the first method that, at a fixed occupancy-grid resolution, refines the representation of free space while preserving map fidelity and compatibility with existing search-based planners. It can therefore be integrated into existing planning pipelines, yielding higher pathfinding success rates and shorter path lengths, especially in cluttered environments, while substantially improving computational efficiency.
Yihui Mao, Tian Tan, Xuehui Shen +2
Department of Mechanical and Aerospace Engineering, University of Florida, Gainesville, FL 32611 USA · Department of Electrical and Systems Engineering, University of Pennsylvania, Philadelphia, PA 19104 USA
Real-time 3D mapping is fundamental for autonomous robotic navigation, with Euclidean Signed Distance Fields (ESDFs) serving as the standard representation for online motion planning. While recent advancements in non- projective distance fields yield highly accurate maps, their computational overhead remains a severe bottleneck. Conventional integrators redundantly re-fuse millions of depth pixels every frame, even long after the corresponding voxels have converged, wasting significant computational resources in environments dominated by large planar surfaces. In this paper, we present SHIFT (Surface-aware High-speed Integration For TSDFs), an efficient mapping framework designed to reduce this per-frame update cost. By exploiting structural redundancy directly from 3D depth geometry, SHIFT compresses flat local regions into weighted super-rays and freezes flat-voxel gradients. A compact ESDF voxel layout further reduces the memory footprint of the remaining wavefront. Extensive evaluations across various RGB-D and LiDAR sequences show that SHIFT cuts TSDF cost by 1.42 to 4.07 times, while holding mesh error within millimeters, and reduces ESDF-layer memory by up to 28%
Ayaan Choudhury, Lokender Tiwari
Indian Institute of Technology Jodhpur · BotLab Dynamics