# GraphSLAM equation doubt

I have question about GraphSLAM implementation.

To find out the path and map using GraphSLAM we rely on this equation:

$$\mu=\Omega^{-1}\xi$$

where $\Omega$ is our information matrix which determine the link between two nodes of graph and link between a landmark and a node. The $\xi$ vector give us the value of constraint between two consecutive robot pose or a robot pose and landmark.

I want to clarify that why a matrix inversion multiplied by a vector give us path and map. What is the mathematical logic, algorithm or assumption behind this?

Is it related to Floyds' shortest path algorithm or any other graph based algorithm related to it?

For reference I studied The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures and listened to the lecture series of Udacity.

However, I cannot understand where this equation came from. Also, I have a little bit of doubt about implementing GraphSLAM in a real dataset for the huge dimension on $\Omega$ Matrix and its inversion. Is it possible for a 75000 dataset?

• You should re read chapter 3 of Thrun's book, Probabilistic Robotics. In that chapter, the canonical parameterization that you keep posting questions about is introduced there. – koverman47 Aug 1 '18 at 19:09
• Did you talking about Gaussian Filter and within that 3.4 Information Filter and within that 3.4.1 Canonical Representation? – Encipher Aug 1 '18 at 21:08
• Yes, section 3.4 – koverman47 Aug 2 '18 at 3:24

1) I believe the previous replies/comments already directed you to good online resources. I will just add some comments about the intuition behind the equations.

Putting aside SLAM, in general mathematics, I assume you have an understanding of what the covariance matrix $\Sigma$ represents for a multivariate Gaussian. Now the information matrix is defined as its inverse: $$\Omega = \Sigma^{-1}$$ and the information vector is defined as: $$\xi = \Sigma^{-1} \mu$$ Note that these are mathematical definitions, you don't have to worry about the meaning of these quantities just yet. From these definitions, you obviously get: $$\Sigma = \Omega^{-1}$$ $$\mu = \Omega^{-1} \xi$$

Now what do these quantities represent in GraphSLAM?

Remember that the objective of the GraphSLAM algorithm is to compute a Gaussian approximation of the posterior over the entire robot trajectory and the map. This gaussian approximation, like all gaussian distributions, has a mean $\mu$ and a covariance matrix $\Sigma$, which means that you can also define its information matrix $\Omega$ and information vector $\xi$ using the definitions above. If you have $\Omega$ and $\xi$ you can get $\Sigma$ and $\mu$ (and vice-versa), it's as simple as that.

The real question is, why would you bother with the information form ($\Omega$ and $\xi$) instead of trying to find the mean $\mu$ and the covariance $\Sigma$ of your posterior directly?

Thrun answered this question previously, in 2002, when he published another algorithm, the Sparse Extended Information Filters (SEIFs). SEIFs was a variation of the Extended Information Filter (EIF), the dual of the very popular Extended Kalman Filter (EKF). The SEIFs paper showed that sparsity in the information form of the SLAM problem: 1) had a 'physical' meaning as a graphical network of locally-interconnected features (which is actually a Gaussian Markov Random Field), and 2) enabled us to use well-known, highly efficient algorithms from linear algebra for matrix factorization.

I highly suggest that you check out the SEIFs paper - or section 3.4 of Probabilistic Robotics as others have suggested - if you want to understand the information form of SLAM.

Now GraphSLAM (2006) is very closely related to SEIFs, but it introduces several new things: 1) it is an offline algorithm (estimates entire trajectory + map) instead of an online algorithm/'filter' (estimates current trajectory + map), 2) after linearizing constraints, it uses a factorization trick (see Fig. 3 in the paper) to shift information between poses and features to information between pairs of poses. This gives you a new structure constraining poses only, making it easier to compute them. Once they are computed, the feature location are computed too, based on the original feature-to-pose information.

Regardless of the specificities of GraphSLAM, if you understand the information form of SLAM from SEIFs and you understand that GraphSLAM is offline instead of online, you should be able to understand $\mu = \Omega^{-1} \xi$.

2) Now on to your 2nd question regarding the dataset. By '75000 dataset' I suppose you mean a dataset of 75000 landmarks. By asking if it is 'possible' I am not sure whether your are concerned about the accuracy of the map, the computational complexity, or both? Inverting a large $\Sigma$ matrix is what initially deterred people from investigating the EIF-route, until they figured out the inherent sparsity of the SLAM problem. As I explained above, this sparsity allows for efficient matrix factorization. Section 7 of the GraphSLAM paper has some quantified results that should answer your question. If not, please clarify.

• 75000 dataset doesnot mean 75000 landmarks. As per my dataset there is only 15 landmarks.This landmarks are observed at various timesteps. So I have a measurement file with 5000 data. 75000 data indicate the odometer data. In my odometer data file ,the file consist of time,forward velocity and angular velocity. As per my understanding this 75000 odometer dataset each refer one of the pose of a Robot. So inversion of a matrix with 75000 data is impossible. – Encipher Aug 26 '18 at 0:47
• The point is that you don't need to invert the matrix, you use efficient matrix factorization taking advantage of the form of the matrix. The complexity of the algorithm is explained in details in section 3.4, and it ultimately depends on the topology of the 'world' being mapped. So I don't have a figure to give you for your dataset, but I can tell you that you won't be inverting a 75000x75000 matrix if you follow GraphSLAM. – al-dev Aug 26 '18 at 9:15
• Thank you sir for your explanation. But I still unable to understand why I should not invert a 75000*75000 matrix?I am using UTIAS Multi-Robot Cooperative Localization and Mapping Dataset(asrl.utias.utoronto.ca/datasets/mrclam). – Encipher Aug 31 '18 at 17:49
• As per SEIF slam. omega=Sigma.inverse(); And mu=Omega.inverse().Xi. So from induction can we say that mu=Sigma.Xi? – Encipher Sep 3 '18 at 19:31
• The initialization of $\Omega$ (and $\xi$) is explained very clearly in section "4. The GraphSLAM Algorithm" of the GraphSLAM paper: 1. generate an initial estimate for the mean poses vector $\mu_{0:t}$ using 'GraphSLAM_initialize'. "It initializes the first pose by zero, and then calculates subsequent poses by recursively applying the velocity motion model. " 2. Construct $\Omega$ and $\xi$ through linearization in 'GraphSLAM_linearize'. To understand the linearization, see "5. Mathematical Derivation of GraphSLAM". If you have further questions, I suggest you open a new thread. – al-dev Sep 4 '18 at 22:11

As I mentioned previously, this isn't my topic of expertise, but I'll again point out the passage on page 417 of the Thrun paper:

The algorithm GraphSLAM_solve in Table 4 calculates the mean and variance of the Gaussian $\mathcal{N}\left(\tilde{\xi},\tilde{\Omega}\right)$:

$$$$\tilde{\Sigma} = \tilde{\Omega}^{-1} \\$$ \tag{43}$$ $$$$\tilde{\mu} = \tilde{\Sigma}\tilde{\xi}$$ \tag{44}$$

In particular, this operation provides us with the mean of the posterior on the robot path; it does not give us the locations of the features in the map.

(emphasis added on that last bit). Bear in mind also that a mathematical map and a human-readable map are two different things entirely.

Here is a link to a video that appears to be by Sebastian Thrun. The video is published by Udacity and links to their CS 271 course. This is where I would probably go to learn more about the Graph SLAM algorithm.