7
$\begingroup$

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...

$\endgroup$
3
  • 1
    $\begingroup$ Are you asking why the schur complement falls out of the marginalization of a MV Gaussian? Or are you asking why the same form appears in the bundle-adjustment type optimization? $\endgroup$ Jan 16, 2016 at 0:52
  • $\begingroup$ Basically the problem is with why and how it appears in BA. I edited the question to clarify. $\endgroup$
    – Ash
    Jan 16, 2016 at 9:15
  • $\begingroup$ Thanks for the clarification! I see now. I've updated my answer and hopefully others can come along and help out as well. At this point I think we've beat this to death. if you have further questions I suggest you submit a new question. $\endgroup$ Jan 16, 2016 at 16:51

2 Answers 2

4
$\begingroup$

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.
$\endgroup$
4
  • $\begingroup$ Thanks a lot for your answer! However, your link doesn't answer my question, To clarify, I edited the question and added two references to papers, that given your profile, you might already be familiar with :) $\endgroup$
    – Ash
    Jan 16, 2016 at 9:14
  • $\begingroup$ I've updated the answer. I apologize for the lack of links at this point. I could link my thesis but that's pretentious, so I'll try to find some original links (or maybe someone could drop some in). $\endgroup$ Jan 16, 2016 at 16:52
  • $\begingroup$ Btw, the part about an EKF doing only one optimization step is a bit surprising to me. Does it mean that if I start with the Kalman equations for state update, I can arrive at the same equation as in Gauss-Newton? $\endgroup$
    – Ash
    Jan 17, 2016 at 11:43
  • $\begingroup$ Yes, with one measurement and one update step, they better be the same. The difference arrives when you repeat the steps and, critically, re linearize the system. $\endgroup$ Jan 17, 2016 at 20:39
5
$\begingroup$

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 ;)

$\endgroup$
1
  • $\begingroup$ Thanks Partha Ghosh, but on robotics we are fortunate enough to have MathJax support enabled, so it would be far better to include these formulae as text rather than as a big image with no further explanation. Using MathJax you can easily create subscripts, superscripts, fractions, square roots, greek letters and more, allowing you to add both inline and block element mathematical expressions in robotics questions and answers. For a quick tutorial, take a look at How can I format mathematical expressions here, using MathJax? $\endgroup$
    – Mark Booth
    Feb 27, 2016 at 19:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.