# 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` ```cpp struct SubmapId { int trajectory_id; int submap_index; }; ``` Unique identifier for a submap within a trajectory. #### `NodeId` ```cpp struct NodeId { int trajectory_id; int node_index; }; ``` Unique identifier for a trajectory node (pose). #### `Constraint` ```cpp 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` ```cpp enum class ConstraintType { INTRA_SUBMAP = 0, // Local SLAM constraint within a submap INTER_SUBMAP = 1, // Loop closure constraint between submaps }; ``` ### Pure Virtual Methods #### `AddTrajectory` ```cpp virtual int AddTrajectory(const proto::TrajectoryBuilderOptionsWithSensorIds& options_with_sensor_ids) = 0; ``` Adds a new trajectory to the pose graph. Returns trajectory ID. #### `AddSubmap` ```cpp 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` ```cpp 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` ```cpp virtual void AddConstraint(const Constraint& constraint) = 0; ``` Adds a constraint (loop closure or intra-submap) to the pose graph. #### `SetInitialTrajectoryPose` ```cpp 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` ```cpp virtual void FinishTrajectory(int trajectory_id) = 0; ``` Marks trajectory as finished. Triggers final optimization. #### `GetTrajectoryNodes` ```cpp virtual std::vector GetTrajectoryNodes(int trajectory_id) const = 0; ``` Returns all node IDs for a trajectory. #### `GetSubmapData` ```cpp virtual std::map GetSubmapData() const = 0; ``` Returns all submap data for visualization. #### `GetTrajectoryNodePoses` ```cpp virtual std::map GetTrajectoryNodePoses() const = 0; ``` Returns all trajectory node poses (optimized and local). #### `GetLandmarkPoses` ```cpp virtual std::map GetLandmarkPoses() const = 0; ``` Returns landmark poses in the global map frame. #### `GetAllConstraints` ```cpp virtual std::vector GetAllConstraints() const = 0; ``` Returns all constraints in the pose graph. #### `RunFinalOptimization` ```cpp virtual void RunFinalOptimization() = 0; ``` Runs global optimization to convergence. Called automatically on `FinishTrajectory`. #### `SerializeState` ```cpp virtual void SerializeState(io::ProtoStreamWriterInterface* writer) = 0; ``` Serializes pose graph state (trajectories, submaps, nodes, constraints). #### `LoadState` ```cpp virtual std::map LoadState( io::ProtoStreamReaderInterface* reader, bool load_frozen_state) = 0; ``` Loads pose graph state. Returns trajectory ID remapping. #### `IsTrajectoryFinished` ```cpp virtual bool IsTrajectoryFinished(int trajectory_id) const = 0; ``` Checks if a trajectory is marked finished. #### `GetTrajectoryStates` ```cpp virtual std::map GetTrajectoryStates() const = 0; ``` Returns state of all trajectories (ACTIVE, FINISHED, FROZEN, DELETED). #### `DeleteTrajectory` ```cpp 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`: ```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 ```cpp // 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 - [MapBuilderBridge (cartographer_ros)](../../cartographer_ros/cartographer_ros/src/map_builder_bridge.cpp.md) — ROS wrapper for pose graph access - [Submap (cartographer_ros)](../../cartographer_ros/cartographer_ros/include/cartographer_ros/submap.h.md) — ROS submap conversion