Graph SLAM: A Noob’s Guide to Simultaneous Localization And Mapping

Graph SLAM: A Noob’s Guide to Simultaneous Localization And Mapping

Implementation of SLAM on a 2D Graph from Scratch using Python

What is SLAM?

S imultaneous localization and mapping (SLAM) used in the concurrent construction of a model of the environment (the map), and the estimation of the state of the robot moving within it. In other words, SLAM gives you a way to track the location of a robot in the world in real-time and identify the locations of landmarks such as buildings, trees, rocks, and other world features. In addition to localization, we also want to build up a model of the robot’s environment so that we have an idea of objects, and landmarks that surround it and so that we can use this map data to ensure that we are on the right path as the robot moves through the world. So the key insight in building a map is that the robot itself might lose track of where it is by virtue of its motion uncertainty since there is no presence of an existing map because we are building the map simultaneously. That’s where SLAM comes into play.

Working of SLAM:

The basis for simultaneous localization and mapping (SLAM) is to gather information from a robot’s sensors and motions over time, and then use information about measurements and motion to reconstruct a map of the world. In this case, we’ll be localizing a robot in a 2D grid world and therefore, a graph-based SLAM approach constructs a simplified estimation problem by abstracting the raw sensor measurements. These raw measurements are replaced by the edges in the graph which can then be seen as virtual measurements.

Let’s assume we have a robot and the initial location, x0=0 & y0=0 . For this example, we don’t care about heading direction just to keep things simple. Let’s assume the robot moves to the right in the X-direction by 10. So, In a perfect world, you would know that x1 , the location after motion is the same as x0+10 in other words, x1=x0+10 , and y1 is the same as y0 .

But according to Kalman filters and various other robotic techniques, we have learned that the location is actually uncertain. So, rather than assuming in our X-Y coordinate system the robot moved to the right by 10 exactly, it’s better to understand that the actual location of the robot after the x1= x0+10 motion update is a Gaussian centered around (10,0), but it’s possible that the robot is somewhere else.

Here’s the math for the Gaussian of x variable:

Rather than setting x1 to x0+10, let’s express it in Gaussian that peaks when these two things are the same. So, if you subtract x1-x0-10, put this into a square format, and turn this into a Gaussian, we get a probability distribution that relates x1 and x0. We can do the same for y. Since there is no change in y according to our motion, y1 & y0 are as close together as possible.

The product of these two Gaussian is now our constraint. The goal is to maximize the likelihood of the position x1 given the position x0 is (0,0). So, what Graph SLAM does is, it defines the probabilities using a sequence of such constraints. Say we have a robot that moves in some space, GRAPH SLAM collects its initial location which is (0,0) initially, also called as Initial Constraints , then collects lots of relative constraints that relate each robot pose to the previous robot pose also called as Relative Motion Constraints . As an example, let’s use landmarks that can be seen by the robot at various locations which would be Relative Measurement Constraints every time a robot sees a landmark. So, Graph SLAM collects those constraints in order to find the most likely configuration of the robot path along with the location of landmarks, and that is the mapping process.