Octree Mapping Design

Overview

OctoMap-based 3D spatial data structure providing efficient memory usage and probabilistic voxel storage.

Key Features:

Sparse representation: Only observed voxels are stored
Hierarchical structure: Recursive octant division supports multiple resolutions
Probabilistic storage: Log-odds based occupancy probability storage

1. Data Structure

1.1 Octree Structure

Definition: A tree structure that recursively divides 3D space into 8 octants

         Root (entire space)
        /   |   |   \
    Octant 0  1  2  ...  7
      /  \
   Child  Child
     .
     .
   Leaf (voxel)

Octant Indexing:

Index	X	Y	Z
0	-	-	-
1	+	-	-
2	-	+	-
3	+	+	-
4	-	-	+
5	+	-	+
6	-	+	+
7	+	+	+

Properties:

Resolution (\(r\)): Minimum voxel size (default: 0.05m)
Max depth: \(\log_2(\text{map\_size} / r)\)

1.2 Voxel Key

Definition: Converts world coordinates to voxel indices

\[\text{key} = (i_x, i_y, i_z)\]

where:

\[i_x = \lfloor x / r \rfloor\]

\[i_y = \lfloor y / r \rfloor\]

\[i_z = \lfloor z / r \rfloor\]

Inverse (Key to World Center):

\[x = (i_x + 0.5) \times r\]

\[y = (i_y + 0.5) \times r\]

\[z = (i_z + 0.5) \times r\]

1.3 Dual Storage Strategy

The system maintains two storage types simultaneously:

Storage	Type	Purpose	Access
OcTree	`octomap::OcTree`	Spatial queries, ray casting	\(O(\log N)\)
Hash map	`unordered_map<key, double>`	Exact log-odds values	\(O(1)\)

Rationale:

OctoMap internally converts log-odds to float with clamping
Hash map preserves exact log-odds values computed by IWLO
For visualization, filter by threshold from hash map and return results

2. Memory Efficiency

2.1 Sparse Representation

Dense Grid vs Octree:

Map Size	Dense Grid	Octree (10% occupied)	Savings
10m³ @ 0.05m	32 GB	3.2 GB	10x
100m³ @ 0.05m	32 TB	320 GB	100x

Property: Unobserved regions consume no memory

2.2 Pruning Algorithm

Definition: Merges child nodes with identical probabilities into parent node

ALGORITHM: Prune_Octree
INPUT: octree root node

1. FOR each internal node (bottom-up):
     IF all 8 children exist AND
        all children are leaves AND
        all children have same probability THEN
       REPLACE 8 children with single node
       deleted_count += 7
     ENDIF

2. RETURN deleted_count

Memory Reduction: Reduces node count after pruning (effective in homogeneous regions)

3. Operations

3.1 Point Update

ALGORITHM: Update_Point
INPUT: point (x, y, z), log_odds value

1. COMPUTE key:
   ix := floor(x / resolution)
   iy := floor(y / resolution)
   iz := floor(z / resolution)
   key := (ix, iy, iz)

2. UPDATE hash map:
   hash_map[key] := log_odds

3. UPDATE OcTree:
   coord := octree.keyToCoord(key)
   node := octree.updateNode(coord, occupied=true)
   node.setLogOdds(log_odds)

3.2 Query Occupied Voxels

ALGORITHM: Get_Occupied_Voxels
INPUT: min_probability threshold
OUTPUT: list of (x, y, z, probability)

1. COMPUTE log_odds threshold:
   L_th := log(min_probability / (1 - min_probability))

2. INITIALIZE results := []

3. FOR each (key, log_odds) in hash_map:
     IF log_odds >= L_th THEN
       probability := 1 / (1 + exp(-log_odds))
       (x, y, z) := key_to_world(key)
       results.append((x, y, z, probability))
     ENDIF

4. RETURN results

3.3 Ray Casting

ALGORITHM: Insert_Ray
INPUT: origin (x0, y0, z0), endpoint (x1, y1, z1)

1. TRACE ray from origin to endpoint:
   FOR each voxel along ray (excluding endpoint):
     octree.updateNode(voxel, occupied=false)

2. UPDATE endpoint as occupied:
   octree.updateNode(endpoint, occupied=true)

Note: Current system does not use ray casting; performs direct point updates only

4. Implementation Classes

Class	Role	Location
`OctreeMapper`	OctoMap wrapper	API Reference
`ProbabilityUpdater`	IWLO + OctreeMapper combination	API Reference

6. References

Hornung et al. (2013): OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees
Wurm et al. (2010): OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems