octree_mapper.cpp / .h

C++ low-level OctoMap wrapper for spatial octree operations.

Location: sonar_3d_reconstruction/cpp/octree_mapper.cpp

Overview

OctreeMapper provides a thin wrapper around the OctoMap library’s OcTree class. It handles probabilistic voxel updates, batch processing, and file I/O operations.

Key Features:

OctoMap-based spatial octree structure
Dual storage: OcTree + hash map for Python compatibility
Batch update operations
File persistence (binary format)

Note: This is a low-level class. Use ProbabilityUpdater for high-level IWLO updates.

Architecture

┌─────────────────────────────────────────────────────────────────────┐
│  OctreeMapper                                                       │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  octomap::OcTree (unique_ptr)                                │   │
│  │  - Spatial octree structure                                  │   │
│  │  - Efficient spatial queries                                 │   │
│  │  - Native OctoMap operations                                 │   │
│  └─────────────────────────────────────────────────────────────┘   │
│                                                                     │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  log_odds_map_: unordered_map<point3d, double>              │   │
│  │  - Python SimpleOctree compatible                            │   │
│  │  - Direct log-odds access                                    │   │
│  │  - Used by get_occupied_voxels()                            │   │
│  └─────────────────────────────────────────────────────────────┘   │
│                                                                     │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  Cache System                                                │   │
│  │  - cached_num_nodes_: size_t                                │   │
│  │  - cache_valid_: bool                                       │   │
│  │  - Avoids expensive calcNumNodes() calls                    │   │
│  └─────────────────────────────────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────────────┘

Struct: MemoryStats

Memory usage statistics returned by get_memory_usage().

struct MemoryStats {
    size_t num_nodes;        // Total nodes in octree
    size_t num_leaf_nodes;   // Leaf nodes only
    double memory_mb;        // Estimated memory in MB
    double memory_efficiency; // Ratio: leaf_nodes / total_nodes
};

Class: OctreeMapper

Constructor

OctreeMapper(double resolution = 0.05,
             double prob_hit = 0.7,
             double prob_miss = 0.3,
             double prob_thres_min = 0.12,
             double prob_thres_max = 0.97);

Parameter	Default	Description
`resolution`	0.05	Voxel size in meters
`prob_hit`	0.7	Probability for occupied updates
`prob_miss`	0.3	Probability for free updates
`prob_thres_min`	0.12	Clamping minimum
`prob_thres_max`	0.97	Clamping maximum

Initialization:

Creates octomap::OcTree with given resolution
Sets OctoMap internal threshold to 0.5 (for spatial queries only)
Initializes log-odds bounds: L_min = -10.0, L_max = 10.0

Note: The actual occupied threshold used for visualization is passed as argument to get_occupied_voxels(min_probability), configured via ROS2 parameter mapping.occupied_threshold (default: 0.7).

Update Methods

update_point

void update_point(const Eigen::Vector3d& point, bool occupied);

Update single voxel. Calls octree_->updateNode() internally.

batch_update

void batch_update(const Eigen::MatrixXd& points, const std::vector<bool>& occupied_flags);

Batch update using OctoMap’s native probability model.

Parameter	Shape	Description
`points`	Nx3	Point coordinates
`occupied_flags`	N	Boolean flags

Implementation (lines 80-124):

Validate coordinates (check isfinite)
Collect valid updates into vector
Apply updates sequentially (minimizes node expansion conflicts)

batch_update_with_log_odds

void batch_update_with_log_odds(const Eigen::MatrixXd& points,
                                const Eigen::VectorXd& log_odds_updates);

Primary update method used by ProbabilityUpdater.

Parameter	Shape	Description
`points`	Nx3	Point coordinates
`log_odds_updates`	N	Absolute log-odds values

Implementation (lines 126-172):

Direct assignment to log_odds_map_ (NOT incremental)
Clamp values to [L_min, L_max]
Update OcTree for spatial queries

Important: Values are absolute log-odds (not deltas). IWLO computes final values.

insert_ray

void insert_ray(const Eigen::Vector3d& origin, const Eigen::Vector3d& endpoint);

Ray casting: marks free space along ray, occupied at endpoint.

Query Methods

get_occupied_voxels

Eigen::MatrixXd get_occupied_voxels(double min_probability = -1.0) const;

Returns Nx4 matrix [x, y, z, probability] for voxels above threshold.

Implementation (lines 185-227):

Convert probability threshold to log-odds
Iterate log_odds_map_ (not OcTree)
Filter by log-odds threshold
Convert log-odds to probability for output

Default threshold: 0.5 (if min_probability < 0)

get_memory_usage

MemoryStats get_memory_usage() const;

Returns memory statistics. Memory estimate: num_nodes × 32 bytes.

get_num_nodes

size_t get_num_nodes() const;

Returns cached node count. Uses lazy evaluation pattern.

get_resolution

double get_resolution() const;

Returns voxel resolution in meters.

Configuration Methods

set_probability_params

void set_probability_params(double prob_hit, double prob_miss);

Update OctoMap probability parameters.

set_occupancy_thresholds

void set_occupancy_thresholds(double min_thresh, double max_thresh);

Update occupancy and clamping thresholds.

Parameter	Description
`min_thresh`	Clamping minimum (probability floor)
`max_thresh`	Occupancy threshold + clamping maximum

Note: max_thresh serves dual purpose - both occupancy threshold and clamping ceiling.

Maintenance Methods

prune_tree

size_t prune_tree();

Merge identical child nodes. Returns nodes removed.

clear

void clear();

Clear both OcTree and log_odds_map_.

File I/O

save_to_file

bool save_to_file(const std::string& filename) const;

Save OcTree to binary file (.bt format).

load_from_file

bool load_from_file(const std::string& filename);

Load OcTree from binary file.

Note: File I/O uses SuppressOutput RAII class to suppress OctoMap log messages.

Internal Implementation

SuppressOutput Class

RAII class to redirect stdout/stderr to /dev/null:

class SuppressOutput {
public:
    SuppressOutput() {
        // Save file descriptors, redirect to /dev/null
        stdout_fd_ = dup(fileno(stdout));
        freopen("/dev/null", "w", stdout);
    }
    ~SuppressOutput() {
        // Restore original file descriptors
        dup2(stdout_fd_, fileno(stdout));
        close(stdout_fd_);
    }
};

Purpose: OctoMap prints verbose messages during file operations.

Cache System

Expensive calcNumNodes() calls are cached:

void update_cache() const {
    if (!cache_valid_) {
        cached_num_nodes_ = octree_->calcNumNodes();
        cache_valid_ = true;
    }
}

void invalidate_cache() {
    cache_valid_ = false;
}

Cache invalidated after any update operation.

Point3d Hash

Custom hash for octomap::point3d used in log_odds_map_:

struct Point3dHash {
    std::size_t operator()(const octomap::point3d& p) const {
        std::size_t h1 = std::hash<float>{}(p.x());
        std::size_t h2 = std::hash<float>{}(p.y());
        std::size_t h3 = std::hash<float>{}(p.z());
        return h1 ^ (h2 << 1) ^ (h3 << 2);
    }
};

Dual Storage Rationale

Storage	Purpose	Used By
`octree_` (OcTree)	Spatial queries, ray casting	`insert_ray()`, native OctoMap ops
`log_odds_map_`	Exact log-odds values	`get_occupied_voxels()`, Python compatibility

Why both? OctoMap internally converts log-odds to probability with clamping. The hash map preserves exact IWLO-computed values for visualization.

Usage Example

#include "octree_mapper.h"

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

// Batch update with log-odds
Eigen::MatrixXd points(100, 3);
Eigen::VectorXd log_odds(100);
// ... fill data ...

mapper->batch_update_with_log_odds(points, log_odds);

// Get occupied voxels (probability > 0.7)
auto occupied = mapper->get_occupied_voxels(0.7);

// Save to file
mapper->save_to_file("map.bt");