Uncertainty from the information matrix

"I gave the robot a pose. It asked, sensibly, how sure I was. I handed it a matrix, and only then did I realize I had been guessing in one direction the whole time."

A Self-Aware AI Agent

The Hessian you already computed is the inverse covariance

At the MAP solution the negative log-posterior is, to second order, a quadratic bowl, and its curvature is the information matrix. Recall from Section 11.2 that stacking every factor's whitened residual into \(r(x)\) and its Jacobian into \(J\) turns one Gauss-Newton step into the normal equations \(J^\top J\,\delta = -J^\top r\). The matrix on the left is exactly the (Gauss-Newton) Hessian, and it is the posterior information matrix

a sum of one term per factor, each measurement contributing its own slab of certainty. Under the Laplace approximation, the posterior over the whole state is Gaussian with covariance

This is the entire idea in one line: the covariance is the inverse of the information matrix, and the information matrix is the thing the solver factorizes anyway. A pose that many factors constrain has a large diagonal block in \(\Lambda\) and therefore a small block in \(\Sigma_x\): more evidence means more information means less variance. That duality, information adds while covariance shrinks, is the same one that makes the information form of the Kalman filter in Chapter 9 so convenient for fusing many sensors.

Recover marginal covariances without inverting the whole thing

Naively you might write \(\Sigma_x = \Lambda^{-1}\) and call a dense inverse. For a real SLAM problem with tens of thousands of variables this is both ruinously expensive (\(O(n^3)\)) and pointless: you rarely want the full \(n \times n\) covariance. You want a few marginal blocks, the \(6 \times 6\) covariance of the current pose, or the \(3 \times 3\) covariance of a landmark you are about to associate. The marginal covariance of variable \(i\) is the diagonal block \((\Lambda^{-1})_{ii}\), and cross-covariance between two variables is the off-diagonal block \((\Lambda^{-1})_{ij}\). Two subtleties matter. First, the marginal block of the inverse is not the inverse of the marginal block of \(\Lambda\); you cannot just invert the local diagonal block and get the right answer, because that ignores how the rest of the graph constrains the variable. Second, marginal covariance (uncertainty after accounting for everything else being unknown) differs from conditional covariance (uncertainty if everything else were fixed), and confusing the two silently produces overconfident error bars.

The efficient route uses the square-root factor \(R\) directly. Kaess and Dellaert's recursive formula computes selected entries of \(\Sigma_x = R^{-1}R^{-\top}\) starting from the bottom-right of \(R\) and working up, touching only the entries dictated by the sparsity of \(R\). Because a smoother keeps \(R\) sparse (this is exactly what variable ordering and the iSAM2 Bayes tree of Section 11.3 preserve), recovering the marginal for one variable costs work proportional to the fill of a few columns, not a full inverse. Listing 11.6 shows the exact relationship on a small dense problem so you can see the structure before trusting a library to exploit the sparsity.

import numpy as np

# Tiny pose graph: 3 variables, 2 dims each (e.g. planar landmarks/poses).
# Build the information matrix Lambda = J^T J from three stacked factors.
J = np.array([
    [ 1., 0., 0., 0., 0., 0.],   # prior on x0
    [ 0., 1., 0., 0., 0., 0.],
    [-1., 0., 1., 0., 0., 0.],   # relative x0 -> x1
    [ 0.,-1., 0., 1., 0., 0.],
    [ 0., 0.,-1., 0., 1., 0.],   # relative x1 -> x2
    [ 0., 0., 0.,-1., 0., 1.],
])
Lam = J.T @ J                       # information matrix (6x6, block-tridiagonal)

R = np.linalg.cholesky(Lam).T       # square-root information factor, Lam = R^T R
Sigma = np.linalg.inv(R) @ np.linalg.inv(R).T   # full covariance = R^-1 R^-T

# Marginal covariance of the LAST variable x2 = bottom-right 2x2 block.
marg_x2 = Sigma[4:6, 4:6]
# WRONG way: inverting only x2's own information block ignores the rest of the graph.
naive_x2 = np.linalg.inv(Lam[4:6, 4:6])
print("marginal cov of x2 (correct):\n", np.round(marg_x2, 3))
print("naive block-inverse (too confident):\n", np.round(naive_x2, 3))

Run Listing 11.6 and the two printed blocks disagree: the correct marginal is larger, because \(x_2\) sits at the end of a chain and inherits the accumulated uncertainty of \(x_0\) and \(x_1\), while the naive block-inverse pretends the earlier poses are known exactly. This is the single most common uncertainty bug in hand-rolled estimators, and it always fails in the dangerous direction, toward overconfidence.

from gtsam import Marginals
# graph and result (a Values) come from your optimizer, e.g. LevenbergMarquardt
marginals = Marginals(graph, result)
cov_pose_k = marginals.marginalCovariance(pose_key)          # 6x6 for SE(3)
joint = marginals.jointMarginalCovariance([key_i, key_j])    # includes cross-cov

Reading the matrix: ellipsoids, correlations, and directions the graph never saw

A covariance block is not just a set of standard deviations; its eigenstructure is a shape. The eigenvectors of a pose's \(\Sigma\) point along the axes of the \(1\sigma\) uncertainty ellipsoid, and the eigenvalues are the squared semi-axis lengths. A long, thin ellipsoid means the estimate is well pinned in some directions and loose in others, which is exactly what happens to a robot driving down a featureless corridor: it localizes across the corridor from the walls but slides freely along it, so one eigenvalue dwarfs the rest. Off-diagonal (cross-covariance) blocks tell you which variables move together; a strong correlation between two landmarks means observing one refines the other, information a naive per-variable error bar would miss entirely.

The extreme case is an eigenvalue that goes to zero: an unobservable direction. If nothing in the graph constrains some combination of states, \(\Lambda\) is rank-deficient (singular) in that direction and the covariance is formally infinite. In pure SLAM this is the gauge freedom: with only relative measurements the whole map can be translated and rotated at no cost, so the global pose is unobservable until you anchor it with a prior. A near-singular \(\Lambda\) also shows up as an exploding condition number, which is your early warning that the problem is under-constrained and any covariance you extract is unreliable. Checking the smallest eigenvalue of the information matrix before trusting its inverse is cheap insurance.

Are the error bars honest? Consistency before trust

A covariance is only useful if it is calibrated: a reported \(1\sigma\) should contain the truth about 68 percent of the time. Factor-graph covariances are systematically optimistic for two reasons. First, the Laplace approximation replaces the true posterior with a Gaussian fit at the mode, ignoring curvature and multimodality, so on strongly nonlinear problems (bearing-only landmarks, large rotations) the real spread exceeds the reported one. Second, wrong noise models feed \(\Lambda\) the wrong \(\Sigma_k\): if you tell the graph your odometry is twice as precise as it really is, every downstream covariance inherits that lie. The standard check is the normalized estimation error squared (NEES): on data where ground truth is known, compute \(\epsilon = (x - \hat{x})^\top \Sigma_x^{-1} (x - \hat{x})\) and confirm it follows a chi-square distribution with the right degrees of freedom. A NEES that runs high means the filter is overconfident (the usual failure); one that runs low means it is needlessly conservative. This consistency testing connects directly to the calibration and coverage machinery of Chapter 18, and it must be run on held-out trajectories, never the ones you tuned the noise model on, to avoid the leakage traps of Chapter 5.

What's Next

In Section 11.7, we pull the whole chapter together and answer the practical question it has been building toward: why modern SLAM and visual-inertial odometry systems abandoned the recursive filter for the factor-graph smoother. Loop closure, marginalization, robust costs, and the calibrated uncertainty of this section are not separate tricks; they are the reasons a graph-based back end became the default architecture for machines that must know both where they are and how sure they are.