UFOMap: Efficient 3D Mapping for Robotics

• UFOMap is a hierarchical probabilistic 3D mapping framework that explicitly models occupied, free, and unknown states for enhanced spatial reasoning.
• It employs an adaptive octree structure with memory optimizations, achieving up to 45% savings and high insertion rates for dense mapping.
• It integrates semantic label and color fusion to support real-time volumetric mapping and conversion to 2D maps for diverse robotics pipelines.

UFOMap is an efficient hierarchical probabilistic 3D mapping framework designed to represent and process both geometric and semantic information for robotics applications. Extending OctoMap, UFOMap makes the unknown state an explicit part of the map's logic, leading to improved memory efficiency and algorithmic flexibility in occlusion reasoning, real-time volumetric mapping, semantic accumulation, and conversion between 3D and task-appropriate 2D representations. It is implemented as a C++ library and integrated into the Robot Operating System (ROS) ecosystem, supporting a range of mobile robotics pipelines in autonomous exploration, navigation, and scene understanding (Duberg et al., 2020, Khoche et al., 2022, Fredriksson et al., 2024).

1. Foundational Principles and Representation

UFOMap departs from classical binary occupancy grid frameworks by explicitly modeling three per-voxel states: occupied, free, and unknown. Each octree node's state is tracked via a 2-bit flag, making the distinction between unobserved and observed-empty regions algorithmically accessible. This tri-state representation enables explicit reasoning about exploration frontiers, occluded regions, and map completeness.

Occupancy is encoded probabilistically, typically using the log-odds parameterization: for node $n$, $l = \log\big(p/(1-p)\big)$, with $l_0=0$ as the prior (unknown state, $p=0.5$). Updates from sensor data are performed via an inverse sensor model: $$l_t = \mathrm{clamp}(l_{t-1} + l(z) - l_0, L_{\min}, L_{\max})$$ where $z$ is a measurement, and clamping bounds are set to prevent numerical overflow. For a typical sensor, $l(z=\mathrm{occupied})=+4.0$ and $l(z=\mathrm{free})=-1.0$, with $L_{\min}=-10$ and $L_{\max}=+10$ (Duberg et al., 2020).

2. Hierarchical Octree Structure and Data Efficiency

UFOMap utilizes a pointer-based adaptive octree. The structure enables logarithmic access and amortized storage for large-scale sparse 3D environments. In contrast to prior art, leaf nodes or subtrees with uniform unknown state are represented by singleton sentinel nodes, with shared reference counting, which prevents unnecessary allocation of memory for vast unobserved regions.

Table: Memory Comparison (resolution = 1 cm) (Duberg et al., 2020)

Environment	OctoMap	UFOMap	Savings
Office (2 m³)	210 MB	115 MB	45%
Courtyard	330 MB	190 MB	42%
KITTI sequence	500 MB	295 MB	41%

Memory per node (excluding unknown-subtrees):

• 8 child-pointers (64 B)
• 1 float16 log-odds (2 B)
• state flag + padding (1 B)
• optional color buffer pointer (8 B)
• total ≈ 75 B

This design yields measured savings of ≈40% compared to traditional occupancy octrees and enables dense representations at sub-centimeter resolution.

3. Incremental Mapping, Updates, and Performance

Beam/ray-tracing integration underpins real-time volumetric mapping. Each measurement updates all traversed voxels (as free) and the endpoint voxel (as occupied). Pseudocode for a single beam is:

$l_0=0$5

Complexity per beam is $l = \log\big(p/(1-p)\big)$0, with octree depth $l = \log\big(p/(1-p)\big)$1 and voxel resolution $l = \log\big(p/(1-p)\big)$2. Optimizations include short-circuiting traversal for homogeneous unknown subtrees. UAV-scale maps (e.g., 1 cm resolution, 100,000 points) sustain $l = \log\big(p/(1-p)\big)$360 Hz insertion on contemporary CPUs.

Empirical evaluation:

• Laboratory room (1 cm): 8M pts/s at 80 Hz, 115 MB memory (OctoMap: 210 MB, <10 Hz)
• Outdoor courtyard (2 cm): 5M pts/s at 50 Hz, 190 MB
• KITTI sequence (5 cm): 2M pts/s at 20 Hz, 295 MB (Duberg et al., 2020)

4. Semantic and Colored Mapping Extensions

UFOMap supports fusion of semantic labels and color information during update. For channels such as per-point DNN segmentation probabilities, semantic label counts $l = \log\big(p/(1-p)\big)$4 are maintained in each leaf node:

• Count-based: Increment $l = \log\big(p/(1-p)\big)$5 if $l = \log\big(p/(1-p)\big)$6 (top-1 hypothesis)
• Confidence-weighted: $l = \log\big(p/(1-p)\big)$7

Posterior voxel label probabilities: $l = \log\big(p/(1-p)\big)$8

Color fusion maintains an RGB triplet and measurement count, fusing new measurements $l = \log\big(p/(1-p)\big)$9 as: $l_0=0$0

This enables accumulation of both color and rich semantic context, essential for downstream reasoning and for constructing 2.5D/BEV abstractions (Khoche et al., 2022).

5. Map Query, Conversion, and Practical Integration

Hierarchical queries enable spatial and semantic extraction at arbitrary resolution:

• getIndicatorOccupied(), getIndicatorFree(), getIndicatorUnknown() return constituent voxels within a bounding volume.
• Unknown and occluded space can be efficiently identified for exploration or risk assessment.

Conversion from 3D octree to robot-compatible 2D occupancy maps leverages UFOMap’s volumetric state. For each 2D grid cell:

1. Extract vertical free/occupied intervals from 3D octree column.
2. Discard free intervals below robot vertical safety margin $l_0=0$1.
3. Store resulting (floor, ceiling) pair or mark as unknown.
4. Build a local slope map via 3x3 least-squares plane fit on floor heights; cells above traversable slope threshold $l_0=0$2 are marked occupied for UGVs.

For aerial and ground robots, two distinct 2D maps are formed: $l_0=0$3 (UAV-specific) from free-space projection and $l_0=0$4 (UGV-specific) with additional slope filtering. Heights are re-used for "lifting" 2D planned paths to collision-safe 3D trajectories, further validated by safety volumes against extracted ceiling heights (Fredriksson et al., 2024).

Table: Conversion Performance (cave scenario, 0.1 m voxels) (Fredriksson et al., 2024)

Step	Time (ms)
Range extraction + height map	50
Slope estimation	20
2D occupancy map generation	10
Total	80

3D UFOMap: ≈60 MB; 2D height+slope+occupancy: ≈5 MB.

6. Applications in Robotics and Autonomous Systems

UFOMap is utilized in diverse real-time robotics applications:

• Autonomous exploration and frontier detection: explicit handling of unknown space enables efficient exploration with guaranteed coverage (Duberg et al., 2020).
• Planning and collision checking: direct queries return point clouds of free/occupied/unknown space for OMPL/MoveIt!
• Dense colored reconstruction: achieves <5 mm accuracy in indoor scene recovery.
• Autonomous driving: supports multi-label semantic fusion for risk-aware local mapping and occlusion reasoning, operating at frame rates >10 Hz, improving LiDAR segmentation performance (e.g., +2% mIoU on SemanticKITTI) (Khoche et al., 2022).

Fast 3D→2D map conversion makes efficient navigation pipelines feasible for UAVs and UGVs, decouples global planning from volumetric complexity, and enables map sharing in bandwidth-constrained multi-robot deployments (Fredriksson et al., 2024).

7. Limitations, Challenges, and Future Developments

Current limitations include:

• Dynamic environments: occupancy states are static; moving objects may cause lingering false occupancies until cleared by explicit time decay or manual resetting.
• Multi-robot coordination: no built-in map merging, necessitating user-defined synchronization for collaborative mapping.
• Disk persistence: binary map dumps are supported, but there is no incremental streaming protocol.

Proposed future enhancements:

• Per-node timestamps and exponential forgetting for dynamic object filtering.
• Multi-map support with late fusion for collaborative mapping.
• GPU-accelerated bulk-insertion and ray-tracing to support higher sensor rates and finer resolutions.
• Integration with signed distance fields for advanced gradient-based planning (Duberg et al., 2020, Khoche et al., 2022).

A plausible implication is that as semantic fusion and dynamic scene modeling become more tightly integrated, UFOMap's octree abstraction will offer a scalable substrate for rich, multi-modal spatial reasoning in real-world robotic systems, bridging current gaps between dense geometric mapping and high-level scene understanding.