Factor Graph

Factor nodes (typically squares) represent the local functions in the factorization. Each factor encodes a constraint or a measurement likelihood. For example:

• A binary factor might connect a robot pose and a landmark, representing the constraint from a lidar measurement.
• A unary factor connects to a single variable, often representing a prior belief (e.g., the robot's known starting location).
• The factor's mathematical form defines the error model, such as a Gaussian for a Kalman filter-like constraint or a robust cost function for outlier rejection.

Variable nodes (typically circles) represent the unknown random variables we wish to estimate. In robotics and SLAM, these are often:

• Robot poses (position and orientation) at different timestamps.
• Landmark positions (3D points in the environment).
• Sensor calibration parameters (e.g., camera intrinsics). Each variable node can be connected to multiple factor nodes, showing how different measurements inform the same unknown state. The graph structure visually reveals which variables are coupled by shared measurements.

Edges are undirected connections that define the bipartite structure of the graph. An edge exists only between a factor node and a variable node, never between two nodes of the same type. This bipartite property is fundamental. The connectivity pattern determines:

• Sparsity: Real-world problems yield sparse graphs, where each variable is connected to only a few factors, enabling efficient computation.
• The Markov Property: A variable is conditionally independent of all other variables given its immediate neighbors (the factors it connects to). This local structure is key to distributed inference algorithms like belief propagation.

The graph represents the factorization of a global probability distribution. The joint distribution over all variables (X) is proportional to the product of all factors: [ p(X) \propto \prod_{f \in \mathcal{F}} f(X_f) ] where (X_f) is the subset of variables connected to factor (f), and (\mathcal{F}) is the set of all factors. Each factor (f(X_f)) is a non-negative function, often derived from:

• A sensor model (likelihood): ( p(\text{measurement} \mid \text{state}) ).
• A process/motion model: ( p(\text{next state} \mid \text{current state}, \text{control}) ).
• A prior distribution on a variable.

The primary computational benefit of a factor graph is enabling efficient inference (estimating variable states) via message-passing algorithms. Beliefs about variables are refined by passing local messages along edges:

• Sum-Product Algorithm: Computes marginal distributions for each variable.
• Max-Sum Algorithm: Finds the most likely joint assignment (the MAP estimate). In robotics, this is often implemented as nonlinear least squares optimization (e.g., using Gauss-Newton). Each factor contributes a residual term to a large but sparse cost function, which is minimized to find the optimal variable values.

For real-time applications like SLAM, the factor graph is built incrementally. The iSAM2 (Incremental Smoothing and Mapping) algorithm is a landmark technique. It:

• Adds new variable and factor nodes as the robot moves and observes landmarks.
• Maintains a Bayes tree data structure for efficient re-linearization and variable re-ordering.
• Performs fluid relinearization only on the part of the graph affected by new measurements. This allows for constant-time incremental updates in typical scenarios, rather than re-solving the entire optimization from scratch, which is critical for long-term autonomy.

VIO systems fuse high-frequency inertial measurement unit (IMU) data with lower-frequency camera images to estimate precise, high-rate motion. A factor graph elegantly handles this multi-rate sensor fusion:

• IMU measurements between camera frames create pre-integrated factors that connect consecutive pose nodes, providing a motion prior.
• Visual observations of feature tracks create factors between the pose node where the feature was first seen and subsequent pose nodes where it was re-observed.
• The graph structure allows for efficient sliding window optimization, where only a recent history of poses is kept in the graph to bound computational cost. This is the core of state-of-the-art VIO systems like OKVIS and VINS-Fusion.

Factor graphs provide a clean formulation for the data association problem in tracking multiple objects. Here:

• Nodes represent both the states of objects (position, velocity) over time and assignment variables linking detections to tracks.
• Factors encode multiple constraints:
  • Motion factors between an object's state at time t and t+1 (e.g., a constant velocity model).
  • Measurement factors linking an object's state node to a detection node, if the assignment variable indicates a match.
  • Exclusivity factors that enforce that one detection is assigned to at most one track.
• Inference on this graph solves for the most probable set of trajectories and associations jointly, which is more robust than greedy sequential association methods.

Determining the spatial (extrinsic) and temporal relationships between sensors is a prerequisite for fusion. Factor graphs frame this as a joint optimization problem:

• Extrinsic Calibration: Nodes represent the rigid transformation (rotation and translation) between sensor coordinate frames. Factors are created from paired observations of the same physical feature (e.g., a checkerboard corner seen by both a camera and a lidar). The graph is solved to find the optimal transform.
• Temporal Synchronization: Nodes can represent clock offsets and drifts. Factors are created from timestamped events or measurements that should be coincident. This allows software correction of hardware synchronization errors.
• This approach enables online calibration, where calibration parameters are continuously refined during operation.

In distributed systems like sensor networks or multi-robot teams, a single central factor graph is impractical. Decentralized fusion uses a network of local factor graphs:

• Each robot or sensor node maintains its own local factor graph of its observations.
• When robots communicate, they exchange linearized factors or summarized constraints (e.g., using Hessian matrices).
• These transmitted constraints are incorporated into each local graph as virtual factors, allowing all nodes to converge toward a consensus on the global state estimate without sharing raw data.
• This architecture provides scalability, robustness to single-point failures, and preserves bandwidth by communicating only summarized information.

Real sensor data contains outliers (e.g., incorrect loop closures in SLAM). Factor graphs integrate robust cost functions to mitigate their effect:

• Standard factors use a quadratic cost (Gaussian noise assumption), which is highly sensitive to outliers.
• Robust kernels (e.g., Huber, Cauchy, DCS) replace the quadratic cost with a function that grows more slowly for large errors, effectively down-weighting outlier constraints during optimization.
• The graph structure makes it easy to attach fault detection factors or to use expectation-maximization to estimate the inlier probability of each factor. This results in systems that can automatically identify and reject erroneous GPS jumps, incorrect visual matches, or faulty lidar returns.

Bayesian filtering is the foundational probabilistic framework for recursively estimating the state of a dynamic system from a sequence of noisy observations. It provides the theoretical basis for algorithms implemented using factor graphs.

• Core Principle: Maintains a belief state (posterior distribution) that is updated in two steps: a prediction step (using a process model) and an update step (using a sensor model).
• Relation to Factor Graphs: Algorithms like the Kalman filter and particle filter are specific implementations of Bayesian filtering. A factor graph can represent the factorization of the joint posterior distribution over a sequence of states, making the recursive Bayesian update explicit through its graphical structure.

Graph optimization, often called graph-based SLAM, is a paradigm for solving large-scale estimation problems by formulating them as a graph to be optimized. This is the primary computational use case for factor graphs in robotics.

• Graph Structure: Nodes represent variables to estimate (e.g., robot poses, landmark positions). Edges represent constraints (factors) derived from sensor measurements or prior knowledge.
• Solving the Graph: The goal is to find the variable assignments that maximize the product of all factors (i.e., the probability of the measurements). This is typically solved via nonlinear least squares optimization (e.g., using Gauss-Newton or Levenberg-Marquardt algorithms).
• Key Advantage: Unlike filtering, it performs batch optimization over the entire history, allowing re-linearization and loop closure, which correct accumulated drift.

Simultaneous Localization and Mapping is the canonical robotics problem where an agent must build a map of an unknown environment while simultaneously tracking its location within it. Modern SLAM systems are predominantly built on factor graphs.

• Factor Graph Representation: Each robot pose and each mapped landmark is a variable node. Factors connect poses to landmarks (observation factors), consecutive poses (odometry factors), and poses to other poses upon loop closure.
• Efficiency: The sparsity of the factor graph (landmarks are only connected to poses that observed them) enables efficient optimization even for large-scale environments.
• Example: A robot using lidar creates factors between poses based on scan matching (ICP) and factors between poses and wall landmarks extracted from point clouds.

A Markov Random Field is an undirected graphical model that represents the conditional dependencies between random variables. Factor graphs are a generalization and often a more explicit representation of MRFs.

• Key Difference: An MRF uses clique potentials to represent factors. A factor graph explicitly separates the random variables (variable nodes) from the local functions (factor nodes), providing a clearer representation of the factorization.
• Conversion: Any MRF can be converted into a factor graph by creating a factor node for each clique potential and connecting it to all variable nodes in that clique.
• Application Context: MRFs are widely used in computer vision (e.g., image segmentation, stereo vision). The sum-product algorithm for inference on factor graphs is directly applicable to MRFs.

The sum-product algorithm is a fundamental message-passing algorithm for performing efficient inference on factor graphs, enabling the computation of marginal distributions for each variable.

• Message Passing: The algorithm operates by passing messages along the edges of the graph. A message from a variable to a factor is the product of messages from other factors. A message from a factor to a variable is the sum (or integral) over the product of the factor and incoming messages.
• Exact Inference: On tree-structured factor graphs, the sum-product algorithm yields exact marginals after two passes (forward and backward).
• Approximate Inference: On graphs with cycles (loopy graphs), running loopy belief propagation (iterative sum-product) often provides good approximate marginals, which is crucial for SLAM and sensor fusion problems.

These are the fundamental mathematical components that become the factors in a factor graph for dynamic estimation.

• Process Model (Motion Model): A factor that encodes the predicted evolution of the system state between time steps. It defines a probabilistic constraint between consecutive state variables, (p(\mathbf{x}t | \mathbf{x}{t-1}, \mathbf{u}_t)), where (\mathbf{u}) is a control input. For a robot, this could be a model based on wheel odometry or inertial data.
• Sensor Model (Measurement Model): A factor that encodes the likelihood of a sensor measurement given the state. It defines a probabilistic constraint between a state variable and a measurement, (p(\mathbf{z}_t | \mathbf{x}_t)). Examples include a camera projection model or a lidar range-bearing model.
• Role in the Graph: Together, these models create the chain of factors and variables that structure the estimation problem, with process models linking states over time and sensor models anchoring states to observable data.