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

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

Members

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

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

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

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():

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:

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

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

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