Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments

Abstract

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–15\times smaller and require 6–8\times 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.

I Introduction

Volumetric occupancy mapping is a core building block of autonomous navigation, and its real-time cost is dominated by the finest resolution at which measurements are integrated and stored [5, 8, 10]. A system that uses centimeter-scale fidelity everywhere also incurs it’s cost everywhere, even though most of a large environment is never on, or even close to, the area the robot will actually navigate on.

This paper turns that observation into a mechanism: allocate resolution by plan-relevance, finely mapping near the plan and coarsely everywhere else, as shown in Fig. 1, according to the diagram presented in Fig. 2. The natural objection – why not map everything coarse? – is exactly the discriminator our experiments are built around: a uniform coarse map quantizes the terrain the robot must traverse, degrading and over-optimizing the paths the planner reasons about. Tiering is only worth defending if it avoids that failure while approaching the coarse map’s savings, and on the harder of our two real-world scenarios, that is precisely the trade it achieves.

This paper contributes:

• An online, plan-conditioned resolution-allocation mechanism and a resolution router that split each LiDAR sweep into a fine and a coarse stream by distance from the robot center or the live planned-path corridor and integrates both into a single hashed-wavelet octree via two simultaneous integrators;
• An evaluation methodology isolating resolution allocation as the sole independent variable across six configurations, scored offline under a shared, independent reference map;
• A secondary ablation against a plan-free, budget-matched sensor-range baseline.

Section II situates this against prior mapping work; Section III describes the mechanism; Section IV presents the evaluation’s results and discussion, and Section V presents the conclusion and future work.

II Background and Related Work

Octree-based mapping frameworks such as OctoMap [5], Voxblox [8], and UFOMap [2] allocate resolution uniformly or as a fixed function of the data structure, rather than in response to the task. Wavemap [10], which our system uses as the mapping framework, generalizes this through wavelet compression on a hashed-wavelet octree supporting multiple simultaneous resolutions in one tree and providing access to multiple integrators, which have been used in the original publication to integrate information from different sensors, such as depth cameras and LiDARs. Closest to our work, Funk et al. [3] choose resolution per frame via max-min pooling of the depth image, coupling resolution to local sensor geometry rather than the planner’s current intent, and adaptive-resolution mapping has been explored for energy-constrained aerial navigation along regions of high traversal probability [7]; neither conditions the split on a planner’s live output nor scores path quality against an independent oracle. Bandwidth-constrained map compression for multi-robot communication [9] shares our principle – not all of a map is equally worth preserving at full fidelity – for a different resource. We plan in a continuous traversability field with sampling-based, asymptotically optimal planners from the RRT*/BIT* family [6, 4], as also demonstrated for large-scale outdoor ground-robot navigation using Wavemap-based volumetric mapping [1] – this choice is orthogonal to our contribution, which only needs the planner to expose its current path.

III Methodology

The system under test, SPAM (Simultaneous Planning And Mapping), presented in Fig. 2, runs a volumetric mapper and a sampling-based planner concurrently in ROS. Mapping is performed with a custom fork of Wavemap: a single hashed-wavelet octree with a base minimum voxel width of 0.10\text{\,}\mathrm{m}, written by one or more integrators, each capped at a configurable maximum update resolution that it can never refine past. Planning uses a hybrid approach of RRT* and BIT*. RRT* is used to generate paths when the final goal is not in known terrain, returning the best possible approximate path in the goal’s direction; BIT* is used when the terrain where the goal sits becomes mapped, to generate an exact solution, considering that it converges faster to a better solution than RRT*.

The map tiering mechanism is a ROS node that subscribes to the raw LiDAR cloud and the planner’s current path and, on every sweep: (1) computes each point’s distance from the corridor swept out by the current plan; (2) splits the sweep into a fine cloud within a configurable high-resolution distance (hrd) of the corridor and a coarse cloud for everything else; (3) integrates both into the same octree via two simultaneous integrators, capped at 0.10\text{\,}\mathrm{m} and 0.40\text{\,}\mathrm{m}, respectively. We call this split_mode: path, the system under test; a split_mode: sensor_range variant replaces distance-from-plan with distance-from-sensor as a plan-free baseline. Resolution allocation is the only variable that differs across configurations: every arm shares the same 0.10\text{\,}\mathrm{m} tree, Ouster projection model (128\times 1024), continuous_beam measurement model, and planner, so any measured difference is attributable to how resolution was allocated.

The planner and offline scorer share one 2.5D traversability objective, cost per unit length c=1\cdot c_{\text{length}}+4\cdot c_{\text{slope}}+2\cdot c_{\text{roughness}}, with z pinned to ground height. Preliminary testing regarding the traversability weighted function showed that, for fixed endpoints, a pure-length route can cost up to \sim63 terrain-units versus \sim29 for the full objective on a route only \sim6% longer.

IV Experimental Validation

IV-A Setup

We use two real-world LiDAR datasets both collected using an Ouster OS1-128, 128\times 1024@10 Hz, recorded by teleoperating the CURTmini ground robot, shown in Fig. 3 through natural terrain: Scenario 1 is a sparse olive-tree orchard, and Scenario 2 is a denser wooded terrain with downed trees and branches). Scenario 2 is the harder of the two and is where routes actually cross coarsened terrain; Scenario 1’s short queries cross little of it, making it a near-control. Fig. 4 shows both trajectories with the five goal poses used per scenario.

We evaluate six experimental arms by replaying these datasets and running localization, mapping and path planning in real time, isolating resolution allocation as the only variable: uniform_fine (0.10\text{\,}\mathrm{m} everywhere, best-quality reference) and uniform_coarse (0.40\text{\,}\mathrm{m} everywhere, cheapest mapping); tiered (hrd = 3.0\text{\,}\mathrm{m}, the system under test) plus tiered_hrd1p5 and tiered_hrd6p0 to isolate the effect of the corridor width; and a plan-free range_r{6,5}p0 baseline (fine near sensor instead of plan), radius budget-matched to tiered’s node count on each scenario. The laptop used for these experiments is an Asus ROG Strix Scar G16, equipped with an Intel Core Ultra 9 275HX processor and 64GB of RAM.

Each run replays a recorded teleoperation bag rather than driving live, so the robot’s pose sequence is identical across every run and any inter-run difference is attributable to the map, not to divergent trajectories (odometry from iG-LIO). Five goals per scenario (Fig. 4) are issued at fixed sim-clock timestamps at poses the robot actually occupied and act as planning probes, not navigation waypoints. Six configurations \times 5 repeats \times 2 scenarios gives 60 runs, all completed, zero failures.

Route quality is scored offline under a seventh, independent control map per scenario (uniform-fine, driven through with no goal ever issued, so it cannot be biased by any arm’s own planning). We then obtain path’s planned on each configuration’s own map and re-score them under the control; re-scoring the same paths under the arm’s own map, with the same objective, yields a confidence cost, giving an optimism ratio c_{\text{control}}/c_{\text{confidence}} – how much an arm’s own map flattered itself. A same-map control gives optimism \approx 1.000. We report map cost (octree nodes/memory, mapping CPU, process RSS) and route quality over the matched set (cost per meter of progress, exact-solve fraction, optimism), with 95% confidence intervals (CI) across 5 repeats against uniform_fine (ns: CI spans zero; sig: excludes zero).

IV-B Results and Discussion

Table I and Fig. 5(a) report map cost. Tiering cuts octree nodes by 92.7% (13.6\times) on Scenario 1 and 93.4% (15.1\times) on Scenario 2, with matching memory reductions. Mapping CPU (summed across the mapper and, for tiered, also the router, so its overhead is taken into account) drops 83.8%/87.6%, i.e., 6–8\times less CPU usage during integration. Process RSS drops less (38–44%) since a fixed ROS/odometry baseline dominates it.

Table II and Fig. 5(b) report route quality (25/25 coverage, every configuration). On Scenario 2, the discriminator, uniform coarse costs 209.8% more per meter and solves only 40% of queries exactly, while tiered, at essentially the same \sim93% size saving, matches fine (+3.1%, ns) with exact-solve reasonably close (0.576 vs. 0.736) – i.e., the coarse map’s size close to the fine map’s quality. On Scenario 1, coarsening has little effect even for uniform coarse (+5.7%, ns), so tiered’s own +20.6% (also ns) point estimate, whose CI spans zero at n=5 is not considered a regression, meaning that tiering is most likely to help when a route must actually traverse coarse terrain, which Scenario 1 mostly avoids and Scenario 2 forces. Optimism corroborates this, since uniform coarse flatters itself (1.15 to 1.21, hiding cost it would actually pay), while tiered stays near 1.0 (1.013 on Scenario 2).

Sweeping hrd shows the trade is genuine, as an hrd of 1.5 m gives a smaller map (149,977 vs. 201,632 nodes on Scenario 1) but a significant Scenario 2 regression (+30.3%), while 6.0 m gives a larger map (299,215 nodes) and the closest-to-fine Scenario 2 quality (-4.9%, ns); 3.0\text{\,}\mathrm{m} sits at a sensible middle, \sim93% saving at fine parity. Budget-matched to tiered’s node count, the plan-free range baseline is competitive on cost on both datasets (Scenario 1 +11.3%, Scenario 2 +4.7%, both ns) and exceeds tiered on Scenario 2 exact-solve (0.864 vs. 0.576), though tiered leads on Scenario 1 exact-solve (0.568 vs. 0.352). These results support the claim of the advantage of tiered approaches over uniform strategies, with the range comparison a secondary ablation on whether plan feedback specifically is doing the work in these scenarios. In this case, it is not performing significantly different, but we expect this to change when testing in a closed-loop setup with longer paths, which we mention as future work.

V Conclusion and Future Work

Tiered-resolution mapping exploits the assumption that, especially when using asymptotically optimal path planners, the area surrounding the proposed plan at each iteration is likely to contain a lower cost path, should one exist. Furthermore, refining the area around the proposed plan can also help to refine it in the next iterations against real terrain topography that was filtered away by the initial coarse resolution. Integrating LiDAR fine inside the planned-path corridor and coarse elsewhere achieves 13–15\times smaller maps and 6–8\times less integration CPU than uniform fine, at a similar path quality to that achieved using fine quality on both real-world scenarios, while a uniform coarse map of the same size collapses (+210%) on the scenario whose routes actually cross coarsened terrain, actively misleading the planner about route cost by our optimism metric. Priority next steps are closed-loop validation, even more challenging outdoor scenarios and a higher amount of experimental runs to tighten Scenario 1’s confidence interval, and explore a hybrid plan/sensor-range split to capture immediate terrain topography as well as farther-away features in the vicinity of the planned path.

Acknowledgments

This work was supported by the Fundação para a Ciência e a Tecnologia (FCT) under the Affiliated Ph.D. CMU Portugal Program, ref. PRT/BD/153920/2022. Support for this research was also provided by FCT through the ISR’s Multiannual Funding for R&D Units (UIDB/00048/2025, DOI: https://doi.org/10.54499/UID/00048/2025), and by the RUGGED Project (2023.16314.ICDT, DOI: https://doi.org/10.54499/2023.16314.ICDT).

References

1. [1] A. E. Carvalho, D. Portugal, and P. Peixoto (2026) Towards efficient large-scale path planning for outdoor ground robots. In 2026 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), pp. 85–90.
2. [2] D. Duberg and P. Jensfelt (2020) UFOMap: an efficient probabilistic 3D mapping framework that embraces the unknown. IEEE Robotics and Automation Letters 5 (4), pp. 6411–6418.
3. [3] N. Funk, J. Tarrio, S. Papatheodorou, M. Popović, P. F. Alcantarilla, and S. Leutenegger (2021) Multi-resolution 3D mapping with explicit free space representation for fast and accurate mobile robot motion planning. IEEE Robotics and Automation Letters 6 (2), pp. 3553–3560.
4. [4] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot (2015) Batch informed trees (BIT*): sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs. In IEEE International Conference on Robotics and Automation (ICRA), pp. 3067–3074.
5. [5] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard (2013) OctoMap: an efficient probabilistic 3D mapping framework based on octrees. Autonomous Robots 34 (3), pp. 189–206.
6. [6] S. Karaman and E. Frazzoli (2011) Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research 30 (7), pp. 846–894.
7. [7] T. Liu, H. Liu, B. Li, L. Chen, and K. Huang (2025) EANS: reducing energy consumption for UAV with an environmental adaptive navigation strategy. arXiv preprint arXiv:2506.20485.
8. [8] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto (2017) Voxblox: incremental 3D Euclidean signed distance fields for on-board MAV planning. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1366–1373.
9. [9] E. Psomiadis, D. Maity, and P. Tsiotras (2023) Communication-aware map compression for online path-planning. arXiv preprint arXiv:2309.13451.
10. [10] V. Reijgwart, C. Cadena, R. Siegwart, and L. Ott (2023) Efficient volumetric mapping of multi-scale environments using wavelet-based compression. In Proceedings of Robotics: Science and Systems, Daegu, Republic of Korea.