A brief introduction to GraphSLAM
GraphSLAM is a SLAM algorithm that solves the full SLAM problem, i.e, the algorithm recovers the entier path and map instead of just the recent pose and map. This allows it to consider dependencies between current and previous poses. One of the major benefits of the GraphSLAM algorithm is its reduced need for significant onboard processing capability. Unlike FastSLAM, which uses particles to estimate the robot’s most likely pose, GraphSLAM works with all of the data at once to find the optimal solution.
What is a graph?
Let’s understand how a graph is constructed
Any two nodes are connected with an edge, and they are called a soft spatial constraint. It’s called soft because motion and measurement data are uncertain and constraints will have some amount of error present in them. Soft constraints come in two forms: Motion constraints between two successive robot poses and Measurement constraints between a robot pose and a feature in the environment. In the image above, x0 and x1 represent robot poses. These two poses are tied together with a solid line, called motion constraint. If the robot were to sense its environment and encounter a feature m1, a soft measurement constraint would be added. Measurement constraints are represented in dashed edges and motion constraints in solid edges for simplicity.
As the robot moves around, more and more nodes are added to the graph and over time, the graph constructed by the mobile robot becomes very large in size, as shown below. Luckily, graphSLAM is able to handle large numbers of features. In the end, the goal is to find the node configuration that minimizes the overall error present in all the constraints.
Recap
For what it’s worth, it is quite helpful to go through the math involved in computing the analytical and numerical solutions to the Maximum Likelihood Estimation(MLE) problem since this is the basis of GraphSLAM. Follow the two posts below before continuing further.
1-D to n-D
1-Dimensional Graphs
In the previous examples, we were working with 1-dimensional graphs. The robot’s motion and measurements were limited to one dimension — they could either be performed forwards or backward.
In such a case, each constraint could be represented in the following form,
n-Dimensional Graphs
In multi-dimensional systems, we must use matrices and covariances to represent the constraints. This generalization can be applied to the system of 2-dimensions, 3-dimensions, and really any n-number of dimensions. The equations for the constraints would look like so,
Where h() and g() represent the measurement and motion functions, and Qt and Rt are the covariances of the measurement and motion noise.
The multidimensional formula for the sum of all constraints is presented below.
The first element in the sum is the initial constraint — it sets the first robot pose to equal to the origin of the map. The covariance, Ω0, represents complete confidence. Essentially,
Information Matrix and Information Vector
Now that we have seen what the multidimensional constraints look like, we need a more elegant way to solve the system of linear equations produced by a graph of constraints. Information matrix and information vector are two data structures that can be used to store information from our constraints.
The information matrix is represented by Ω and the information vector is represented by ξ. Fundamentally, the information matrix is an inverse of a covariance matrix. Every off-diagonal cell in the matrix is a link between two poses, two features, or a pose and feature. The cell has a value of zero when no information is available about a link.
Let’s look at a graph containing five constraints, one initial constraint, two motion constraints, and two measurement constraints. The information matrix and vector are populated successively with each constraint. The initial constraint will tie the poses x0 to a value, usually zero. This constraint will populate one cell in the information matrix and one cell in the information vector.
A motion constraint, like x0→x1, will tie together two robot poses populating four cells in the matrix and two cells in the vector. These are the cells that relate x0 and x1 to each other.
Similarly, a measurement constraint will update four cells in the matrix and two in the vector. These are the cells that relate the feature to the pose from which it was measured.
All the other cells remain 0. The information matrix is considered sparse because most off-diagonal elements are zero. This sparsity is very valuable when it comes to solving the system of equations that is embedded in the information matrix and vector.
Inference
Once the information matrix and information vector have been populated, the path and map can be recovered by the following operation,
The result is a vector, μ defined over all poses and features, containing the best estimate for each. This operation is very similar to what you encountered before in the simple one-dimensional case, with a bit of added structure. Just as before, all constraints are considered when computing the solution.
Completing the above operation requires solving a system of equations. In small systems, this is an easily realizable task, but as the size of the graph and matrix grows — efficiency becomes a concern.
The efficiency of this operation, specifically the matrix inversion, depends greatly on the topology of the system.
Linear Graph
If the robot moves through the environment once, without ever returning to a previously visited location, then the topology is linear. Such a graph will produce a rather sparse matrix that, with some effort, can be reordered to move all non-zero elements to near the diagonal. This will allow the above equation to be completed in linear time.
Cyclical Graph
A more common topology is cyclical, in which a robot revisits a location that it has been to before after some time has passed. In such a case, features in the environment will be linked to multiple poses — ones that are not consecutive but spaced far apart. The further apart in time that these poses are — the more problematic, as such a matrix cannot be reordered to move non-zero cells closer to the diagonal. The result is a matrix that is more computationally challenging to recover.
However, all hope is not lost — a variable elimination algorithm can be used to simplify the matrix, allowing for the inversion and product to be computed quicker.
Variable Elimination
Variable elimination can be applied iteratively to remove all cyclical constraints. Just like it sounds, variable elimination entails removing a variable (ex. feature) entirely from the graph and matrix. This can be done by adjusting existing links or adding new links to accommodate for those links that will be removed.
Think that each of the constraints is a spring, i.e., each of the poses and the features is connected by a spring between them. In this spring analogy, variable elimination removes features but keeps the net forces in the springs unaltered by adjusting the tension on other springs or adding new springs where needed.
This process is demonstrated in the following two images. The first image shows the graph, matrix, and vector as shown in the sections above.
The second image shows the elimination of m1. You can see that in this process the matrix will have five cells reset to zero (indicated in red), and four cells will have their values adjusted (indicated in green) to accommodate the variable elimination. Similarly, the information vector will have one cell removed and two adjusted.
This process is repeated for all of the features, and in the end, the matrix is defined over all robot poses. At this point, the same procedure as before can be applied,
Performing variable elimination on the information matrix/vector prior to performing inference is less computationally intense than attempting to solve the inference problem directly on the unaltered data.
In practice, the analytical inference method described above is seldom applied, as numerical methods are able to converge on a sufficiently accurate estimate in a fraction of the time. Next, we will explore how nonlinear constraints are handled in GraphSLAM.
Nonlinear Constraints
The idea that a robot only moves in a linear fashion is quite limiting, and so it becomes important to understand how to work with nonlinear models. Nonlinear models cannot be applied directly as they would turn the Gaussian distribution into a much more complicated distribution that could not be computed in closed form (analytically, in a finite number of steps). Therefore, motion and measurement constraints must be linearized before they can be added to the information matrix and vector. Otherwise, it would be impractical, if not impossible, to solve the system of equations analytically.
Luckily, we know a technique we can apply, the Taylor Series approximation. If you recall, a Taylor Series approximates a function using the sum of an infinite number of terms. A linear approximation can be computed by using only the first two terms and ignoring all higher-order terms. In multi-dimensional models, the first derivative is replaced by a Jacobian — a matrix of partial derivatives.
Linearizing Constraints
A linearization of the measurement and motion constraints is the following,
To linearize each constraint, you need a value for μt−1 or μt to linearize about. This value is quite important since the linearization of a nonlinear function can change significantly depending on which value you choose to do so about. So, what μt−1 or μt is a reasonable estimate for each constraint?
Well, when presented with a completed graph of nonlinear constraints, you can apply only the motion constraints to create a pose estimate,
, and use this primitive estimate in place of μ to linearize all of the constraints. Then, once all of the constraints are linearized and added to the matrix and vector, a solution can be computed as before, using
This solution is unlikely to be an accurate solution. The pose vector used for linearization will be erroneous, since applying just the motion constraints will lead to a graph with a lot of drift, as errors accumulate with every motion. Errors in this initial pose vector will propagate through the calculations and affect the accuracy of the end result. This is especially so because the errors may increase in magnitude significantly during a poorly positioned linearization (where the estimated μt is far from reality, or the estimated μt lies on a curve where a small step in either direction will make a big difference).
To reduce this error, we can repeat the linearization process several times, each time using a better and better estimate to linearize the constraints.
Iterative Optimization
The first iteration will see the constraints linearized about the pose estimate created using solely motion constraints. Then, the system of equations will be solved to produce a solution, μ.
The next iteration will use this solution, μ, as the estimate used to linearize about. The thought is that this estimate would be a little bit better than the previous; after all, it takes into account the measurement constraints too.
This process continues, with all consequent iterations using the previous solution as the vector of poses to linearize the constraints. Each solution incrementally improves on the previous, and after some number of iterations, the solution converges.
Summary
Nonlinear constraints can be linearized using the Taylor Series, but this inevitably introduces some error. To reduce this error, the linearization of every constraint must occur as close as possible to the true location of the pose or measurement relating to the constraint. To accomplish this, an iterative solution is used, where the point of linearization is improved with every iteration. After several iterations, the result, μ, becomes a much more reasonable estimate for the true locations of all robot poses and features.
The workflow for GraphSLAM with nonlinear constraints is summarized below:
- Collect data, create a graph of constraints,
- Unit convergence:
- Linearize all constraints about an estimate, μ, and add linearized constraints to the information matrix & vector,
- Solve the system of equations using:
Conclusion
Now you know how to construct a graph, how to represent 1-D and n-D constraints, how to solve a system of linear equations using information matrix and vector, and how to linearize a nonlinear constraint. You even went through examples of how to optimize constraints in GraphSLAM using MLE. Next, we will look at RTAB-Mapping, a flavor of GraphSLAM which is appearance-based, robust, and more importantly, real-time.
Source: Udacity’s Self Driving Nano-degree program
