octree_storage.cpp / .h

C++ OctoMap-based implementation of VoxelStorage.

Location: sonar_3d_reconstruction/cpp/octree_storage.h, octree_storage.cpp

Overview

OctreeStorage is the primary implementation of the VoxelStorage interface. It uses OctoMap’s OcTree for spatial indexing combined with a hash map for IWLO metadata (log-odds, observation counts).

Key Features:

  • OctoMap OcTree for efficient spatial queries

  • Separate metadata map preserving exact IWLO values

  • Binary file format for IWLO metadata persistence

  • Dirty flag for efficient save operations

Architecture

┌─────────────────────────────────────────────────────────────────────┐
│  OctreeStorage                                                       │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  iwlo_meta_: unordered_map<OcTreeKey, IWLOMeta>              │   │
│  │  - Source of truth for IWLO algorithm                        │   │
│  │  - Stores exact log-odds and observation counts              │   │
│  └─────────────────────────────────────────────────────────────┘   │
│                              │                                      │
│                              ▼ sync_to_octree()                     │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  octree_: unique_ptr<OcTree>                                 │   │
│  │  - For visualization (RViz)                                  │   │
│  │  - For spatial queries                                       │   │
│  │  - Discretized probability values                            │   │
│  └─────────────────────────────────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────────────┘

Class: OctreeStorage

Constructor

explicit OctreeStorage(double resolution);

Parameter

Type

Default

Description

resolution

double

-

Voxel resolution in meters

Creates a new OcTree with the specified resolution.

Copy/Move Semantics

OctreeStorage(const OctreeStorage&) = delete;           // No copy
OctreeStorage& operator=(const OctreeStorage&) = delete; // No copy assignment
OctreeStorage(OctreeStorage&&) = default;               // Move allowed
OctreeStorage& operator=(OctreeStorage&&) = default;    // Move assignment allowed

VoxelStorage Interface Implementation

Core Operations

All core operations are implemented as specified in VoxelStorage:

  • get_log_odds() / set_log_odds()

  • get_observation_count() / increment_observation_count()

  • get_or_create_meta()

Coordinate Operations

octomap::OcTreeKey coord_to_key(const octomap::point3d& point) const override;
octomap::point3d key_to_coord(const octomap::OcTreeKey& key) const override;

Delegates to the underlying OcTree’s coordinate system.

Query Operations

std::vector<OccupiedVoxel> get_occupied_voxels(double min_probability = 0.5) const override;
size_t get_num_voxels() const override;
bool has_occupied_voxels(double min_probability = 0.5) const override;

Queries are performed on iwlo_meta_ map with probability conversion.

Persistence Operations

bool save(const std::string& path) override;
bool load(const std::string& path) override;

Saves/loads to a directory with two files:

  • octree.bt - OctoMap binary tree

  • iwlo_meta.bin - IWLO metadata binary

Maintenance Operations

void clear() override;
void sync_to_octree() override;
size_t prune() override;
size_t get_memory_usage() const override;

Additional Methods

get_resolution

double get_resolution() const;

Returns the voxel resolution in meters.

Dirty Flag Management

bool is_dirty() const;
void mark_clean();
void mark_dirty();

Tracks whether storage has been modified since last save.

Usage: Tile class uses this for efficient persistence:

if (storage_->is_dirty()) {
    storage_->save(path);
    storage_->mark_clean();
}

File Formats

octree.bt

Standard OctoMap binary format. Created by octree_->writeBinary().

iwlo_meta.bin

Custom binary format for IWLO metadata:

Header (16 bytes):
┌──────────────────────────────────────────┐
│  magic: uint32 (0x49574C4F = "IWLO")     │  4 bytes
│  version: uint32 (1)                      │  4 bytes
│  count: uint64 (number of entries)        │  8 bytes
└──────────────────────────────────────────┘

Entries (18 bytes each):
┌──────────────────────────────────────────┐
│  key[0]: uint16                           │  2 bytes
│  key[1]: uint16                           │  2 bytes
│  key[2]: uint16                           │  2 bytes
│  log_odds: double                         │  8 bytes
│  observation_count: int32                 │  4 bytes
└──────────────────────────────────────────┘

Usage Example

#include "octree_storage.h"

// Create storage with 5cm resolution
auto storage = std::make_unique<OctreeStorage>(0.05);

// Update a voxel
octomap::point3d point(1.0, 2.0, 3.0);
auto key = storage->coord_to_key(point);

auto& meta = storage->get_or_create_meta(key);
meta.log_odds += 1.5;  // Update log-odds
meta.observation_count++;

storage->mark_dirty();

// Sync to OcTree for visualization
storage->sync_to_octree();

// Save to disk
storage->save("/path/to/tile_dir");
storage->mark_clean();

// Query occupied voxels
auto occupied = storage->get_occupied_voxels(0.7);
for (const auto& voxel : occupied) {
    std::cout << voxel.x << ", " << voxel.y << ", " << voxel.z
              << " -> " << voxel.probability << std::endl;
}

Design Rationale

Why dual storage?

  • OctoMap internally converts log-odds to float with clamping

  • Hash map preserves exact log-odds values for IWLO algorithm

  • sync_to_octree() copies discretized values for visualization

Memory trade-off:

  • Additional ~20 bytes per voxel for metadata

  • Enables accurate probability tracking without OctoMap limitations