# probability_updater.cpp / .h

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

**Location**: `sonar_3d_reconstruction/cpp/probability_updater.cpp`

**Implements**: [IMapperBackend](mapper_backend.md)

---

## 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](iwlo_updater.md))
- 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

```cpp
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

```cpp
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

```cpp
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

```cpp
void set_clamping_thresholds(double min_prob, double max_prob);
```

Set probability clamping limits.

#### set_intensity_params

```cpp
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

```cpp
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

```cpp
void set_occupied_threshold(double occupied_thresh);
```

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

---

### Core Update Method

#### batch_update_iwlo

```cpp
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):
1. Compute intensity-based weight: `w = sigmoid(sharpness × (normalized - 0.5))`
2. Compute learning rate: `α = max(min_alpha, 1/(1 + decay_rate × n))`
3. Apply adaptive scaling (bidirectional protection)
4. Compute log-odds update:
   - Occupied: `ΔL = L_occ × w × α × scale`
   - Free: `ΔL = L_free × α × scale`
5. Apply with saturation: `L_new = clamp(L + ΔL, L_min, L_max)`

See [IWLO Design](../design/iwlo_design.md) for detailed algorithm explanation.

---

### Query Methods

#### get_occupied_voxels

```cpp
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

```cpp
MemoryStats get_memory_usage() const;
```

Returns memory statistics from underlying OctreeMapper.

#### get_num_nodes

```cpp
size_t get_num_nodes() const;
```

Returns total number of nodes in octree.

#### get_resolution

```cpp
double get_resolution() const;
```

Returns voxel resolution in meters.

#### get_observation_count

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

Returns observation count for a specific voxel key.

---

### Maintenance Methods

#### prune_tree

```cpp
size_t prune_tree();
```

Prune unnecessary nodes. Returns number of nodes removed.

#### clear

```cpp
void clear();
```

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

---

### Synchronization Methods

#### set_incremental_sync

```cpp
void set_incremental_sync(bool enable);
```

Enable/disable incremental synchronization (default: enabled).

#### force_full_sync

```cpp
void force_full_sync();
```

Force full synchronization from `voxels_log_odds_` to `octree_mapper_`.

---

## Internal Implementation

### Storage Strategy

```cpp
// 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.

```cpp
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

```cpp
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

```cpp
#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

```cpp
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](mapper_backend.md) for the full interface specification.

---

## Related

- [IMapperBackend](mapper_backend.md) - Abstract interface implemented by this class
- [IWLOUpdater](iwlo_updater.md) - IWLO algorithm utilities
- [octree_mapper.cpp](octree_mapper.md) - Low-level OctoMap wrapper (used internally)
- [IWLO Design](../design/iwlo_design.md) - Algorithm details
- [Configuration Reference](config.md) - All ROS2 parameters