cartographer/mapping/pose_graph_interface.h

Overview

PoseGraphInterface defines the interface for the global pose graph optimization component. It maintains trajectories, submaps, constraints (loop closures), and performs global optimization using Ceres Solver.

Class: cartographer::mapping::PoseGraphInterface

Types

SubmapId

struct SubmapId {
  int trajectory_id;
  int submap_index;
};

Unique identifier for a submap within a trajectory.

NodeId

struct NodeId {
  int trajectory_id;
  int node_index;
};

Unique identifier for a trajectory node (pose).

Constraint

struct Constraint {
  SubmapId submap_id;
  NodeId node_id;
  transform::Rigid3d relative_pose;
  double translation_weight;
  double rotation_weight;
  ConstraintType type;  // INTRA_SUBMAP, INTER_SUBMAP
};

Represents a constraint between a node and a submap. Intra-submap constraints come from local SLAM; inter-submap constraints come from loop closure detection.

Constraint::Type

enum class ConstraintType {
  INTRA_SUBMAP = 0,   // Local SLAM constraint within a submap
  INTER_SUBMAP = 1,   // Loop closure constraint between submaps
};

Pure Virtual Methods

AddTrajectory

virtual int AddTrajectory(const proto::TrajectoryBuilderOptionsWithSensorIds&
                              options_with_sensor_ids) = 0;

Adds a new trajectory to the pose graph. Returns trajectory ID.

AddSubmap

virtual SubmapId AddSubmap(int trajectory_id,
                           const transform::Rigid3d& local_submap_pose) = 0;

Adds a new submap to a trajectory at the given local pose. Returns submap ID.

AddNode

virtual NodeId AddNode(int trajectory_id,
                       const transform::Rigid3d& local_node_pose) = 0;

Adds a trajectory node (pose) to the pose graph. Returns node ID.

AddConstraint

virtual void AddConstraint(const Constraint& constraint) = 0;

Adds a constraint (loop closure or intra-submap) to the pose graph.

SetInitialTrajectoryPose

virtual void SetInitialTrajectoryPose(
    int trajectory_id, const transform::Rigid3d& initial_pose,
    const common::Time time) = 0;

Sets the initial pose for a trajectory (used for deserialization or known starting pose).

FinishTrajectory

virtual void FinishTrajectory(int trajectory_id) = 0;

Marks trajectory as finished. Triggers final optimization.

GetTrajectoryNodes

virtual std::vector<NodeId> GetTrajectoryNodes(int trajectory_id) const = 0;

Returns all node IDs for a trajectory.

GetSubmapData

virtual std::map<SubmapId, SubmapData> GetSubmapData() const = 0;

Returns all submap data for visualization.

GetTrajectoryNodePoses

virtual std::map<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const = 0;

Returns all trajectory node poses (optimized and local).

GetLandmarkPoses

virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const = 0;

Returns landmark poses in the global map frame.

GetAllConstraints

virtual std::vector<Constraint> GetAllConstraints() const = 0;

Returns all constraints in the pose graph.

RunFinalOptimization

virtual void RunFinalOptimization() = 0;

Runs global optimization to convergence. Called automatically on FinishTrajectory.

SerializeState

virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0;

Serializes pose graph state (trajectories, submaps, nodes, constraints).

LoadState

virtual std::map<int, int> LoadState(
    io::ProtoStreamReaderInterface* reader, bool load_frozen_state) = 0;

Loads pose graph state. Returns trajectory ID remapping.

IsTrajectoryFinished

virtual bool IsTrajectoryFinished(int trajectory_id) const = 0;

Checks if a trajectory is marked finished.

GetTrajectoryStates

virtual std::map<int, TrajectoryState> GetTrajectoryStates() const = 0;

Returns state of all trajectories (ACTIVE, FINISHED, FROZEN, DELETED).

DeleteTrajectory

virtual void DeleteTrajectory(int trajectory_id) = 0;

Deletes a trajectory and all its data from the pose graph.

Implementation: PoseGraph

Located in pose_graph.h/.cc. Coordinates 2D (PoseGraph2D) and 3D (PoseGraph3D) implementations.

Key Components

  1. Constraint Builder — Detects loop closures using fast correlative scan matching, then refines with Ceres

  2. Optimization Problem — Ceres-based bundle adjustment (2D: OptimizationProblem2D, 3D: OptimizationProblem3D)

  3. Trajectory Connectivity — Tracks which trajectories are connected via constraints

  4. Submap Controller — Manages submap lifecycle (active, finished, trimmed)

Optimization

The pose graph optimizes:

  • Trajectory node poses (global frame)

  • Submap poses (global frame)

  • Landmark poses

  • IMU/extrinsic calibration (optional)

Cost functions:

  • SPA (Simultaneous Pose Adjustment) — Constraints between nodes and submaps

  • Acceleration — IMU-derived acceleration constraints

  • Rotation — Rotation-only constraints

  • Landmark — Landmark observation constraints

  • Fixed Frame — GPS/fixed frame constraints

  • Odometry — Odometry constraints

Configuration

From pose_graph.lua:

POSE_GRAPH = {
  optimize_every_n_nodes = 90,
  constraint_builder = { ... },    -- Loop closure detection
  optimization_problem = { ... },  -- Ceres solver config
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  max_num_final_iterations = 200,
  global_sampling_ratio = 0.003,
}

Usage

// Access via MapBuilder
auto* pose_graph = map_builder->pose_graph();

// Get optimized poses
auto node_poses = pose_graph->GetTrajectoryNodePoses();
for (const auto& [node_id, pose] : node_poses) {
  // pose.global_pose is the optimized pose
}

// Get constraints for visualization
auto constraints = pose_graph->GetAllConstraints();

// Run final optimization manually
pose_graph->RunFinalOptimization();

See Also