Pose Graph

A node represents the robot's estimated pose (position and orientation) at a discrete point in time. In 2D SLAM, this is typically (x, y, θ). In 3D, it's a 6-DoF pose (x, y, z, roll, pitch, yaw). Nodes are the unknown variables the graph optimization solves for. They are created from odometry estimates, often at keyframes to manage computational complexity.

An edge represents a probabilistic spatial constraint between two pose nodes, derived from sensor measurements. It encodes the relative transformation (rotation and translation) between the poses and the uncertainty of that measurement as a covariance matrix. Key types include:

• Odometry Edges: From wheel encoders or IMU integration.
• Loop Closure Edges: From recognizing a revisited location, providing a critical global constraint to correct drift.
• Scan Matching Edges: From aligning successive LiDAR scans (e.g., using ICP).

The pose graph is a specific instance of a factor graph, a bipartite graph that makes the underlying probabilistic model explicit. In this view:

• Variable Nodes: The robot poses.
• Factor Nodes: Represent the constraints (edges), each encoding an error function based on the measurement. The graph's total probability is the product of all factors. This representation is the direct input for optimization libraries like g2o or GTSAM.

This is the solver that performs graph optimization (often pose graph optimization). It adjusts all pose node estimates to maximize the consistency of all edge constraints, minimizing the total error. This is typically framed as a non-linear least squares problem. The result is a globally consistent, drift-corrected trajectory. The sparsity of the graph enables efficient solution even for large-scale environments.

While not part of the graph data structure itself, the frontend is the essential component that generates the edges. It processes raw sensor data (LiDAR, camera, odometry) to detect constraints. Its key responsibilities are:

• Keyframe Selection: Deciding when to create a new node.
• Data Association: Matching current sensor data to past data to generate loop closures.
• Relative Pose Estimation: Computing the transformation and covariance for each new edge.

In Heterogeneous Fleet Orchestration, pose graphs scale to multi-agent systems:

• Multi-Robot SLAM: Robots build individual or shared pose graphs, requiring inter-agent loop closures as edges between poses from different robots.
• Unified World Model: The optimized poses from each agent's graph feed into a central world model, providing the fleet state estimation needed for coordination.
• Factor Graphs extend pose graphs to include landmarks, creating a fuller environmental map used for long-term asset tracking.

In multi-robot SLAM, pose graphs enable teams of agents to build a unified, globally consistent map. Each robot maintains its own local pose graph.

• Inter-robot loop closures are detected when robots observe overlapping areas, creating new edges between their respective sub-graphs.
• These edges act as anchor points, allowing the separate graphs to be merged and optimized into a single, coherent representation.
• This application is critical for warehouse inventory robots, search-and-rescue teams, and autonomous underwater vehicle fleets, where a shared situational awareness is paramount.

For robots operating continuously over days or years, the pose graph enables lifelong mapping. The environment is not static; objects move, and structures change.

• The graph is not a fixed map but a living representation. New poses and constraints are continuously added.
• Outdated information can be marginalized or removed, while new loop closures correct the entire historical trajectory.
• This allows an autonomous system to maintain an accurate, up-to-date world model despite significant environmental dynamics, a key requirement for persistent service robots in hospitals, factories, or hotels.

Within Heterogeneous Fleet Orchestration, a centralized or distributed pose graph can act as a unified spatial reference frame for all agents.

• It integrates poses from diverse sources: autonomous mobile robots (AMRs) using LiDAR SLAM, manual vehicles with RTK-GPS, and static infrastructure sensors.
• The optimized graph provides a single source of truth for fleet state estimation, critical for collision avoidance systems, multi-agent path planning, and spatial-temporal scheduling.
• This ensures that all agents' perceived locations are aligned, preventing conflicts and enabling precise coordination in shared workspaces like shipping yards or manufacturing plants.

A factor graph is a bipartite graphical model that represents the factorization of a joint probability distribution. It is the underlying mathematical framework for a pose graph. In SLAM:

• Variable Nodes represent unknown states (e.g., robot poses, landmark positions).
• Factor Nodes represent probabilistic constraints (e.g., odometry measurements, loop closures).
• Pose Graph Optimization is performed by solving for the variable states that maximize the product of all factors, minimizing overall constraint error. This provides a sparse, efficient representation for large-scale inference.

Simultaneous Localization and Mapping (SLAM) is the computational problem of constructing a map of an unknown environment while simultaneously tracking an agent's location within it. The pose graph is a sparse backend for modern SLAM systems:

• Frontend processes raw sensor data (LiDAR, cameras) to detect features and propose constraints (odometry, loop closures).
• Backend takes these constraints and builds/optimizes the pose graph to produce a globally consistent map and trajectory.
• This separation allows for real-time operation, where the frontend runs at sensor rate and the backend performs periodic graph optimization.

Loop closure is the process of detecting that a robot has returned to a previously visited location, creating a spatial constraint between the current pose and a historical pose in the graph. This is critical for correcting accumulated drift.

• Detection uses sensor data (visual appearance, LiDAR scan matching) to recognize a place.
• Constraint Addition inserts a new edge into the pose graph connecting the two poses.
• Global Optimization triggers a re-optimization of the entire graph, distributing the correction across the entire trajectory, which can cause a discontinuous 'jump' in the estimated path.

Bundle adjustment is a photogrammetric optimization technique that refines a 3D reconstruction (structure) and the sensor poses that generated it (motion). It is closely related to pose graph optimization but operates on a denser representation.

• Pose Graph: Optimizes only the robot trajectory (pose nodes).
• Bundle Adjustment: Jointly optimizes both the camera poses and the 3D positions of all observed visual features (landmark nodes).
• While more accurate, bundle adjustment is computationally heavier. Modern systems often use a hybrid approach, with a pose graph for efficiency and local bundle adjustment for high-fidelity sub-maps.

An occupancy grid is a probabilistic, tessellated map representation where each cell stores the probability that it is occupied by an obstacle. It is a complementary representation to a pose graph.

• Pose Graph represents the spatial relationship between robot poses (the 'skeleton' of the trajectory).
• Occupancy Grid represents the geometry of the environment (the 'flesh' of the map).
• In a typical SLAM pipeline, the optimized poses from the pose graph are used to precisely align and integrate individual sensor scans (e.g., LiDAR point clouds) into a consistent occupancy grid map.

Multi-Robot SLAM extends the SLAM problem to a team of agents collaboratively building a shared map. Pose graphs are naturally extended to this domain.

• Each robot maintains its own local pose graph.
• Inter-robot loop closures are detected when robots encounter each other or recognize the same place, creating constraints between nodes in different robots' sub-graphs.
• These constraints fuse the local graphs into a single collaborative pose graph. Optimization then yields a globally consistent map and the trajectories of all robots within a unified coordinate frame, essential for heterogeneous fleet orchestration.