0
$\begingroup$

When doing fixed lag smoothing or Windowed Smoothing for visual odometry and map construction, how does marginaliztion differ from dropping past states? How does each work?

I assumed fixed lag smoothing always optimized over a fixed number of images using the most probable state state of the robot during the prior window but it looks like marginalization is different than this? Can someone explain?

$\endgroup$

2 Answers 2

5
$\begingroup$

Short answer: Marginalization is a fancy way of applying a prior on certain nodes of your factor graph.

Note that Marginalization vs Dropping data is not only specific to the sliding window case, but falls under the general category of how to remove data from a factor graph. To visualize the two approaches you can see in the image below. Circles are our robot poses, Stars are the landmarks, and purple marks the edges/constraints between them. In order to reduce our problem size we want to remove the nodes bounded by the green box. Methods of removing data from a factor graph

Dropping data is easy. You simply remove the nodes you are no longer interested in, and solve the smaller graph. However, this should intuitively seem off to you, because we are throwing away information. Those nodes we are removing do provide constraints on our reduced graph, so it would be cool if we could keep that information without the extra computation required for all the nodes. This is where marginalization comes in. It provides a way to summarize the removed nodes impact, and still constrain the remaining nodes. This is done by computing a marginalization node which constrains all of the nodes that were connected to the removed nodes.

Now how is this done?

  1. Dropping Data
  • For dropping data you just remove the nodes, and solve the graph containing just the nodes you are interested in.
  1. Marginalization
    • One of the disadvantages of Smoothing/Factor graphs is that our uncertainty is stored in the information form ($I=cov^{-1}$) the inverse of the covariance matrix. Now in the covariance form marginalization is easy. You can simply remove the covariance rows and columns that belong to the states you want to remove. In the information form it is more tricky.
    • Method 1: Convert our Information matrix $I$ to the covariance form ($I^{-1} = cov$) and remove the rows and columns of the states you want to remove. Then invert again to get back the Information form. Nobody uses this approach since it is very computationally expensive.
    • Method 2: Schur complement.
      • This takes advantage of you being able to setup the Information matrix as a block matrix composed of the blocks containing the variables you want to keep and the variables you want to remove. You can then use the Schur complement to remove the unwanted variables, and apply their affect on the kept variables. I know not the best answer, but there are a lot of good resources on how to do this online, and examples in code such as Okvis, and Vins Mono.

As for applying these approaches to fixed lag smoothing. As you have correctly deduced this approach works on a limited number of images, to keep the computation size bounded. Typically you use some sort of removal algorithm to pick nodes you want to remove. A very typical algorithm being removal of the oldest node first. When you have selected which nodes you want to remove you can pick between dropping or marginalization. With marginalization being a better approach because you are retaining some of the information. This leads to better results, because your remaining nodes are constrained in such a manner as if you had not removed those nodes.

Just some final thoughts/comments:

  • Marginalization is not as good, as retaining the nodes, since it fixes their estimates. Whereas if you had kept those nodes they would also improve over time.
  • A naive marginalization approach with just the Schur complement can introduce some consistency problems. You typically also have to fix the linearization points of all nodes connected to the Marginalization prior node.
  • This advantage of Marginalization being easy in Kalman filters, is one of the reason's they are sometimes used over Smoothing based approaches.

Math example

\begin{equation*} (J^T J) \hat{x} = J^T y \end{equation*}

The solution of our problem is gotten by solving the following linear equation. This is internally what our factor graph solves. $J$ is the Jacobian, $\hat{x}$ is what we are trying to solve for and $y$ is our observations/error. $J^T*J$ is called the information or Hessian matrix, and is equivalent to the inverse covariance matrix you know from Gaussian math.

This information matrix can be broken up into blocks that corresponds to the various nodes we are trying to solve. In the image below it breaks it up into the individual nodes. The block is only filled if there is a connection between nodes, else it is white or all 0s.Information matrix blocks and graph

We can group those blocks arbitrarily. For our purpose we are going to group it into 3 parts $$\begin{equation*} \begin{bmatrix} A & B & C \\ B^T & D & E \\ C^T & E^T & F \ \end{bmatrix} \begin{bmatrix} x_A \\ x_D \\ x_F \ \end{bmatrix} = \begin{bmatrix} y_A \\ y_D \\ y_F \ \end{bmatrix} \end{equation*}$$

$A$ are the variables we want to remove. $D$ are the variables we want to keep. $F$ will be the new variable we just added to our factor graph. $B,C,E$ are all the cross information terms. $x_X$ are the states we are solving for and $y_X$ are the observations. The subscript designates which variable set it is responsible for. For purpose of our marginalization we can remove $F$ from our matrix since it is not connected to the variables we want to remove. The $C$ part will just be all 0s. So we are left with the following matrix.

$$\begin{equation*} \begin{bmatrix} A & B \\ B^T & D \\ \end{bmatrix} \begin{bmatrix} x_A \\ x_D \\ \end{bmatrix} = \begin{bmatrix} y_A \\ y_D \\ \end{bmatrix} \end{equation*}$$

Now we marginalize away $A$ which will modify the part responsible for variables we are keeping $D$. We now do the schur complement on our block matrix. Which will result in a new information matrix that represents the constraints of $A$ but applied to the $D$ parts of our equation.

$$S = D - BA^{-1}B^T$$

We can use this now to solve a smaller version of the above problem.

$$\begin{equation*} \begin{bmatrix} D - BA^{-1}B^T \end{bmatrix} \begin{bmatrix} x_D \\ \end{bmatrix} = \begin{bmatrix} y_D -BA^{-1}*y_A \\ \end{bmatrix} \end{equation*}$$

Yes $A$ is still involved, but we are only solving for $x_D$ so the problem we are solving is smaller. We get to skip solving for $x_A$. If you check the dimensions of the problem it will be the size of the $D$ matrix, not $A+D$.

Now we can put $F$ back into our equation(our new state), but we will keep this new equation for $D$.

$$\begin{equation*} \begin{bmatrix} D - BA^{-1}B^T & E \\ E^T & F \\ \end{bmatrix} \begin{bmatrix} x_D \\ x_F \\ \end{bmatrix} = \begin{bmatrix} y_D -BA^{-1}*y_A \\ y_F \\ \end{bmatrix} \end{equation*}$$

And this is the setup of our system if we marginalize away the $A$ parts. ( I think the $E$ term also changes, but blanking on that right now)

You can also compare it to if we had dropped $A$ instead. Our linear system would just be $$\begin{equation*} \begin{bmatrix} D & E \\ E^T & F \\ \end{bmatrix} \begin{bmatrix} x_D \\ x_F \\ \end{bmatrix} = \begin{bmatrix} y_D \\ y_F \\ \end{bmatrix} \end{equation*}$$

which is different.

Extra notes:

  • You can add as many new terms to $F$ as needed as long as they don't touch any variables from $A$.
  • You can actually keep $F$ in the equation while we do marginalization. You will ended up with the exact same result, because $F$ and $A$ connections are all $0$.
  • By looking at these equations you can see some of the problems we mentioned. This marginalization is only valid for 1 iteration. After we solve the marginalized problem our Jacobians change, and our marginalization is no longer valid. For computation sake we accept that, since if we had to remarginalize every single iteration there is no point to it.

Bonus: Marginalization of the map.

Not related to the sliding window filter, but since you mentioned optimizing the map away in Graph SLAM decided to give you those equations to. This approach is very popular in solving Bundle adjustment problems because our map is so much bigger than our poses. E.g thousands of points vs hundreds of camera poses.

Let our system equations for a Bundle Adjustment be the following.

$$\begin{equation*} \begin{bmatrix} M & C \\ C^T & P \\ \end{bmatrix} \begin{bmatrix} x_M \\ x_P \\ \end{bmatrix} = \begin{bmatrix} y_M \\ y_P \\ \end{bmatrix} \end{equation*}$$

$M$ corresponds to the jacobians of the map(the landmarks we are trying to estimate), $P$ are our camera poses, and $C$ are the cross correlation terms.

Eliminate the map using the schur complement.

$$\begin{equation*} \begin{bmatrix} P - CM^{-1}C^T \end{bmatrix} \begin{bmatrix} x_P \\ \end{bmatrix} = \begin{bmatrix} y_P -CM^{-1}*y_M \\ \end{bmatrix} \end{equation*}$$

This is the equation to solve for our camera poses $x_P$. However, if we know our camera poses $x_P$ we can very easily figure out the locations of our landmarks. Intuitively you could think of the fact, that knowing camera poses means you can just triangulate the landmarks. That intuitive idea is actually already constrained in our Jacobians. So we can just back-substitute to solve for $x_M$. Equation is just done by some matrix math.

$$M*x_M = y_M-C^T*x_P $$

And now we have solved for both $x_P,x_M$. The reason why we do this is that

number of poses << number of landmarks

Solving a linear system is expensive so the smaller we can make it, then the better. And by doing this we only have to solve a linear system of size # camera poses rather than # of camera poses + # of landmarks. So it ends up being faster, even if we have to take more intermediate steps.

$\endgroup$
19
  • $\begingroup$ Could you explain the process of marginalization a bit more? Generally speaking the functions linking the nodes is nonlinear as you assume gaussian noise which is sent through some function. The way you talk about it implies everything is in a single giant multivariate gaussian. It's unclear how marginaliztion works in this case. $\endgroup$ Apr 28, 2023 at 2:54
  • $\begingroup$ It's funny that you bring up the single giant multivariate gaussian, because a quote from Sibley in the original sliding window paper is "SLAM is tracking a normal distribution through a large state space". So yes everything is just a single giant multivariate gaussian. Also the information matrix is composed of the Jacobians, which are linear approximations of the non linear functions. So you can treat it in a similar manner as linear system just like we do in an EKF. $\endgroup$
    – edwinem
    Apr 28, 2023 at 6:07
  • $\begingroup$ Now you do have a problem that you are not iteratively relinearizing those jacobians like we do in smoothing. That's a price we pay for saving on computation cost. One can make the argument that the older nodes we are removing have already seen all the needed information and were optimized enough that they are at their true state. In which case the Jacobian linearization would be exact. $\endgroup$
    – edwinem
    Apr 28, 2023 at 6:11
  • $\begingroup$ Thanks, could you explain a little bit more about the linearization process? Looking at the graph slam paper: robots.stanford.edu/papers/thrun.graphslam.pdf they talk about a form of marginalization where they marginalize out the map, but they can only do this using the taylor series expansion. During the sliding window process do you just optimize as much as possible, linearize then marginalize the past states/map once you reach convergence? It seems kind of sketchy if it would even be worth doing since the linearization process seems like it kills a lot of information.... $\endgroup$ Apr 29, 2023 at 6:38
  • 1
    $\begingroup$ You don't. In order to keep computation cost down you have to give up accuracy. $\endgroup$
    – edwinem
    May 2, 2023 at 5:29
1
$\begingroup$

I think sometimes it could also make sense to simply truncate the nodes that are no longer needed if the remaining poses are constraint enough through the remaining measurements. Because the to be dropped states/factors could still include linearization errors and/or wrong data association. If we use marginalization these errors would be kept in the graph, whereas if we simply truncate the nodes the errors are no longer present...

$\endgroup$
1

Your Answer

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

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