Factor Graph

A factor node (often drawn as a square) represents a local function in the global factorization. Each factor encodes a probabilistic constraint or measurement relating to a subset of variables. In robotics, common factors include:

• Prior Factor: Anchors a variable with an initial guess.
• Motion Model Factor: Connects consecutive robot poses based on odometry or IMU data.
• Measurement Factor: Connects a pose variable to a landmark variable based on a sensor observation (e.g., a camera projection).
• Loop Closure Factor: Adds a constraint between non-consecutive poses when a previously visited location is recognized. The product of all factor functions defines the joint probability distribution over all variables.

A variable node (often drawn as a circle) represents an unknown state to be estimated. In robotic state estimation, these are typically continuous random variables. Key examples are:

• Pose Variables: Represent the robot's position and orientation (e.g., SE(2) or SE(3)) at specific timesteps.
• Landmark Variables: Represent the 3D position of persistent features or objects in the environment.
• Velocity/Bias Variables: Often included when fusing inertial data. Each variable node is connected to every factor node that depends on it. The graph structure visually encodes the sparsity of the problem—a variable is only connected to the few factors that constrain it.

An edge in a factor graph is an undirected connection between a single factor node and a single variable node. It has no weight or direction of its own. Its sole purpose is to indicate that the connected factor's function depends on the state of the connected variable. The complete set of edges defines the factorization of the joint distribution. For example, a factor (f_k) connected to variables (x_1) and (x_2) means the global probability distribution factors as (... f_k(x_1, x_2) ...). This bipartite connectivity is what allows efficient inference via message-passing algorithms.

The entire factor graph represents the factorization of a joint probability distribution over all variables. Given a set of variable nodes (X) and factor nodes (F), the distribution is proportional to the product of all factors: [ p(X) \propto \prod_{f_k \in F} f_k(X_k) ] where (X_k) is the subset of variables connected to factor (f_k). In robotics, this is typically a posterior distribution (p(\text{State} | \text{Measurements})). Solving the estimation problem means finding the variable values that maximize this posterior (MAP estimation) or computing marginal distributions. The factorization into many small, local functions is what makes solving this large-scale problem computationally tractable.

Belief Propagation is a fundamental algorithm for performing inference on factor graphs. It works by passing messages along the edges between variable and factor nodes. A message represents a belief about the receiving node's state, informed by a part of the graph. The process involves:

• Variable-to-Factor Message: A variable sends a summary of beliefs from all other connected factors.
• Factor-to-Variable Message: A factor sends a message to a variable based on its own function and messages from all other connected variables. After messages converge, the belief (estimated marginal distribution) at each variable node is the product of all incoming messages. For linear-Gaussian models, this is equivalent to sparse linear solving; for non-linear problems, it's the foundation of algorithms like iSAM2.

Factor graphs are a more general and explicit representation than other graphical models:

• vs. Bayesian Networks: Bayes nets are directed, acyclic graphs representing causal relationships and conditional dependencies. A Bayes net can be converted to a factor graph where each conditional probability table becomes a factor. Factor graphs are often preferred for inference because they make the factorization for message-passing explicit.
• vs. Markov Random Fields (MRFs): MRFs are undirected graphs where edges represent direct probabilistic interactions between variables. An MRF's clique potentials correspond directly to factors in a factor graph. The factor graph provides a finer-grained view, breaking down clique potentials into singular functions and explicitly showing the bipartite structure crucial for algorithm derivation. This universality makes the factor graph a unifying language for probabilistic inference across many domains.

Factor graphs are the de facto standard representation for modern Simultaneous Localization and Mapping (SLAM) systems. They elegantly model the problem by connecting pose variables (robot positions) to landmark variables (map points) via observation factors. This structure allows for efficient incremental optimization as the robot explores, making systems like iSAM2 possible. The graph explicitly shows loop closure constraints, which are crucial for correcting accumulated odometry drift.

Factor graphs provide a unified probabilistic framework for tightly-coupled sensor fusion. Different sensor modalities contribute distinct factors to the same graph:

• IMU Preintegration Factors from inertial measurements.
• Visual Reprojection Factors from camera observations.
• LiDAR Scan Matching Factors from point cloud alignment.
• GPS Prior Factors for global anchoring. By adding all constraints to a single graph, the maximum a posteriori (MAP) estimate is found by minimizing the sum of all factor errors, optimally combining heterogeneous data.

The power of factor graphs lies in their sparsity. Each factor typically connects only to a small subset of variables (e.g., a camera observation connects only to the pose from which it was taken and the landmarks it sees). This results in a sparse Jacobian and Hessian matrix during nonlinear optimization (like Gauss-Newton or Levenberg-Marquardt). Solvers exploit this sparsity using Cholesky or QR factorization on a Bayes tree or clique tree, enabling real-time optimization for thousands of variables.

Beyond single-session SLAM, factor graphs enable lifelong and dynamic mapping. The graph can be extended over multiple sessions, with new factors added for re-localization. For dynamic environments, semantic labels can be attached to variables, and parts of the graph can be marginalized or updated. This makes them suitable for long-term autonomy in changing warehouses or cities, where the map must be continuously refined without starting from scratch.

While classic pose graphs are a subset, full factor graphs can model richer problems:

• Bundle Adjustment: Dense structure-from-motion by connecting every camera pose to many 3D point variables.
• Calibration: Intrinsic and extrinsic sensor parameters can be included as variables optimized jointly with the state.
• Semantic SLAM: Adding object-level landmarks with shape and class attributes, connected via detection factors.
• Multi-Robot SLAM: Merging graphs from different agents by identifying shared landmark variables, enabling collaborative mapping.

The factor graph representation directly exposes connections to fundamental inference algorithms:

• The sum-product algorithm for marginalization (computing beliefs over variables).
• The max-product algorithm for finding the MAP estimate.
• Bayesian networks and Markov random fields can be converted to factor graphs, providing a common language. In robotics, optimization dominates, but this theoretical foundation links SLAM back to general probabilistic graphical models, showing it as a specific instance of structured probabilistic inference.

This card illustrates the core conceptual mapping. In a robotics Simultaneous Localization and Mapping (SLAM) context, the factor graph elegantly structures the problem:

• Variable Nodes: Represent unknown robot poses (x1, x2, x3) and landmark positions (l1, l2).
• Factor Nodes: Encode probabilistic constraints between variables.
  • Odometry Factors: Connect sequential robot poses (e.g., between x1 and x2), based on wheel encoder or IMU data.
  • Measurement Factors: Connect a robot pose to a landmark (e.g., between x2 and l1), based on camera or LiDAR observations.
  • Loop Closure Factor: A special measurement factor that connects non-sequential poses (e.g., x3 back to x1), correcting accumulated drift.

Solving the graph via nonlinear least-squares optimization (e.g., Levenberg-Marquardt) yields the most probable configuration of all poses and landmarks.

For real-time operation, robots cannot re-optimize the entire history every time a new measurement arrives. Incremental inference is key. Libraries like GTSAM use a Bayes tree—a data structure derived from the factor graph's chordal Bayes net—to enable efficient updates.

• Process: When a new factor is added (e.g., a loop closure), only the affected part of the Bayes tree needs to be re-eliminated and re-solved.
• Benefit: Computational complexity scales with the size of the update, not the size of the entire map, enabling lifelong operation.
• Contrast: Batch optimization (as often used with Ceres) solves the entire problem from scratch each time, which becomes intractable for long trajectories.

Factor graph libraries typically perform smoothing, which differs fundamentally from the filtering approach of Kalman filters. This distinction is crucial for understanding their application.

• Filtering (e.g., EKF, UKF): Estimates only the current state x_t given all measurements up to time t. It marginalizes out past states, discarding information.
• Smoothing (e.g., GTSAM): Estimates the entire trajectory {x_1, x_2, ..., x_t} given all measurements. It retains the full history, leading to higher accuracy.

Why Smoothing for SLAM? Loop closures provide information about the past. A filter must somehow update old states, which is cumbersome. A smoother naturally re-adjusts the entire consistent history when a loop closure is detected, providing globally consistent maps.

Bayesian filtering is the foundational probabilistic framework for recursively estimating the state of a dynamic system using Bayes' theorem. It provides the mathematical underpinning for factor graphs, which can be seen as a batch optimization formulation of the same estimation problem.

• Core Principle: Maintains a belief state (probability distribution) over the system's state, updating it with each new sensor measurement.
• Relationship to Factor Graphs: While filters like the Kalman Filter process data sequentially, a factor graph constructed from all measurements represents the full joint posterior probability, enabling global optimization after data collection.
• Key Assumptions: Often assumes Markov property (the future state depends only on the present).

Maximum A Posteriori (MAP) estimation is the optimization objective solved by inference on a factor graph. It seeks the single most probable state configuration given all observed data and prior knowledge.

• Mathematical Form: argmax P(State | Measurements). This is equivalent to minimizing the negative log of the posterior, which factor graphs explicitly represent as a sum of squared error terms.
• Role in Factor Graphs: Each factor encodes a log-likelihood or log-prior. The sum of these factors is the negative log-posterior. Solving the factor graph means performing MAP inference to find the state values that minimize this sum.
• Contrast with MMSE: Unlike Minimum Mean Square Error estimation, which computes the full posterior mean, MAP finds the mode (peak) of the distribution.

Bundle adjustment is a classic computer vision and photogrammetry problem that is a canonical application of factor graphs. It is the joint nonlinear optimization of camera poses and 3D landmark positions to minimize reprojection error.

• Factor Graph Representation: Each camera pose and 3D point is a variable node. Each observed 2D image feature generates a binary factor connecting a camera pose variable and a landmark variable, based on the camera's projection model.
• Scale: Modern systems like large-scale Structure-from-Motion (SfM) create factor graphs with millions of variables and tens of millions of factors.
• Solvers: Efficiently solved using sparse linear algebra (e.g., Cholesky decomposition on the Hessian) due to the graph's sparsity, which is explicitly leveraged by frameworks like g2o and GTSAM.

Factor graphs are the dominant back-end representation for modern SLAM systems. They provide a sparse, efficient way to model the complex web of constraints between robot poses and mapped landmarks.

• Graph Structure: Variables represent robot poses at different times and positions of landmarks in the map. Factors represent odometry constraints (between consecutive poses), landmark observation constraints, and loop closure constraints.
• Incremental Solving: As a robot moves, new variables and factors are added. Incremental inference algorithms (like iSAM2) efficiently update the MAP estimate without re-solving the entire graph.
• Advantages: This formulation naturally handles the non-convexity of loop closures and allows for global consistency after detecting revisited areas.

A Markov Random Field (MRF) is an undirected probabilistic graphical model that represents the conditional dependencies between random variables. Factor graphs are a more general representation that can encode MRFs.

• Relationship: Any MRF can be converted into a factor graph where each clique (fully connected subset of variables) in the MRF becomes a factor connecting those same variable nodes.
• Key Difference: MRFs emphasize conditional independence (the Markov property), while factor graphs emphasize the factorization of the global function. This makes factor graphs often more explicit and intuitive for representing sensor models and priors in estimation problems.
• Application Context: MRFs are heavily used in computer vision (e.g., image segmentation, stereo vision), problems which can also be framed within the factor graph paradigm for inference.

When all factors in a graph are Gaussian, the resulting system is a Gaussian Factor Graph. This special case is exceptionally important because it leads to a quadratic optimization problem solvable with linear algebra.

• Mathematical Form: Each Gaussian factor corresponds to a squared Mahalanobis distance error term: ||h(x) - z||²_Σ. The total negative log-posterior is a sum of such quadratic terms.
• Solution: The MAP estimate is found by solving a linear system H * dx = b, where H is the sparse information matrix (Hessian) of the system. The sparsity pattern of H is exactly the adjacency matrix of the factor graph.
• Efficiency: The Cholesky factorization of H directly corresponds to variable elimination on the factor graph, and its square root factor reveals the Bayes tree, a data structure used for efficient incremental updates in SLAM.