Bundle Adjustment

Bundle adjustment simultaneously refines all unknown parameters in a reconstruction problem. This includes:

• Camera poses (position and orientation for each keyframe).
• 3D landmark points (the structure of the scene).
• Camera intrinsic parameters (e.g., focal length, distortion coefficients), if not pre-calibrated. By optimizing all variables together, it achieves a globally consistent solution that is superior to sequentially estimating poses and then structure.

The core objective function is the sum of squared reprojection errors. For each observed 2D feature point in an image, the error is the distance between its measured pixel location and where its corresponding estimated 3D landmark projects onto the image plane given the estimated camera pose. The optimizer (e.g., Levenberg-Marquardt) adjusts all parameters to minimize this total error. This geometrically corrects for noise in feature detection and initial pose estimates.

The optimization problem is inherently large but sparse. Each landmark is observed by only a few cameras, creating a sparse pattern in the underlying Hessian matrix. The Schur Complement trick is a critical computational technique that exploits this sparsity. It allows for efficient solution by first eliminating the 3D point variables, solving a much smaller system only for camera poses, and then recovering the points. This reduces complexity from O(n³) to a manageable level for thousands of cameras and millions of points.

In a SLAM system, bundle adjustment operates as the back-end optimizer. It takes constraints generated by the front-end (feature matching, odometry, loop closures) and performs global refinement.

• Local Bundle Adjustment runs frequently, optimizing a sliding window of recent keyframes and their landmarks to maintain local accuracy.
• Global Bundle Adjustment is triggered after a loop closure or periodically, optimizing all or a large subset of the map to correct accumulated drift and achieve global consistency.

The problem is formulated as a Non-Linear Least Squares (NLLS) optimization. The cost function F(x) is: F(x) = Σ || z_ij - proj(P_i, X_j) ||² Where:

• z_ij is the measured 2D observation.
• proj(P_i, X_j) projects landmark X_j using camera P_i.
• The sum is over all camera-landmark observation pairs. Solvers like Levenberg-Marquardt or Gauss-Newton iteratively linearize this function and solve a linear system to update the parameters until convergence.

Real sensor data contains outliers (incorrect feature matches). Standard least squares is highly sensitive to these. Therefore, robust loss functions (or M-estimators) are applied:

• Huber Loss: Lessens the influence of moderate outliers.
• Cauchy Loss: Strongly down-weights large residuals. These functions replace the squared error term, preventing a few bad matches from corrupting the entire optimization. This is often combined with pre-filtering using algorithms like RANSAC.

In a SLAM pipeline, back-end optimization is the computational process of refining the global map and trajectory estimates by solving a large-scale inference problem over all collected sensor constraints. It is the stage where bundle adjustment operates, taking initial estimates from the front-end and minimizing global error.

• Purpose: Achieve globally consistent maps and poses.
• Input: A graph of poses, landmarks, and noisy measurements (factors).
• Output: The optimal set of parameters that best explain all observations.
• Methods: Includes non-linear least squares solvers like Levenberg-Marquardt or Gauss-Newton, often applied to a factor graph representation.

A factor graph is a bipartite graphical model that represents the factorization of a complex probability distribution. In modern SLAM, it is the predominant mathematical structure for the back-end optimization problem, explicitly showing how state variables (poses, landmarks) are connected by factors (sensor measurements).

• Nodes: Represent random variables (e.g., robot pose at time t, 3D landmark position).
• Factors: Represent probabilistic constraints between variables (e.g., a visual feature observation creates a factor between a pose node and a landmark node).
• Optimization: The goal is to find the variable values that maximize the product of all factors, equivalent to minimizing the sum of squared reprojection errors in bundle adjustment.

Reprojection error is the fundamental cost metric minimized during bundle adjustment. It measures the discrepancy, typically in pixels, between where a 3D landmark is projected onto an image given the current camera pose estimate and where it was actually observed in that image.

• Calculation: For a landmark X observed in an image at pixel (u_obs, v_obs), the reprojection error is || (u_proj, v_proj) - (u_obs, v_obs) ||, where (u_proj, v_proj) is the projection of X using the estimated camera pose and intrinsic parameters.
• Role in BA: Bundle adjustment's objective function is the sum of squared reprojection errors across all landmarks and all camera observations. Minimizing this sum jointly refines both the 3D structure and the camera poses.

Non-linear least squares is the class of optimization algorithms used to solve the bundle adjustment problem. Since the relationship between camera poses, 3D points, and image observations is inherently non-linear (due to perspective projection), these iterative methods are required.

• Objective: Minimize a sum of squared residual functions: min Σ || r_i(θ) ||², where r_i is a residual (e.g., reprojection error) and θ are all parameters (poses and landmarks).
• Common Solvers: The Gauss-Newton and Levenberg-Marquardt algorithms are industry standards. They iteratively linearize the problem around the current estimate and solve a linear system (the normal equations) to compute an update step.
• Sparsity: The Jacobian matrix in bundle adjustment is highly sparse, a property exploited by solvers like SuiteSparse or Ceres Solver for computational efficiency.

Pose graph optimization is a simplified, but highly related, form of back-end optimization used in SLAM. Instead of jointly optimizing over both camera poses and 3D landmarks (like full bundle adjustment), it optimizes only the trajectory (pose graph) using relative constraints between poses.

• Graph Structure: Nodes are robot poses. Edges represent constraints between poses, derived from odometry, loop closures, or by marginalizing out landmarks from a full bundle adjustment.
• Comparison to BA: It is computationally lighter than full BA because the state dimension is smaller (no landmark parameters). It is often used for trajectory correction after loop closure, while full BA provides the highest-fidelity 3D reconstruction.
• Use Case: Essential for large-scale mapping where maintaining all landmarks in the optimization is prohibitive.

Front-end processing refers to the real-time perceptual tasks in a SLAM pipeline that provide the necessary inputs for bundle adjustment. It converts raw sensor data into the constraints and initial estimates that the back-end optimizes.

• Key Tasks: Includes feature extraction (e.g., detecting ORB features), data association (matching features across frames), initial pose estimation (often using Visual Odometry or a Particle Filter), and triangulation of new 3D landmarks.
• Role Relative to BA: The front-end produces the factor graph—the set of observed constraints between variables. Bundle adjustment (the back-end) then takes this graph and finds the optimal configuration. Poor front-end data (e.g., incorrect feature matches) will lead to a poor optimization result, regardless of the bundle adjustment solver's sophistication.