Graph Optimization

The core abstraction of graph-based SLAM is the factor graph, a bipartite graph that factorizes a complex probability distribution. Nodes represent variables like robot poses or landmark positions. Edges, or factors, represent constraints derived from sensor measurements (e.g., odometry, lidar scans, loop closures). This representation cleanly separates the probabilistic model from the optimization algorithm, making the system modular and extensible. For example, adding a new sensor type simply requires defining a new factor that computes error based on its measurements.

The primary computational method for solving the graph is sparse nonlinear least squares. The goal is to find the configuration of node variables that minimizes the sum of squared errors from all factors. Crucially, the Hessian matrix (the matrix of second derivatives) of this problem is inherently sparse because each factor only connects a small subset of nodes. This sparsity enables the use of highly efficient solvers like g2o, GTSAM, or Ceres Solver, which employ Cholesky or QR factorization on the sparse matrix, allowing real-time optimization of maps with thousands of poses.

Graph optimization excels at handling loop closures—when a robot recognizes a previously visited location. This creates a new constraint in the graph between non-sequential poses. The optimization then distributes the resulting error across the entire trajectory, correcting the accumulated drift from odometry. Modern systems use incremental solvers (like iSAM2) that update the solution efficiently as new factors are added, rather than re-solving from scratch. This is critical for online SLAM where the map must be updated in real-time as the robot moves.

Real sensor data contains outliers (e.g., incorrect loop closures from perceptual aliasing). To maintain system integrity, graph optimization employs robust cost functions (or robust kernels) like Huber, Cauchy, or DCS. These functions reduce the influence of large residual errors during optimization, preventing a single bad measurement from corrupting the entire map. This is a form of M-estimation integrated directly into the least-squares framework. The choice of kernel is a key hyperparameter that balances sensitivity to outliers against convergence speed.

There are two main graph formulations. The pose graph is a simplification where landmarks are marginalized out; nodes represent only robot poses, and edges represent constraints between them. This drastically reduces the problem size and is standard for lidar SLAM. Full SLAM includes both pose and landmark nodes, preserving the full posterior distribution. It is more expressive and accurate but computationally heavier, often used in visual SLAM (e.g., ORB-SLAM) where re-observing visual features provides rich constraints.

A major advantage over filtering approaches (like EKF) is the direct access to the covariance matrix of the entire estimated state. After optimization, the marginal covariance for any node or set of nodes can be recovered from the inverted Hessian (the information matrix). This provides a principled measure of uncertainty for each estimated pose and landmark, essential for tasks like active exploration, path planning under uncertainty, and sensor fusion with other estimators. The information matrix's sparsity mirrors the graph's connectivity.

A factor graph is a bipartite graph used to represent the factorization of a complex probability distribution. In sensor fusion and SLAM, it provides a visual and computational structure where:

• Variable nodes represent unknown states (e.g., robot poses, landmark positions).
• Factor nodes represent probabilistic constraints derived from sensor measurements or motion models. This graphical representation directly translates into the nonlinear least squares problem solved in graph-based SLAM, making the optimization's sparsity pattern explicit for efficient computation.

Simultaneous Localization and Mapping is the computational problem of constructing a map of an unknown environment while simultaneously tracking an agent's location within it. Graph optimization, or graph-based SLAM, is a leading backend solution for this problem. It formulates the entire history of poses and landmarks as a graph, which is then optimized to find the most probable configuration given all sensor constraints. This approach is favored for its ability to perform loop closure—correcting accumulated drift by recognizing previously visited places.

Bayesian filtering is the overarching probabilistic framework for recursively estimating the state of a dynamic system from a sequence of noisy observations. It maintains a belief state (the posterior) that is updated as new data arrives. Key algorithms are special cases of this framework:

• Kalman Filter: Optimal for linear, Gaussian systems.
• Extended/Unscented Kalman Filter (EKF/UKF): Handle nonlinearities.
• Particle Filter: Handles non-Gaussian distributions via sampling. While filters are online (sequential), graph optimization is often batch or sliding-window, re-optimizing over a history of states for greater accuracy at higher computational cost.

State estimation is the process of inferring the hidden variables (the state) of a dynamic system from indirect, noisy sensor measurements. The state typically includes quantities like position, velocity, and orientation. This is the fundamental goal of sensor fusion. Graph optimization achieves state estimation by framing it as a maximum a posteriori (MAP) inference problem. It finds the set of states (e.g., all robot poses in a trajectory) that are most consistent with the entire set of sensor measurements, rather than just the most recent one.

Robust estimation refers to statistical methods designed to be insensitive to outliers—erroneous measurements that violate assumed noise models. In graph optimization for robotics, outliers are common due to sensor noise, incorrect data association, or dynamic objects. Techniques like:

• M-estimators (e.g., Huber, Cauchy loss) replace the quadratic cost function with one that grows more slowly for large residuals.
• RANSAC (Random Sample Consensus) identifies inlier sets. These methods are critical for preventing a few bad measurements from corrupting the entire optimized graph, ensuring system reliability in real-world, cluttered environments.

The covariance matrix is a square matrix representing the uncertainty (variance) of a state estimate and the correlations between its components. In graph optimization, the Hessian (information matrix) is inverted to solve the system and obtain the covariance, which quantifies the precision of the estimated poses and landmarks.

The Mahalanobis distance is a statistical distance measure that accounts for this covariance. It is used extensively in sensor fusion for:

• Gating: Determining if a new sensor measurement is plausible enough to be associated with an existing track.
• Outlier Rejection: Identifying measurements that are statistically improbable given the current state uncertainty.