# Graph Slam Landmark remove and then again add it

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.

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$.
• Thank you sir. This is a good explanation. But I have a doubt. I have to find out path and map using Graph Slam Algorithm. At page 411 Table 2. Calculate the omega and Xi. Now if I applied the formula mu=omega.inv()*Xi I get Path and Map. So what is the need of reduction as per table 3 and again update mu as per table 4. Table 2. Itself cannot be able to produce desire output if I just add one more line mu=omega.inv().Xi? – Encipher Aug 1 '18 at 15:36
• @EllenaMori - The path covariance $\Sigma$ is the inverse of $\Omega$. The authors state in the abstract that this algorithm is capable of creating/handling maps with $10^8$ features. This seems to be possible because the reduction step removes the features from the data and keeps only the pose information; this is explained on page 413. If 100 pose updates is enough for the robot to detect 10,000 features, then you can either invert a matrix that has 100+10,000 variables, or you can reduce the matrix to just the 100 pose variables and invert that. This is the reason for the reduction. – Chuck Aug 1 '18 at 17:30
• Exactly sir here is my doubt. As per my understanding features means landmarks. My dataset consist of 15 landmarks so do I need the reduction step? 2nd As per my knowledge each timestamp of the odometer dataset represent one node of Graph. So if I have 75000 time stamp I should have 75000 node of graph. Problem arise here. Practically it is impossible to invert a 75000*75000 matrix. Do I have any mistake in my understanding? – Encipher Aug 1 '18 at 21:04
• @EllenaMori - I'm not sure what to say here. How do you have 75,000 poses but only 15 landmarks? If you're trying to learn the algorithm, I would suggest starting with a smaller data set first, to be sure you understand the algorithm, and then trying to proceed from there. There are resources online, such as OpenSLAM, that provide free, prewritten implementations of various SLAM algorithms. G2O is a graph SLAM algorithm that is one of the ones offered there. – Chuck Aug 2 '18 at 13:57