cartographer/mapping/2d/submap_2d.h

Overview

Submap2D is the 2D implementation of the Submap interface. It wraps a ProbabilityGrid or TSDF grid and manages the submap lifecycle.

Class: cartographer::mapping::Submap2D

Inheritance

Submap (abstract)
└── Submap2D

Construction

Submap2D(const transform::Rigid3d& local_pose,
         std::unique_ptr<Grid2D> grid);

Methods

const Grid2D* grid() const { return grid_.get(); }
Grid2D* grid() { return grid_.get(); }

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

Grid Access

The underlying grid can be ProbabilityGrid or TSDF. Use dynamic_cast to access specific functionality:

auto* prob_grid = dynamic_cast<ProbabilityGrid*>(submap->grid());
if (prob_grid) {
  // Use probability grid methods
}

auto* tsdf = dynamic_cast<TSDF*>(submap->grid());
if (tsdf) {
  // Use TSDF methods
}

Submap Creation

Created by LocalTrajectoryBuilder2D via MapBuilder/PoseGraph:

// In LocalTrajectoryBuilder2D::AddSubmap()
auto grid = std::make_unique<ProbabilityGrid>(limits);
auto submap = std::make_unique<Submap2D>(local_pose, std::move(grid));
SubmapId submap_id = pose_graph_->AddSubmap(trajectory_id, local_pose);

Configuration

From trajectory_builder_2d.lua:

submaps = {
  num_range_data = 90,  -- Insertions before finishing
  grid_options_2d = {
    grid_type = "PROBABILITY_GRID",  -- or "TSDF"
    resolution = 0.05,
  },
  range_data_inserter = {
    range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
    probability_grid_range_data_inserter = { ... },
    tsdf_range_data_inserter = { ... },
  },
}

Grid Types

Type

Description

Use Case

PROBABILITY_GRID

Log-odds occupancy

Standard 2D SLAM

TSDF

Truncated Signed Distance Field

Surface reconstruction

Inserter Types

Type

Compatible Grid

Description

PROBABILITY_GRID_INSERTER_2D

ProbabilityGrid

Standard log-odds update

TSDF_RANGE_DATA_INSERTER_2D

TSDF

Distance field update

Submap Lifecycle

1. Created with initial pose and empty grid
2. Receives range data insertions (up to num_range_data)
3. Marked finished (insertion_finished = true)
4. Added to pose graph for loop closure
5. Optionally trimmed by OverlappingSubmapsTrimmer2D

Finishing a Submap

void FinishSubmap() {
  submap_->set_insertion_finished();
  // Notify pose graph
  pose_graph_->SetSubmapFinished(submap_id);
}

Serialization

message Submap2D {
  SubmapId id;
  Rigid3d local_pose;
  int32 num_range_data;
  bool insertion_finished;
  ProbabilityGrid2D probability_grid;  // or TSDF2D
}

Usage

// Access from pose graph
auto submap_data = pose_graph->GetSubmapData();
for (const auto& [submap_id, data] : submap_data) {
  if (auto* submap2d = dynamic_cast<Submap2D*>(data.submap.get())) {
    const auto* grid = submap2d->grid();
    // Iterate cells
    grid->ApplyToCells([&](const Eigen::Array2i& cell) {
      float prob = grid->GetProbability(cell);
      // Process...
    });
  }
}

See Also