SLAM : Why is marginalization the same as schur's complement?

Question

Consider the system $$ \tag 1 H\delta x=-g $$ where $H$ and $g$ are the Hessian and gradient of some cost function $f$ of the form $f(x)=e(x)^Te(x)$. The function $e(x)=z-\hat{z}(x)$ is an error function, $z$ is an observation (measurement) and $\hat{z}$ maps the estimated parameters to a measurement prediction.

This minimization is encountered in each iteration of many SLAM algorithms, e.g.one could think of $H$ as a bundle adjustment Hessian. Suppose $x=(x_1,x_2)^T$, and let $x_2$ be some variables that we seek to marginalize. Many authors claim that this marginalization is equivalent to solving a smaller liner system $M\delta x_1=-b$ where $M$ and $g$ are computed by applying Schur's complement to (1), i.e. if $$H= \begin{pmatrix} H_{11} & H_{12}\\ H_{21} & H_{22} \end{pmatrix} $$ then $$ M=H_{11}-H_{12}H_{22}^{-1}H_{21} $$ and $$ b=g_1-H_{12}H_{22}^{-1}g_2 $$

I fail to understand why that is equivalent to marginalization... I understand the concept of marginalization for a Gaussian, and I know that schur's complement appears in the marginalization if we use the canonical representation (using an information matrix), but I don't see the link with the linear system.

Edit: I understand how Schur's complement appears in the process of marginalizing or conditioning $p(a,b)$ with $a,b$ Gaussian variables, as in the link supplied by Josh Vander Hook. I had come to the same conclusions, but using the canonical notation: If we express the Gaussian $p(a,b)$ in canonical form, then $p(a)$ is gaussian and its information matrix is the Schur complement of the information matrix of $p(a,b)$, etc. Now the problem is that I don't understand how Schur's complement appears in marginalization in bundle adjustment (for reference, in these recent papers: c-klam (page 3 if you want to look) and in this (part titled marginalization). In these papers, a single bundle adjustment (BA) iteration is performed in a manner similar to what I initially described in the question. I feel like there is a simple connection between marginalizing a Gaussian and the marginalization in BA that I am missing. For example, one could say that optimizing $f$ (one iteration) is equivalent to drawing a random variable following a denstiy $$e^{-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)}$$ where $\Sigma$ is the inverse of the Hessian $H$ of $f$, and $\mu$ is the true value for $x$ (or an approximation of that value), and that marginalizing this density is equivalent to using Schur's compelement in the bundle? I am really confused...

Answer 1

See the walk-through

The Schur complement helps with the closed form derivation but isn't necessary. It's just a nice convenient property of Gaussians and the covariance matrices.

In these papers, a single bundle adjustment (BA) iteration is performed in a manner similar to what I initially described in the question.

The reason the marginal / schur complement appears over and over is that all of the objective functions, noise models, and (in some sense) optimization methods are the same.

All the problems attempt to find a vector $x$ which maximizes the likelihood of the observations $z=h(x)+\eta $ where $\eta$ is some Gaussian noise. $h(x)$ is often not linear, but we linearize it so that $h(x)\approx h(x)+H_x(a-x) $ where (sorry for abusing notation), $H$ is the Jacobian.

I feel like there is a simple connection between marginalizing a Gaussian and the marginalization in BA that I am missing.

Everyone and their mother assumes that the noise in the observation $z$ is Gaussian. If you construct the Gaussian noise terms, you'll find something like $e^{ (z-h(\hat{x}))^T\Sigma^{-1}(z-h(\hat{x}) )}$ (modulo the denominator).

However, this isn't the only formulation. We can also minimize the error (difference in observations and state estimate). A careful look reveals that maximizing the likelihood of the observations is equivalent to finding a vector $x$ which minimizes the error $\eta=z-h(x)$.

Thus, minimizing the mean squared error $(\eta^T\eta)$ is the same as finding $x$ which maximizes the likelihood of the observations $z$. Thus, minimizing mean squared error and maximizing Gaussian observation likelihood results in the same objective function.*

If we substitute our linear approximation, we need to minimize

$$J=(z-h(x)+H_x(a-x))^T(z-h(x)+H_x(a-x))$$

If you multiply that out, and minimize by taking the derivative, you get the solutions you showed above. This is the same across any ML estimation of a state vector given Gaussian noise. The difference in various estimation methods is summarized as follows, as best as I know.

• The EKF does only one optimization step, and uses only the last measurement and the current state approximation
• Other methods, like IteratedEKF use many iterations of the solution, but only use the most recent measurement
• If you can afford to use more than one measurement, you've arrived at a larger state (one copy of the state vector for each time step, and a stacked vector of observations). Doing this optimization over this larger state is what BA does, but is also known as Iterative Weighted Least Squares. More generally, these are batch estimators.
• What's nice in the computer vision world, is that a huge state vector and observation vector have independent components (because a camera cannot see all parts of the world at all time). This structure is exploited to find what is classically known as Bundle Adjustment.
• If you wish to discard the Gaussian noise assumption and evaluate a larger space of possible soutions, you can use sampling based methods. However, almost all sampling based methods I'm aware of sample $x$ (the state), and run one of the above estimators for each $x$ using the observations, then find the posterior probability that the sample you drew was correct.

For example, one could say that optimizing ff (one iteration) is equivalent to drawing a random variable following a density ...

Here you're a bit off. That's not correct that we're drawing samples, but it is correct that by trying to minimize error, we're actually maximizing likelihood of the Gaussian. This is how the error minimization objective of BA, the Gaussian noise model (and resulting appearance of a marginalization i.e., schur complement), and general slam frameworks are all related. They're the same, just found by different methods across different disciplines. Isn't research great?

• *Kinda. In this derivation it does, but we do assume that the observation noise is Gaussian to arrive there. It's a difference in starting point, not path.

Answer 2

The following are mostly based on "Factor Graphs for Robot Perception" by Frank Dellaert and Michael Kaess, with additional notes:

As a reminder, marginalization is about having a joint density $p(x, y)$ over two variables $x$ and $y$, and we would like to marginalize out or "eliminate a variable", lets say $y$ in this case:

\begin{equation} p(x) = \int_{y} p(x, y) \end{equation}

resulting in a density $p(x)$ over the remaining variable $x$.

Now, if the density was in covariance form with mean $\boldsymbol{\mu}$ and covariance $\mathbf{\Sigma}$, partitioned as follows:

\begin{equation} p(x, y) = \mathcal{N}( % Mean \begin{bmatrix} \boldsymbol\mu_{x} \\ \boldsymbol\mu_{y} \end{bmatrix}, % Covariance \begin{bmatrix} \mathbf\Sigma_{xx}, \mathbf\Sigma_{xy} \\ \mathbf\Sigma_{yx}, \mathbf\Sigma_{yy} \end{bmatrix} ) \end{equation}

marginalization is simple, as the corresponding sub-block $\mathbf{\Sigma}_{xx}$ already contains the covariance on $x$ after marginalizing out $y$, i.e.,

\begin{equation} p(x) = \mathcal{N}( % Mean \boldsymbol\mu_{x}, % Covariance \mathbf\Sigma_{xx} ) \end{equation}

However, in the nonlinear least squares formulation, we don't have the covariance $\mathbf{\Sigma}$, we can however estimate it via the following property:

\begin{equation} \mathbf{\Sigma} = \mathbf{H}^{-1} \end{equation}

where $\mathbf{H}$ is the hessian in a Gauss-Newton system (approximated with $\mathbf{J}^{T} \mathbf{J}$), it just so happens that when we're trying to invert the Hessian via Schur's complement, we are actually:

1. Inverting a sub-block of $\mathbf{H}$: to solve $\mathbf{H} \delta\mathbf{x} = \mathbf{b}$ for $\delta\mathbf{x}$
2. Performing marginalization to remove old states

both at the same time. The marginalized $\mathbf{H}$ (after Schur's complement) will be equivalent to $\mathbf{\Sigma}_{xx}^{-1}.$

Derivation of Schurs Complement in Gauss-Newton

From the Gauss-Newton system, $\mathbf{H} \delta\mathbf{x} = \mathbf{b}$, we can derive the marginalization of the old states $\mathbf{x}_{2}$ algebraically. Let us decompose the system as: \begin{equation} % H \begin{bmatrix} \mathbf{H}_{11} & \mathbf{H}_{12} \\ \mathbf{H}_{21} & \mathbf{H}_{22} \end{bmatrix} % x \begin{bmatrix} \delta\mathbf{x}_{1} \\ \delta\mathbf{x}_{2} \end{bmatrix} = % b \begin{bmatrix} \mathbf{b}_{1} \\ \mathbf{b}_{2} \end{bmatrix} \end{equation} If we multiply out the block matrices and vectors out we get: \begin{equation} % Line 1 \mathbf{H}_{11} \delta\mathbf{x}_{1} + \mathbf{H}_{12} \delta\mathbf{x}_{2} = \mathbf{b}_{1} \\ % Line 2 \mathbf{H}_{21} \delta\mathbf{x}_{1} + \mathbf{H}_{22} \delta\mathbf{x}_{2} = \mathbf{b}_{2} \end{equation} Now if we want to marginalize out the $\mathbf{x}_{2}$, we simply rearrange the second equation above to be w.r.t. $\mathbf{x}_{2}$ like so: \begin{align} % Line 1 \mathbf{H}_{21} \delta\mathbf{x}_{1} + \mathbf{H}_{22} \delta\mathbf{x}_{2} &= \mathbf{b}_{2} \\ % Line 2 \mathbf{H}_{22} \delta\mathbf{x}_{2} &= \mathbf{b}_{2} - \mathbf{H}_{21} \delta\mathbf{x}_{1} \\ % Line 3 \delta\mathbf{x}_{2} &= \mathbf{H}_{22}^{-1} \mathbf{b}_{2} - \mathbf{H}_{22}^{-1} \mathbf{H}_{21} \delta\mathbf{x}_{1} \\ \end{align} substitute our $\delta\mathbf{x}_{2}$ above back into $\mathbf{H}_{11} \delta\mathbf{x}_{1} + \mathbf{H}_{12} \delta\mathbf{x}_{2} = \mathbf{b}_{1}$, and rearrange the terms so it is w.r.t $\delta\mathbf{x}_{1}$ to get: \begin{align} % Line 1 \mathbf{H}_{11} \delta\mathbf{x}_{1} + \mathbf{H}_{12} (\mathbf{H}_{22}^{-1} \mathbf{b}_{2} - \mathbf{H}_{22}^{-1} \mathbf{H}_{21} \delta\mathbf{x}_{1}) &= \mathbf{b}_{1} \\ % Line 2 \mathbf{H}_{11} \delta\mathbf{x}_{1} + \mathbf{H}_{12} \mathbf{H}_{22}^{-1} \mathbf{b}_{2} - \mathbf{H}_{12} \mathbf{H}_{22}^{-1} \mathbf{H}_{21} \delta\mathbf{x}_{1} &= \mathbf{b}_{1} \\ % Line 3 (\mathbf{H}_{11} - \mathbf{H}_{12}\mathbf{H}_{22}^{-1}\mathbf{H}_{21}) \mathbf{x}_{1} &= \mathbf{b}_{1} - \mathbf{H}_{12} \mathbf{H}_{22}^{-1} \mathbf{b}_{2} \end{align} We end up with the Schur Complement of $\mathbf{H}_{22}$ in $\mathbf{H}$: \begin{align} \mathbf{H} / \mathbf{H}_{22} := \mathbf{H}_{11} - \mathbf{H}_{12}\mathbf{H}_{22}^{-1}\mathbf{H}_{21} \\ \mathbf{b} / \mathbf{b}_{2} := \mathbf{b}_{1} - \mathbf{H}_{12} \mathbf{H}_{22}^{-1} \mathbf{b}_{2} \end{align} If you want to marginalize out $\delta\mathbf{x}_{1}$ you can follow the same process above but w.r.t $\delta\mathbf{x}_{1}$, that is left as an exercise to the reader ;)