Factor Graph

Represent the unknown quantities to be estimated. In SLAM, these are typically:

• Robot poses at different timestamps (e.g., position and orientation).
• Landmark positions (3D points or features in the map). Each node holds a belief (probability distribution) about its state, which is refined during optimization.

Encode constraints and measurements that connect variables. Each factor represents a local function in the factorization of the joint probability distribution.

• Odometry factors model motion between consecutive poses.
• Landmark observation factors connect a pose to a landmark based on a sensor measurement (e.g., a camera projection).
• Loop closure factors impose strong constraints when a place is re-recognized, correcting accumulated drift.
• Prior factors anchor the graph with absolute information (e.g., a known starting pose).

Edges connect factor nodes to the variable nodes they constrain. An edge exists only between a factor and a variable. This bipartite structure makes dependencies explicit:

• A unary factor connects to one variable (e.g., a prior).
• A binary factor connects to two variables (e.g., an odometry constraint between two poses).
• A higher-order factor can connect to multiple variables (e.g., a visual reprojection factor linking one landmark and two observing poses).

The entire factor graph represents the factorization of the system's posterior probability. If X is the set of all variables and Z is the set of all measurements/factors, the graph models: P(X | Z) ∝ ∏ᵢ fᵢ(Xᵢ) where each fᵢ is a factor function over a subset of variables Xᵢ. In SLAM, this is typically a Maximum A Posteriori (MAP) estimation problem: finding the variable values that maximize this product.

The factor graph structure is solved via non-linear least squares optimization. Under the assumption of Gaussian noise models, each factor corresponds to a squared error term in a cost function. The optimization (e.g., using Gauss-Newton or Levenberg-Marquardt algorithms) adjusts the variable node estimates to minimize the sum of these errors, reconciling all constraints. Libraries like g2o, GTSAM, and Ceres Solver are built to efficiently perform this inference on factor graphs.

A pose graph is a specific, simplified type of factor graph used in SLAM.

• Pose Graph: Variable nodes are only robot poses. Factors represent constraints between poses (odometry, loop closures). Landmarks are marginalized out.
• Full Factor Graph: Variable nodes include both poses and landmarks. Factors include direct observation constraints between poses and landmarks. Pose graphs are more compact; full factor graphs preserve more information but are larger. Modern graph SLAM systems often use a hybrid or sliding-window approach.

In modern Graph SLAM, the factor graph is the explicit data structure for the back-end optimization. It encodes the complete inference problem:

• Nodes (Variables): Represent unknown quantities—robot poses (x1, x2,...) and landmark positions (l1, l2,...).
• Factors (Constraints): Represent probabilistic measurements connecting nodes. Each factor defines a cost function based on sensor models.
• Examples: An odometry factor connects two consecutive pose nodes. A landmark observation factor connects a pose node and a landmark node. The goal is to find the variable assignments (the map and trajectory) that maximize the product of all factors, which is equivalent to minimizing the sum of negative log-likelihoods—a non-linear least squares problem.

iSAM2 (Incremental Smoothing and Mapping 2) is a canonical algorithm for real-time factor graph optimization in SLAM. Its key innovation is incremental and bayesian tree-based updating.

• Incremental Solving: Instead of re-solving the entire graph from scratch, iSAM2 efficiently updates the solution as new factors (measurements) are added, which is critical for online robotics.
• Elimination and the Bayes Tree: It uses variable elimination to transform the factor graph into a Bayes tree, a data structure that allows for efficient re-linearization and updates only to affected parts of the solution.
• Impact: This algorithm enabled practical, large-scale SLAM in real-time and is the optimization engine behind frameworks like GTSAM.

Factor graphs scale in complexity based on the problem formulation:

• Pose Graph: A simplified factor graph containing only pose variables. Edges are odometry and loop closure constraints. This is the core of pose graph SLAM after marginalizing out landmarks.
• Landmark-based Graph: Includes both pose and landmark variables, with factors for all observations. This is the full Bundle Adjustment problem.
• Dense Factors: Modern approaches use photometric error or point-to-plane ICP error as factors, creating a graph with a very large number of constraints (one per pixel or point). This moves from sparse feature-based to direct or dense SLAM formulations, where the factor graph principle remains the same but the computational challenge increases significantly.

Graph SLAM is an optimization-based approach to the Simultaneous Localization and Mapping problem. It formulates the entire history of robot poses and landmark observations as a graph:

• Nodes represent variables to estimate (robot poses, landmark positions).
• Edges represent constraints between nodes, derived from sensor measurements (odometry, loop closures). The goal is to find the configuration of nodes that maximizes the probability of all constraints, typically solved via non-linear least squares optimization. Factor graphs provide the explicit factorization of this probability distribution.

A pose graph is a simplified, but highly common, instance of a factor graph used in SLAM. It is a graph where:

• Nodes represent robot poses (position and orientation) at different times.
• Edges represent relative pose constraints between these nodes. These constraints come from odometry measurements between consecutive poses or from loop closure detections between non-consecutive poses. Optimizing the pose graph corrects accumulated drift and produces a globally consistent trajectory. It is a specialization where the factor graph's variables are exclusively poses.

Bundle Adjustment is a classic non-linear optimization technique in computer vision and photogrammetry that is naturally expressed as a factor graph. It jointly refines:

• The 3D coordinates of a set of scene points (landmarks).
• The camera poses from which those points were observed. The optimization minimizes the total reprojection error—the difference between the observed 2D image locations of features and their projected 3D positions. In a Visual SLAM factor graph, each camera pose-landmark observation forms a binary factor connecting a pose node and a landmark node.

In the SLAM pipeline, the back-end optimization is the process that takes the noisy measurements and constraints generated by the front-end (e.g., visual odometry, loop closure detection) and infers the most probable map and trajectory. The factor graph is the central model used in modern back-ends. Solving this graph involves:

• Linearization of non-linear measurement models (e.g., using a Jacobian).
• Constructing and solving a large, sparse system of linear equations, often via methods like Gauss-Newton or Levenberg-Marquardt. This process is what corrects errors and ensures global consistency.

Factor graphs are closely related to other probabilistic graphical models:

• A Bayesian network is a directed graphical model representing conditional dependencies. The factorization it represents can be converted into a factor graph.
• A Markov Random Field is an undirected graphical model representing local dependencies. Factor graphs provide a more explicit representation of the factorization in both cases. In SLAM, the underlying estimation problem has a natural probabilistic structure (e.g., P(poses, landmarks | measurements)), which these models describe. The factor graph is often the preferred representation for performing efficient inference via message passing or linear algebra.

iSAM is a seminal algorithm for efficient, incremental solution of factor graphs in SLAM. Instead of re-solving the entire optimization from scratch each time a new measurement (factor) is added, iSAM:

• Updates the solution by modifying only the parts of the factorization (e.g., QR, Cholesky) affected by the new data.
• Periodically performs variable reordering to maintain sparsity and efficiency.
• Enables real-time performance for large-scale, long-term SLAM. It is a prime example of an inference engine designed specifically for the sparse, incremental factor graphs generated by robotic state estimation.