probability_updater.cpp / .h

C++ high-level probabilistic updater for In-Memory mode sonar mapping.

Location: sonar_3d_reconstruction/cpp/probability_updater.cpp

Overview

ProbabilityUpdater wraps OctreeMapper and provides a Python-friendly interface for sonar data processing. It implements the IWLO (Intensity-Weighted Log-Odds) algorithm for probabilistic voxel updates.

Key Features:

IWLO probability update algorithm (via IWLOUpdater)
Hash map storage for Python compatibility
Incremental synchronization to OctoMap
Adaptive scaling for noise reduction
Implements IMapperBackend interface (Backend Type: "RAM")

Architecture

┌─────────────────────────────────────────────────────────────────────┐
│  ProbabilityUpdater                                                 │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  Primary Storage (Python SimpleOctree compatible)            │   │
│  │  voxels_log_odds_: unordered_map<string, double>            │   │
│  │  observation_counts_: unordered_map<string, int>            │   │
│  │  modified_keys_: unordered_set<string>                      │   │
│  └─────────────────────────────────────────────────────────────┘   │
│                              │                                      │
│                              ▼ sync                                 │
│  ┌─────────────────────────────────────────────────────────────┐   │
│  │  OctreeMapper (for visualization)                            │   │
│  │  - OctoMap wrapper                                           │   │
│  │  - get_occupied_voxels()                                     │   │
│  └─────────────────────────────────────────────────────────────┘   │
└─────────────────────────────────────────────────────────────────────┘

Class: ProbabilityUpdater

Constructor

explicit ProbabilityUpdater(double resolution = 0.05);

Parameter	Type	Default	Description
`resolution`	double	0.05	Voxel resolution in meters

Default Parameters (initialized in constructor):

L_occ: 3.5 (internal: log_odds_occupied_)
L_free: -3.0 (internal: log_odds_free_)
sharpness: 0.1 (stonefish recommended)
min_alpha: 0.3
L_min: -10.0, L_max: 10.0

Configuration Methods

set_log_odds_params

void set_log_odds_params(double log_odds_occupied, double log_odds_free);

Set log-odds increment/decrement values for Bayesian updates.

Parameter	ROS2 Name	Description
`log_odds_occupied`	`iwlo.L_occ`	Log-odds increment for occupied
`log_odds_free`	`iwlo.L_free`	Log-odds decrement for free

set_adaptive_params

void set_adaptive_params(bool adaptive_enabled, double adaptive_threshold, double adaptive_max_ratio);

Configure adaptive scaling for noise reduction.

Parameter	Default	Description
`adaptive_enabled`	true	Enable/disable adaptive scaling
`adaptive_threshold`	0.5	Probability threshold for protection
`adaptive_max_ratio`	0.3	Maximum scaling ratio

set_clamping_thresholds

void set_clamping_thresholds(double min_prob, double max_prob);

Set probability clamping limits.

set_intensity_params

void set_intensity_params(double intensity_threshold, double intensity_max);

Parameter	Default	Description
`intensity_threshold`	35.0	Minimum intensity to consider
`intensity_max`	255.0	Maximum intensity value

set_iwlo_params

void set_iwlo_params(double sharpness, double decay_rate, double min_alpha,
                     double L_min, double L_max);

Configure IWLO algorithm parameters.

Parameter	Default	Description
`sharpness`	0.1	Sigmoid steepness for intensity-to-weight
`decay_rate`	0.1	Learning rate decay rate
`min_alpha`	0.3	Minimum learning rate
`L_min`	-10.0	Saturation lower bound
`L_max`	10.0	Saturation upper bound

set_occupied_threshold

void set_occupied_threshold(double occupied_thresh);

Set probability threshold for occupied classification (default: 0.7).

Core Update Method

batch_update_iwlo

void batch_update_iwlo(
    const Eigen::MatrixXd& points,
    const Eigen::VectorXd& intensities,
    const std::vector<bool>& is_occupied
);

Main entry point for probability updates using IWLO algorithm.

Parameter	Shape	Description
`points`	Nx3	Point coordinates (x, y, z)
`intensities`	N	Intensity values per point
`is_occupied`	N	Boolean flags (currently unused in IWLO)

IWLO Algorithm Steps (lines 349-384):

Compute intensity-based weight: w = sigmoid(sharpness × (normalized - 0.5))
Compute learning rate: α = max(min_alpha, 1/(1 + decay_rate × n))
Apply adaptive scaling (bidirectional protection)
Compute log-odds update:
- Occupied: ΔL = L_occ × w × α × scale
- Free: ΔL = L_free × α × scale
Apply with saturation: L_new = clamp(L + ΔL, L_min, L_max)

See IWLO Design for detailed algorithm explanation.

Query Methods

get_occupied_voxels

Eigen::MatrixXd get_occupied_voxels(double min_probability = 0.5) const;

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

get_memory_usage

MemoryStats get_memory_usage() const;

Returns memory statistics from underlying OctreeMapper.

get_num_nodes

size_t get_num_nodes() const;

Returns total number of nodes in octree.

get_resolution

double get_resolution() const;

Returns voxel resolution in meters.

get_observation_count

int get_observation_count(const std::string& key) const;

Returns observation count for a specific voxel key.

Maintenance Methods

prune_tree

size_t prune_tree();

Prune unnecessary nodes. Returns number of nodes removed.

clear

void clear();

Clear all data (octree, observation counts, log-odds map).

Synchronization Methods

set_incremental_sync

void set_incremental_sync(bool enable);

Enable/disable incremental synchronization (default: enabled).

force_full_sync

void force_full_sync();

Force full synchronization from voxels_log_odds_ to octree_mapper_.

Internal Implementation

Storage Strategy

// Primary storage (Python SimpleOctree compatible)
std::unordered_map<std::string, double> voxels_log_odds_;  // key -> log_odds
std::unordered_map<std::string, int> observation_counts_;  // key -> count
std::unordered_set<std::string> modified_keys_;            // for incremental sync

Voxel Key Format: "ix_iy_iz" where indices are discretized coordinates.

std::string world_to_key(double x, double y, double z) const {
    int ix = static_cast<int>(std::floor(x / resolution_));
    int iy = static_cast<int>(std::floor(y / resolution_));
    int iz = static_cast<int>(std::floor(z / resolution_));
    return std::to_string(ix) + "_" + std::to_string(iy) + "_" + std::to_string(iz);
}

Sync Strategies

Strategy	Method	Complexity	When Used
Incremental	`sync_modified_voxels_to_octree()`	O(N modified)	Default
Full	`sync_all_voxels_to_octree()`	O(M total)	`force_full_sync()`

Probability Conversion

double log_odds_to_probability(double log_odds) const {
    return 1.0 / (1.0 + std::exp(-log_odds));
}

double probability_to_log_odds(double probability) const {
    probability = std::max(1e-6, std::min(1.0 - 1e-6, probability));
    return std::log(probability / (1.0 - probability));
}

Usage Example

#include "probability_updater.h"

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

// Configure IWLO parameters
updater->set_iwlo_params(
    0.1,    // sharpness
    0.1,    // decay_rate
    0.3,    // min_alpha
    -10.0,  // L_min
    10.0    // L_max
);

// Configure intensity parameters
updater->set_intensity_params(35.0, 255.0);

// Process sonar data
Eigen::MatrixXd points(1000, 3);
Eigen::VectorXd intensities(1000);
std::vector<bool> is_occupied(1000);
// ... fill data ...

updater->batch_update_iwlo(points, intensities, is_occupied);

// Get results
auto occupied = updater->get_occupied_voxels(0.7);

Parameter Name Mapping

ROS2 parameter names (used in YAML/launch) vs C++ internal variable names:

ROS2 Parameter	C++ Variable	Description
`iwlo.L_occ`	`log_odds_occupied_`	Occupied log-odds increment
`iwlo.L_free`	`log_odds_free_`	Free log-odds decrement
`iwlo.L_min`	`L_min_`	Saturation lower bound
`iwlo.L_max`	`L_max_`	Saturation upper bound
`iwlo.sharpness`	`sharpness_`	Sigmoid slope
`iwlo.decay_rate`	`decay_rate_`	Learning rate decay
`iwlo.min_alpha`	`min_alpha_`	Minimum learning rate
`filtering.intensity_threshold`	`intensity_threshold_`	Intensity threshold

Note: ROS2 parameters use L_occ, L_free for consistency with IWLO design documentation. C++ uses more descriptive log_odds_occupied_, log_odds_free_ internally.

IMapperBackend Implementation

As an IMapperBackend implementation, ProbabilityUpdater provides:

Backend Identity

std::string get_backend_type() const override { return "RAM"; }
bool supports_persistence() const override { return false; }

Optional Methods

Method	Behavior
`flush()`	No-op (RAM-only storage)
`preload_region()`	No-op
`get_disk_usage()`	Returns 0
`prune()`	Delegates to `prune_tree()`

See IMapperBackend for the full interface specification.