Robots and autonomous vehicles need to keep track of their surroundings before they can move around it. Building a map turned out to be quite hard ; especially if you have to calculte your own position in the arena. SLAM seemed like the way to go.
Simultaneous Localization And Mapping, or as it is usually called, SLAM. SLAM is a technique used when building a map of an unknown environment without prior knowledge, or to update a map within a known environment with prior knowledge while keeping track of the current location.
Note
It was developed by Hugh Durrant-White and John J. Leonard which was based on earlier work by Smith, Self and Cheesman. It was first abbreviated to SMAL but later changed to give it a better impact.
SLAM isn’t one big algorithm, but instead divided into several methods of which each can be implemented in various ways.
The Landmark extraction parth basically searches for easy to spot place in the environment which is retreived gets from the map (if any) or from its sensors Secondly, the data association step will compare the landmarks in the map to the landmarks detected by the sensors to see if the current map is still accurate.
The third step - the state estimation - will also compare the current readings with the map to see if the robot has moved. If it has moved, find out in which direction and how much it moved.
Finally, in the landmark update step, the current landmarks and the position of the robot are updated and the proces continues from the beginning.
RANSAC (RANdom SAmple Consensus) is a method for extracting a line from a set of observed points. It is a non-deterministic algorithm that does not produce an optimal solution, but rather provides a reasonable result with a certain probability. This probability increases when more iterations are allowed. It is based on the idea that the data set consists of inliers and outliers. Inliers are data points that fit the model, while outliers are data points that do not. RANSAC attempts to fit a model to the inliers, while ignoring the outliers.
while iterations < maxIterations:
randomly pick n values v from the dataset
fit a model m to these points
for p in points:
if p fits model with threshold:
add p to v
if enough points in v:
fit a model to v
calculate quality of the fit
if new model is better than the previous best:
replace it
increase iterations
return best model
This way it will randomly pick a couple of points and see if this is a good start to create a line through the set. All the fitting of lines will be done with a least squares fit, using NumPy.
This method simply finds a linear function through the points where the sum of the squared differences of the points to this line is minimal. By allowing a lot of iterations obviously the chances of finding a better fit will increase. Nevertheless is there no guarantee that a good or even a reasonable solution will be found, as it is ultimately an algorithm that relies on picking random points, but if the algorithm variables are chosen correctly the probability of a good solution is high.
Unfortunately the basic RANSAC does not support the extraction of multiple lines from a single set of data. To solve this we impemented an algorithm to separate the points belonging to different lines. We looked at the way RANSAC separates inliers from outliers; take a set of random points as normal to construct a model and find points that fit the model. If there are enough points in the set v we assume that we can reliably construct a single line from that set of data.
Once we have a set of points, we use the normal RANSAC on that set of points to extract the best possible line.
The results from this algorithm are quite accurate. Unfortunately the algorithm does not perform too well; mainly because of the number of fits we had to do.
The time it takes us to extract the lines from one set of data is about 0.25s.
This is too slow for a system that is required to calculate the position in real-time. We tried to tweak the algorithm but the difference was not enough to make it usable. The speed of the language (Python) was not the bottleneck, as we used the NumPy extension for all the list operations.
To tackle this issue we created our own algorithm which abused the fact that the points returned by the laser are already sorted.
Because of the poor performance of RANSAC, we designed an alternative algorithm that can detect walls by finding line segments. While RANSAC assumes a set of randomly distributed points, the Concensus collect algorithm takes advantage of the fact that in the (ordered) laser data points from the same wall will be next to each other in the set.
Walls are detected by iterating through the array of data points and comparing the distance and the angle to the origin with the previous point in the array. If the distance and angle are within the threshold, the points are assumed to be part of the same wall. Once a consensus set of points from the same wall has been found, a least squares fit is used to find the best matching line for the wall.
NumPy is a Python extension module, written mostly in C, that defines the numerical array and matrix types and basic operations on them.
NumPy’s array class (which is used to implement the matrix class) is implemented with speed in mind, so accessing NumPy arrays is faster than accessing Python lists. Further, NumPy implements an array language, so that most loops are not needed. For example, Plain Python (and similarly for C, etc.):
a = range(10000000)
b = range(10000000)
c = []
for i in range(len(a)):
c.append(a[i] + b[i])
This loop can take up to 10 seconds on a several GHz processor. With NumPy:
import numpy as np
a = np.arange(10000000)
b = np.arange(10000000)
c = a + b
Not only is this much more compact and readable, it is almost instant by comparison, and even the NumPy import is faster than the loop in plain Python.
Why? Python is an interpreted language with dynamic typing. This means that on each loop iteration it needs to check the type of the operands a and b to select the right variant of the ‘+’ operator for those types (in Python, ‘+’ is used for many things, like concatenating strings, and lists can have elements of different types). The NumPy add function, which Python automatically selects when one of the operands of ‘+’ is a NumPy array, does this check only once. It then executes the “real” addition loop in a compiled C function. This is very fast by comparison to the interpreted loop in plain Python.
This component implements position estimation using landmark extraction.
The position component accepts one command: LASER.
Line(a, b)
itemgetter(item, ...) –> itemgetter object
Return a callable object that fetches the given item(s) from its operand. After, f=itemgetter(2), the call f(r) returns r[2]. After, g=itemgetter(2,5,3), the call g(r) returns (r[2], r[5], r[3])
itemgetter(item, ...) –> itemgetter object
Return a callable object that fetches the given item(s) from its operand. After, f=itemgetter(2), the call f(r) returns r[2]. After, g=itemgetter(2,5,3), the call g(r) returns (r[2], r[5], r[3])
Model class, fits a certain set of data with fitLine()