# cartographer/mapping/3d/submap_3d.h

## Overview

`Submap3D` is the 3D implementation of the `Submap` interface. It uses two `HybridGrid` instances (high and low resolution) to represent the local map.

## Class: `cartographer::mapping::Submap3D`

### Inheritance

```
Submap (abstract)
└── Submap3D
```

### Construction

```cpp
Submap3D(const transform::Rigid3d& local_pose,
         float high_resolution,
         float high_resolution_max_range,
         float low_resolution,
         const RangeDataInserterOptions& range_data_inserter_options);
```

### Members

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

RangeDataInserterOptions range_data_inserter_options_;
```

### Methods

```cpp
const HybridGrid<uint16_t>* high_resolution_hybrid_grid() const;
const HybridGrid<uint16_t>* low_resolution_hybrid_grid() const;

HybridGrid<uint16_t>* high_resolution_hybrid_grid();
HybridGrid<uint16_t>* low_resolution_hybrid_grid();

std::unique_ptr<Submap> ToProto() const override;
void UpdateFromProto(const proto::Submap& proto) override;
```

### Grid Access

```cpp
// Get appropriate grid for a point
const HybridGrid<uint16_t>* GetGrid(const Eigen::Vector3f& point) const {
  float range = point.norm();
  if (range <= high_resolution_max_range_) {
    return high_resolution_hybrid_grid_.get();
  }
  return low_resolution_hybrid_grid_.get();
}
```

## RangeDataInserterOptions

```cpp
struct RangeDataInserterOptions {
  float hit_probability = 0.55f;
  float miss_probability = 0.49f;
  int num_free_space_voxels = 2;
  int intensity_threshold = 40;
};
```

- `hit_probability` / `miss_probability` — Log-odds update values
- `num_free_space_voxels` — Voxels to mark free along each ray
- `intensity_threshold` — Points below this intensity are treated as misses

## Insertion Algorithm

In `submap_3d.cc::InsertRangeData()`:

```cpp
void InsertRangeData(const sensor::RangeData& range_data) {
  for (const auto& hit : range_data.returns) {
    // Determine grid based on distance from submap origin
    const auto* grid = GetGrid(hit);
    auto* mutable_grid = const_cast<HybridGrid<uint16_t>*>(grid);
    
    // Update hit cell
    UpdateCell(mutable_grid, hit, hit_probability);
    
    // Ray cast from origin to hit, mark free space
    CastRays(range_data.origin, hit, num_free_space_voxels, 
             miss_probability, mutable_grid);
  }
  
  // Also process misses (ray endpoints with no return)
  for (const auto& miss : range_data.misses) {
    const auto* grid = GetGrid(miss);
    auto* mutable_grid = const_cast<HybridGrid<uint16_t>*>(grid);
    CastRays(range_data.origin, miss, num_free_space_voxels,
             miss_probability, mutable_grid);
  }
}
```

## 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,
  },
}
```

## Submap Lifecycle

```
1. Created by LocalTrajectoryBuilder3D with initial pose
2. Receives accumulated range data insertions
3. After num_range_data insertions, marked finished
4. Added to PoseGraph3D for loop closure detection
5. New submap created for continued mapping
```

## Serialization

```proto
message Submap3D {
  SubmapId id;
  Rigid3d local_pose;
  int32 num_range_data;
  bool insertion_finished;
  HybridGrid high_resolution_hybrid_grid;
  HybridGrid low_resolution_hybrid_grid;
  float high_resolution;
  float low_resolution;
  float high_resolution_max_range;
}
```

## Usage

```cpp
// Access from pose graph
auto submap_data = pose_graph->GetSubmapData();
for (const auto& [submap_id, data] : submap_data) {
  if (auto* submap3d = dynamic_cast<Submap3D*>(data.submap.get())) {
    // High-res grid
    auto* high_res = submap3d->high_resolution_hybrid_grid();
    high_res->ApplyToCells([](const Eigen::Array3i& cell, uint16_t value) {
      // Process high-res cell
    });
    
    // Low-res grid
    auto* low_res = submap3d->low_resolution_hybrid_grid();
    low_res->ApplyToCells([](const Eigen::Array3i& cell, uint16_t value) {
      // Process low-res cell
    });
  }
}
```

## See Also

- [HybridGrid](hybrid_grid.md) — Sparse 3D voxel grid
- `Submap3D Insertion` — Insertion implementation
- [TrajectoryBuilderInterface](../trajectory_builder_interface.md) — Creates submaps
- [PoseGraphInterface](../pose_graph_interface.md) — Uses submaps for loop closure