# 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](../api/octree_mapper.md) |
| `ProbabilityUpdater` | IWLO + OctreeMapper combination | [API Reference](../api/probability_updater.md) |

---

## 5. Related Documents

### Design Documents
- [IWLO Probability Update](iwlo_design.md) - Probability update algorithm
- [Out-of-Core Tile Mapping](outofcore_design.md) - Large-scale storage

### API Reference
- [octree_mapper](../api/octree_mapper.md) - Low-level OctoMap wrapper
- [probability_updater](../api/probability_updater.md) - High-level IWLO updater

---

## 6. References

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