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

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

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

Grid Access

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

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

Calls function for every populated cell.

Serialization

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

Coordinate Conversion

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:

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:

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:

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

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

// 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