Pose Graph

Nodes represent the robot's state (pose) at discrete points in time. Each node typically contains:

• A pose estimate (position and orientation in 2D or 3D).
• A unique timestamp or identifier.
• An associated covariance matrix representing the uncertainty of that estimate.

In practice, nodes are often created from keyframes—carefully selected sensor snapshots that provide sufficient information for mapping without storing redundant data.

Odometry edges connect consecutive pose nodes, modeling the robot's motion between them. They are derived from:

• Wheel encoders or inertial measurement units (IMUs) for proprioceptive sensing.
• Visual odometry or LiDAR odometry algorithms.

Each edge encodes a relative transformation (e.g., 'the robot moved 1 meter forward and rotated 0.1 radians') and a corresponding uncertainty matrix. These constraints form the backbone of the graph's initial structure but are prone to accumulating drift over time.

Loop closure edges are the critical mechanism for correcting accumulated drift. They are created when the system recognizes it has revisited a location (place recognition).

This process involves:

1. Detection: Identifying a current observation matches a past observation.
2. Constraint Creation: Computing the spatial relationship between the non-sequential poses and adding it as a new edge.

These 'long-range' edges introduce global constraints that pull the entire graph into consistency during optimization, drastically reducing overall error.

The back-end is the solver that adjusts all node poses to satisfy the constraints imposed by the edges. It formulates the pose graph as a non-linear least squares problem.

Key steps include:

• Constructing an error function for each edge based on the difference between the measured and predicted relative pose.
• Using algorithms like Levenberg-Marquardt or Gauss-Newton to minimize the sum of squared errors.
• The output is a globally consistent set of pose estimates for all nodes, correcting the drift inherent in the odometry edges.

Pose graphs come in two primary formulations:

• Pose-Landmark Graph: Includes landmark nodes (3D points or features) in addition to pose nodes. Edges connect poses to the landmarks they observed. This is the classic Bundle Adjustment formulation but can be computationally heavy.
• Pose-Only Graph: A simplified representation where landmarks are marginalized out. Only pose nodes remain, and edges represent constraints between poses, even if derived from common landmark observations. This is the standard Pose Graph Optimization (PGO) used in modern large-scale SLAM for its sparsity and efficiency.

A factor graph is a more explicit probabilistic graphical model equivalent to a pose graph. It clarifies the factorization of the joint probability distribution.

• Variable Nodes: Represent the unknown states (robot poses).
• Factor Nodes: Represent the probabilistic constraints (measurements) connecting variables.

Each factor (edge) defines a cost function. The optimization finds the variable values that maximize the product of all factors. Libraries like GTSAM and g2o use this representation, providing a flexible framework for incorporating diverse sensor measurements and prior knowledge.

A factor graph is a bipartite graphical model that explicitly represents the factorization of a probability distribution. In SLAM, it is the underlying mathematical representation of the optimization problem, where:

• Variable nodes represent unknown states (robot poses, landmark positions).
• Factor nodes represent probabilistic constraints (sensor measurements, prior information). The pose graph is a specific, simplified instance of a factor graph where nodes are only robot poses and edges are relative pose constraints, excluding explicit landmark nodes.

Back-end optimization is the computational process in a SLAM pipeline that refines the global map and trajectory estimate by solving a large-scale inference problem over all accumulated constraints. For a pose graph, this involves:

• Formulating a non-linear least-squares problem from all pose constraints.
• Solving this problem using optimizers like Levenberg-Marquardt or Gauss-Newton to find the set of poses that minimizes the total constraint error.
• Performing this optimization periodically or upon loop closure detection to correct accumulated drift and ensure global consistency.

Loop closure is the detection that a robot has returned to a previously visited location, followed by the addition of a new constraint to the pose graph. This process is critical for correcting unbounded drift.

• Detection: Often uses place recognition algorithms on visual or LiDAR data.
• Constraint Creation: A new edge is added between the current pose node and the historical pose node representing the revisited location.
• Graph Update: The new, long-range constraint triggers a back-end optimization, pulling the entire trajectory into global consistency.

Non-linear least squares (NLLS) optimization is the standard mathematical framework for solving the pose graph. The goal is to find the set of poses X* that minimizes the sum of squared errors between predicted and measured constraints: X* = argmin Σ || e_ij ||^2 where e_ij is the error function for the constraint between pose i and pose j.

• Solvers: Libraries like g2o, Ceres Solver, and GTSAM implement efficient NLLS solvers (e.g., Levenberg-Marquardt) specifically designed for the sparse structure of pose graphs.

Front-end processing is the real-time perceptual layer of a SLAM system responsible for constructing the pose graph from raw sensor data. Its outputs are the nodes and edges fed to the back-end. Key tasks include:

• Odometry Estimation: Using visual odometry or visual-inertial odometry (VIO) to create sequential edges between consecutive pose nodes.
• Feature Extraction & Matching: Identifying trackable features (e.g., ORB features) to support odometry and loop closure.
• Data Association: Determining which sensor observations correspond to the same world entity, a critical step for adding correct constraints.

Graph SLAM is an optimization-based paradigm for the Simultaneous Localization and Mapping problem. It contrasts with filtering approaches (e.g., EKF-SLAM) by maintaining all historical data and performing batch optimization.

• Core Idea: The entire history of poses and measurements is modeled as a graph.
• Pose Graph SLAM: A specific, highly efficient variant of Graph SLAM that marginalizes out landmark observations, leaving only pose nodes and constraints. This drastically reduces the problem size while maintaining accuracy, making it the dominant approach for large-scale mapping.