Graph Slam Landmark remove and then again add it

Question

I try to implement Graph Slam in real dataset. My data set have some data that describe that the Robot observe same landmark over and over with a large amount of time difference. Prof. Sebastian Lectures(Udacity: https://classroom.udacity.com/courses/cs373) is not enough to understand this type of situation. So I study " The Graph Slam Algorithm with Application to Large - Scale MApping of Urban Structure" by Sebastian Thrun and Michael Montemerlo.(http://robots.stanford.edu/papers/thrun.graphslam.pdf) In this paper page number 414 at Table 4: Algorithm for Updating the Posterior mu" is written. I cannot understand this algorithm. First I remove all the landmark location then again I add it. But how could I add this? This is not clear to me. Also what is the need to first remove all landmarks then again add it? If any one know that please help me.

Answer 1

Let me start by saying that SLAM and other algorithms like that are beyond the scope of the work I've done. I am okay with the math, though, so I'll point out what I think is happening:

First, it appears that Table 3 on page 414 is not removing landmarks, it's collapsing them. Consider a hypothetical scenario where you have a bunch of measurements $x$ and $y$. If you want to do some operation on that data set, you have to carry those measurement vectors around everywhere and work on the entire set every time.

It might be easier, if the scenario you're in allows, to just use the average value of those sets. This appears to be what Table 3 is doing. It's important to notice the difference between the full set of data, $\Omega$ and $\xi$, and the reduced (averaged, conceptually) data $\tilde{\Omega}$ and $\tilde{\xi}$.

Second, it appears that there's a typo in Table 4. The equations given are:

$$ \begin{array}{c} 2: & \Sigma_{0:t} & = & \tilde{\Omega}^{-1} \\ 3: & \mu_{0:t} & = & \Sigma_{0:t}\tilde{\xi} \\ \end{array} $$

However, this is contrary to the statement on page 410 which says,

The posterior over the robot path is now recovered as $\tilde{\Sigma}=\tilde{\Omega}^{-1}$ and $\tilde{\mu}=\tilde{\Sigma}\xi$.

and additionally the equations (43) and (44) on page 417, which says:

The algorithm GraphSLAM_solve in Table 4 calculates the mean and variance of the Gaussian $\mathcal{N}(\tilde{\xi},\tilde{\Omega})$: $$ \begin{equation} \tilde{\Sigma} = \tilde{\Omega}^{-1} \\ \end{equation} \tag{43} $$ $$ \begin{equation} \tilde{\mu} = \tilde{\Sigma}\tilde{\xi} \end{equation} \tag{44} $$

So, in both locations (410 and 417), it is said that the equation given in Table 4 should be:

$$ \begin{array}{c} 2: & \tilde{\Sigma}_{0:t} & = & \tilde{\Omega}^{-1} \\ 3: & \tilde{\mu}_{0:t} & = & \Sigma_{0:t}\tilde{\xi} \\ \end{array} $$

This would then make line six of Table 4 make more sense:

$$ \begin{array}{c} 6: & \mu_j = \Omega^{-1}_{j,j}\left(\xi_j + \Omega_{j,\tau\left(j\right)}\tilde{\mu}_{\tau\left(j\right)}\right) \\ \end{array} $$

So, in summary:

1. Table 3 shows how to reduce the full information matrix $\Omega$ and full information vector $\xi$ to a reduced information matrix $\tilde{\Omega}$ and a reduced information vector $\tilde{\xi}$.
2. Table 4 takes the reduced information matrix $\tilde{\Omega}$ and reduced information vector $\tilde{\xi}$ and uses that to generate a reduced map esimate $\tilde{\mu}$.
3. Table 4 goes on to use the reduced map estimate $\tilde{\mu}$ and the full information matrix $\Omega$ and the full information vector $\xi$ to generate a full map estimate $\mu$.