Factor Graph

Variable nodes represent the unknown random variables whose joint distribution the graph models. In robotics and state estimation, these typically correspond to quantities like:

• Robot poses at different timestamps (e.g., x, y, yaw).
• Landmark positions in a map (e.g., 3D coordinates).
• Sensor calibration parameters (e.g., camera intrinsics). Each node is connected to the factors that depend on it. The primary inference goal is to estimate the marginal or joint distribution over these variables.

Factor nodes represent the local functions, or factors, that encode probabilistic constraints and relationships between the connected variable nodes. Each factor is a non-negative function proportional to a conditional probability. Common examples in SLAM include:

• Motion factors (from odometry or an IMU) that connect consecutive robot poses.
• Observation factors (from cameras or LiDAR) that connect a robot pose to an observed landmark.
• Loop closure factors that impose a constraint between non-consecutive poses when a previously visited location is recognized.
• Prior factors that anchor the graph with an initial belief.

Edges in a factor graph are undirected connections that exist only between a factor node and a variable node. This strict bipartite property is fundamental. An edge indicates that the connected variable is an argument to that factor's function. For example:

• A binary factor connecting two variable nodes (e.g., a motion factor between pose X_t and pose X_{t+1}).
• A unary factor connecting to a single variable (e.g., a prior on the first pose).
• A higher-order factor connecting to three or more variables (e.g., a visual reprojection error factor connecting a camera pose and a 3D landmark point to a 2D image measurement).

The entire factor graph represents the factorization of a global function. In probabilistic inference, this is the joint probability distribution over all variables, expressed as a product of the local factors. Formally, for variables X and factors f_k, the joint distribution is: P(X) ∝ ∏_{k} f_k ( X_k ) where X_k is the subset of variables connected to factor f_k. This factorization turns the complex problem of estimating the full joint distribution into a more manageable problem of defining and optimizing over local relationships. The sparsity of the graph (each factor connects only to a few variables) is what enables efficient computation.

Solving a factor graph means performing probabilistic inference to estimate the variables. This is not a structural component but the computational process applied to the graph. Key algorithms include:

• Belief Propagation (BP) / Sum-Product Algorithm: Passes messages along edges to compute marginal distributions.
• Gaussian Belief Propagation: A specialized, highly efficient version for graphs where all factors are Gaussian, common in SLAM.
• Non-linear Optimization: For maximum a posteriori (MAP) estimation, the problem is often framed as minimizing the sum of negative log-likelihoods from each factor, leading to a non-linear least squares problem solvable via Gauss-Newton or Levenberg-Marquardt methods. Libraries like g2o and GTSAM implement this optimization over factor graphs.

Factor graphs generalize and clarify other graphical models used in robotics.

• Vs. Bayes Nets: A Bayesian network is a directed, acyclic graph representing conditional dependencies. Any Bayes net can be converted to a factor graph, but factor graphs are more explicit about the factorization.
• Vs. Markov Random Fields (MRFs): MRFs are undirected graphs where edges represent mutual influence. A factor graph can represent the same distribution, with each clique in the MRF becoming a factor node connected to all variables in that clique.
• Vs. Pose Graphs: A pose graph is a specific, simplified type of factor graph used in SLAM where variable nodes are only robot poses, and factors represent constraints between them (e.g., from odometry or loop closures). Landmarks, if present, are typically marginalized out.

A pose graph is a specialized, sparse type of factor graph used primarily in SLAM. Its nodes represent robot poses at discrete times, while its edges represent spatial constraints between those poses, derived from sensor measurements like odometry or loop closures. It is optimized to correct accumulated drift and produce a globally consistent trajectory and map. Unlike a full factor graph, a pose graph typically does not include landmark nodes, making it more computationally efficient for large-scale mapping.

Simultaneous Localization and Mapping (SLAM) is the foundational robotics problem where an agent must build a map of an unknown environment while simultaneously tracking its location within it. Factor graphs provide a highly efficient probabilistic graphical model for formulating the SLAM problem. They encode variables (robot poses, landmark positions) and factors (sensor measurements, motion constraints), allowing for scalable optimization via algorithms like Gauss-Newton or Levenberg-Marquardt to solve for the most likely map and trajectory.

Bundle adjustment is a classic optimization technique from computer vision and photogrammetry that is naturally expressed as a factor graph. It refines the 3D coordinates of scene geometry (landmarks) and the parameters (pose, calibration) of the cameras viewing them. Each observation of a landmark in a camera image creates a factor connecting a pose node and a landmark node. Optimizing this graph minimizes reprojection error, producing an optimal, globally consistent 3D reconstruction. It is a cornerstone of visual SLAM systems.

A Bayesian network is a broader class of probabilistic graphical model that represents a set of variables and their conditional dependencies via a directed acyclic graph (DAG). A factor graph is often used as a bipartite representation to facilitate computation on Bayesian networks and other models like Markov random fields. While Bayesian networks emphasize causal relationships and conditional probability tables, factor graphs explicitly represent the factorization of a global function into local functions (factors), making them ideal for message-passing algorithms like the sum-product algorithm.

In the context of probabilistic models, inference refers to the process of computing the posterior distribution over unknown variables (e.g., robot poses) given observed data (e.g., sensor measurements). Factor graphs enable efficient inference. Two primary types are:

• Marginal Inference: Computing the distribution over a single variable (e.g., the robot's current pose).
• Maximum a Posteriori (MAP) Inference: Finding the single most likely assignment to all variables (the standard goal in SLAM). Algorithms like belief propagation and non-linear least squares optimization perform inference on factor graphs.

Graph optimization is the computational framework used to solve the inference problem on factor graphs, especially for MAP estimation in robotics. The graph's factors define an error function; the goal is to find the variable values that minimize the sum of these squared errors. Libraries like g2o, GTSAM, and Ceres Solver implement efficient sparse linear algebra techniques (e.g., solving the linear system HΔx = b) to perform this optimization incrementally or in batch, enabling real-time state estimation for mobile robots and fleets.