# cartographer/mapping/3d/hybrid_grid.h

## Overview

`HybridGrid` is a sparse 3D voxel grid implemented as a flat hash map. It stores occupancy probabilities for 3D SLAM submaps, using a hybrid approach: high-resolution near the sensor, low-resolution at distance.

## Class: `cartographer::mapping::HybridGrid`

### Template Parameters

```cpp
template <typename T>
class HybridGrid {
  // T = Cell type (e.g., uint16 for probability, float for TSDF)
};
```

Typically used as `HybridGrid<uint16_t>` for probability values (correspondence cost).

### Construction

```cpp
explicit HybridGrid(float resolution);
HybridGrid(const proto::HybridGrid& proto);
```

### Grid Access

```cpp
float resolution() const;

bool GetValue(const Eigen::Array3i& cell_index, T* value) const;
T GetValue(const Eigen::Array3i& cell_index) const;
void SetValue(const Eigen::Array3i& cell_index, const T& value);
```

### Iteration

```cpp
void ApplyToCells(const std::function<void(const Eigen::Array3i&, const T&)>& function) const;
```

Calls function for every populated cell.

### Serialization

```cpp
proto::HybridGrid ToProto() const;
void ToProto(proto::HybridGrid* proto) const;
```

### Coordinate Conversion

```cpp
Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const;
Eigen::Vector3f GetCellCenter(const Eigen::Array3i& cell_index) const;
```

## Sparse Storage

Internally uses `std::unordered_map<uint64_t, T>` where the key is a packed 3D index:

```cpp
uint64_t key = (static_cast<uint64_t>(x) << 42) |
               (static_cast<uint64_t>(y) << 21) |
               static_cast<uint64_t>(z);
```

This allows efficient storage of large 3D grids where most voxels are empty.

## Submap3D Structure

`Submap3D` uses **two HybridGrids**:

```cpp
class Submap3D : public Submap {
 public:
  std::unique_ptr<HybridGrid<uint16_t>> high_resolution_hybrid_grid_;
  std::unique_ptr<HybridGrid<uint16_t>> low_resolution_hybrid_grid_;
  
  float high_resolution_;
  float low_resolution_;
  float high_resolution_max_range_;
};
```

### Resolution Hierarchy

| Grid | Resolution | Range | Purpose |
|------|------------|-------|---------|
| High-res | e.g., 0.10m | 0 to `high_resolution_max_range` (e.g., 20m) | Local scan matching detail |
| Low-res | e.g., 0.45m | `high_resolution_max_range` to `max_range` (e.g., 60m) | Loop closure, far obstacles |

### Configuration

From `trajectory_builder_3d.lua`:
```lua
submaps = {
  high_resolution = 0.10,
  high_resolution_max_range = 20.,
  low_resolution = 0.45,
  num_range_data = 160,
  range_data_inserter = {
    hit_probability = 0.55,
    miss_probability = 0.49,
    num_free_space_voxels = 2,
    intensity_threshold = 40,
  },
}
```

## Insertion

Handled by `RangeDataInserter3D` (in `submap_3d.cc`):

For each return point:
1. Determine which grid (high/low res) based on distance from origin
2. Update hit cell probability
3. Cast ray from origin to return, mark `num_free_space_voxels` as free

Intensity threshold: points with intensity < threshold are treated as misses (free space).

## Probability Representation

Uses same log-odds / correspondence cost as 2D:
- `uint16_t` value = correspondence cost (0 = occupied, 32767 = free/unknown)
- Conversion: `ProbabilityToCorrespondenceCost()`, `CorrespondenceCostToProbability()`

## Serialization

```proto
message HybridGrid {
  float resolution = 1;
  repeated uint64 cell_indices = 2;  // Packed indices
  repeated uint32 cell_values = 3;   // Correspondence costs
}
```

Each cell stored as (packed_index, value) pair.

## Usage

```cpp
// Create high-res grid
auto high_res_grid = std::make_unique<HybridGrid<uint16_t>>(0.10f);
auto low_res_grid = std::make_unique<HybridGrid<uint16_t>>(0.45f);

// Insert range data (typically via RangeDataInserter3D)
range_data_inserter.Insert(range_data, high_res_grid.get(), low_res_grid.get());

// Query
Eigen::Array3i cell = high_res_grid->GetCellIndex(Eigen::Vector3f(1, 2, 0.5));
uint16_t cost;
if (high_res_grid->GetValue(cell, &cost)) {
  float prob = CorrespondenceCostToProbability(cost);
}

// Iterate all cells
high_res_grid->ApplyToCells([](const Eigen::Array3i& cell, uint16_t value) {
  // Process cell
});
```

## See Also

- [Submap3D](submap_3d.md) — 3D submap using HybridGrid
- [RangeDataInserter3D](submap_3d.md) — 3D insertion logic
- [ProbabilityGrid](../2d/probability_grid.md) — 2D equivalent