What is SLAM processing?
Technology
Introduction and preliminaries
In this coursework, we explore the architectural properties and assess the performance of various graphical based SLAM implementations. We use a wheeled robot (DriveBot) as our platform as it navigates within a two-dimensional environment on a defined path and estimates landmarks using the MiniSLAM event system [1]. As mentioned in the coursework description, the vehicle is equipped with the following sensors:
1. An odometry sensor to record speed and heading.
2. A GPS sensor to measure the position of the platform at any given time
3. A two-dimensional range bearing sensor to measure the range and the bearing to each landmark in the world
Landmarks data association is provided. In the following sections of this report, we will explain the imple mentation of the prediction and update states that make use of the process model for the vehicle and its GPS. We will explore graph optimizers and implement a full SLAM system for our robot. Finally, we’ll address the performance and present graph pruning as well as key-frame based SLAM.
1 Question 1
1.a Description of the robot prediction graph
In this first section, we present the factor graph used for prediction and implement the prediction step followed by the GPS observation update step. Figure 1 shows the graphic model used in question 1, which stores the conditional probabilities generated by process model. In the graph there are 2 main elements: firstly, the vertices (denoted with white circles), which describe the state of the platform at the current time step and secondly, the edges (denoted with black squares on lines), which specify the conditional probability between vertices [2]. The edges may be binary or unary depending on the number (and thus type) of vertices surround it. These differences are demonstrated in Table 1 and Table 2.
Table 1: Vertices Type.
Object Name Class Type
Vehicle pos VehicleStateVertex Base
Landmarks pos LandmarkStateVertex Base
Table 2: Edges Type.
Object Name Class Type
Init vehicle pos InitialPriorEdge Unary
Landmark observation LandmarkRangeBearingEdge Binary
Process model VehicleKinematicsEdge Binary
GPS observation GPSMeasurementEdge Unary
Figure 1: Graph model for question 1b.
The f0 value corresponds to the InitialPriorEdge that initializes the position of the vehicle. The initial state of the platform is stored in x0. When a vehicle odometry event is generated, f1, the VehicleKinematicsEdge will be connected to x0 and xk, with xk storing the prediction of the next state by the process model. This will repeat until the end of the odometry event.
The equations for VehicleKinematicsEdge and InitialPriorEdge and VehicleStateVertex will be described in section 1.2.b.
Figure 2: Graph model for question 1c.
Figure 2 shows the graphic model that supports the robot’s GPS sensory data. After getting the g2o prediction [3], a GPSMeasurementEdge zGk will be connected to the state vertex. It provides the observation of the position of the vehicle directly so the model does not need to generate additional state vertices. The equations for GPSMeasurementEdge will be described in section 1.3 C.
The equations for LandmarkStateVertex will be described in section 2.
1.b Implementation of the prediction step in DriveBotSLAMSystem i – Description of the edited methods
In function handlePredictToTime:
1. get a new VehicleStateVertex currentVehicleVertex.
2. add currentVehicleVertex to the graph.
3. estimate OmegaQ (the inverse of odometry co-variance matrix)
4. Update prediction
cos(ψ) sin(ψ) 0 −sin(ψ) cos(ψ) 0 0 0 1
predictedX = priorX + ∆T ∗ Mk ∗ (odometry + v)
5. create a new VehicleKinematicsEdge processModelEdge.
6. add the currentVehicleVertex and previous vertex to processModelEdge.
7. add the odometry information and omegaQ to processModelEdge.
8. add processModelEdge to the graph
The implementation of this task involved implementing the edges and the vertices for the process model. The files and functions edited include VehicleKinematicsEdge.m, VehicleStateVertex.m. The equations implemented in the kinematics file denote the error as
error = 1/∆T
cos(ψ) −sin(ψ) 0
sin(ψ) cos(ψ) 0 0 0 1
((Xk − Xk−1 − z))
and the jacobian as
−cos(ψ) −sin(ψ) −sin(ψ)dx + cos(ψ)dy
sin(ψ) −cos(ψ) −cos(ψ)dx − sin(ψ)dy 0 0 −1
Where dx = xk − xk−1 and dy = yk − yk−1. The error is the difference between xk and xk + 1. The equations implemented in the VehicleStateVertex.m involve the oplus operator and simply updates x which is the position and the heading and is then wrapped such that it is in the range of [π, −π].
ii – State error and co-variances computed by the estimator
It is observable from figure 3 that the errors of both x and y oscillate around the true values and that at the beginning of execution oscillations were slightly smaller than the later. The co-variances of the matrices also grew over time whilst oscillating this is because as more iterations are completed and as the system optimizes at a later point in time from the original positions there were more discrepancies added to the graphs from the available information. Also we see that the co-variance fluctuates but grows steadily overtime and we can see that the error is quiet high overall as is the co-variance. This is because the system is relying on only the optometry data and as such the co-variance will always grow and error will always be high.
(a)
(b)
(c)
Figure 3: (a) The first graph shows errors from the output of the implementation of task 1B for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. 4
1.c Implementation of the GPS update step in DriveBotSLAMSystem i – Description of the edited methods
In function handleGPSObservationEvent:
1. get a new GPSMeasurementEdge e.
2. set the current vehicle vertex to e.
3. get its GPS co-variance matrix(information matrix) omegaR.
4. add the data and the matrix to e.
5. add the e to the graph.
The Error functions for GPSMeasurementEdge is:
Figure 4: GPSMeasurementEdge Error
Which is the difference between Xk, Yk pos of vehicle and Xk, Yk pos of GPS.
The Jacobian matrix for GPSMeasurementEdge is:
Figure 5: Jacobian matrix
ii – State error and co-variance
The graphs in figure 6 show that the error rates remain stable and the results show that the system performs better than in the previous sections. However, there are large spikes in heading errors, larger than the previous, this could be caused by angular discontinuities.
The co-variances rise and then fall because this is where the vehicle is restarting from the beginning of the route again as loop closure occurs and therefore there is less variance over time. As such, the error of x and y describes a stable trend as it steadily remains between approximately 0.5 and -0.5 while experiencing slight fluctuations within this range.
Despite this the heading error spikes, likely from when the vehicle turns which it does suddenly. Like with before, as time passes, the time taken for the model to optimize the graph takes longer and increases linearly. It is important to note that the error is a lot lower than our previous set of graphs likely due to the additional information provided. We can also observe that the co-variances of the graphs are lower again due to the additional information which helps to reduces the discrepancies observed in the previous questions.
(a)
(b)
(c)
Figure 6: (a) The first graph shows errors from the output of the implementation of task 1C for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. 6
1.d Optimization at every timestep
The chi-squared value denotes the probability of the absolute value of the deviation from the expected value in our system. The of our noise is assumed to be from that of a standard Gaussian and therefore means we can use it for our purposes. Over time the chi value increases as seen in figure 7 this is because the prediction of the system deviates from the observed data overtime and as such the chi-squared value increases overtime.
This could be because over time our uncertainty of our measurements increases and therefore the chi-squared values increase. As usual the time taken for each prediction takes longer over time as more vertices and edges are added to the graph over time and the co-variances rise linearly. For the errors we see a rise in the error as time goes on as before but again it remains relatively low, this is all observable from figure 8.
(a)
(b)
(c)
Figure 8: (a) The first graph shows errors from the output of the implementation of task 1D for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. 8
2 Question 2
2.a Description of the SLAM system graph
In order to extend the system to support SLAM, LandmarkStateVertex and LandmarkRangeBearingEdge are required, the equation of them will be explained in section 2 B. Figure 10 shows the graphic model for question 2, landmarks vertex are denoted by m and the range bear edges are denoted by h.
Figure 9 shows the equation for range bearing landmark model. Where r represents range, β represents the azimuth and elevation of a landmark relative to the platform frame and ψ represents the heading of the laser.
Figure 9: The SLAM observation model.
Angular discontinuities in the system can effect the accuracy of the system by using measurements that have large errors and as such error is propagated through the system. As such the angular measurements are wrapped such that they are between [π, −π] whilst this doesn’t completely remove potential errors in the system it helps to reduce it.
Figure 10: Graph model for question 2a.
2.b Implementation of SLAM in DriveBotSLAMSystem
i – Description of the edited methods
In function handleLandmarkObservationEvent we implemented:
for every landmark in all landmarks
1. create a new LandmarkStateVertex.
2. create a new LandmarkRangeBearingEdge e.
3. set the current vehicle vertex and landmark state vertex to e.
4. set the hessian matrix omega and information matrix z to e.
5. if newVertexCreated is true, add the current landmark state vertex to the graph(initialize). 6. add the e to the graph.
7. repeat all of the above steps.
The error equation and the jacobian matrix of LandmarkRangeBearingEdge are:
r = ||pos2 − pos1|| − Z
β = tan−1(yi − yk
xi − xk) − ϕk
J1 =
−dx/norm −dy/norm 0
dy/norm2 −dx/norm2 −1
J2 =
dx/norm dy/norm −dy/norm2 dx/norm2
Where dx = xi − xk, dy = yi − yk and norm = ||[xi, yi] − [xk, yk]||. The class holds information on the edges that connect landmarks where landmarks are stored as state vertices and connected to other vertices in the graph via the edges holding the 2d laser measurements and the probability of the transition. LandmarkStateVertex holds information regarding the landmark in which it refers to.
ii – Vehicle co-variance, chi2 values and amount of time to complete an optimization step
The graphs in figure 11 show that, after implementing SLAM, the error rates are fluctuating around 0. Compare to 1b, the error is significantly lower, but comparing to the other tasks, the improvement is not very obvious. Overall the vehicle performance is promising.
The co-variances remains low and does not rise steadily(the range is 0-1). This is because the total timestamp of task 2b is only 1200, which is much smaller than 6000. It increases throughout the time and drops to 0. This becomes expected because fewer edges created while vehicle moves.
The chi-2 value increases over time and the slope of the line decreases at the second half of the graph, this could become explained by the co-variance decreases at that given time.
Figure 11: (a) The first graph shows errors from the output of the implementation of task 2B for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. (d) The fourth graph shows the chi-2 value over time.
11
2.c Properties of the constructed graph
i – Information obtained from the properties of the structure of the graph
Table 3 shows the properties of the graph of question 2c. The number of vehicle poses stored is calculated by subtracting the total number of vertices and the number of landmarks. The number of landmarks initialized is calculated by the counter in DriveBotSLAMSystem. The average number of observations received by each landmark is calculated by subtracting the total number of edges and (the number of vehicle poses -1), and then divided them by time. The average number of observations received by each landmark is calculated by dividing the total number of observation with the number of landmarks.
Table 3: properties of the structure of the graph.
value
number of vehicle poses stored 5273
number of landmarks initalizedn 7
average number of observations made by a robot at each timestep 0.6427
average number of observations received by each landmark 484.1429
number of all vertices 5280
number of all edges 8661
ii – Graph analysis results
The graphs in figure 12 show that:
• The error rates are similar compare to task 2b. The range is around +- 0.5, fluctuating around 0.
• The co-variances remains low (the range is 0-0.5), which is smaller than task 2b. The graph finished a loop closure so the co-variance falls and drops to 0.
• The chi-2 value figure of 2c is missing. This may due to the optimizer of 2c does not run in the main loop. chi-2 value will only be counted after the optimize() function is running, hence there is nothing in the figure. This could also explain why the optimization time is missing too.
2.d Loop closure event : co-variance and vehicle estimates
Loop closure is when the agent finds landmarks that have previously been visited and identifies it as such and can therefore re recognise the area and can therefore use this information to reduce the error in the map and stops the error of the system from growing indefinitely over time. From figure 13 we can see that when the loop closure event occurs the error of the system reduces and the co-variance of the positions of the system reduces significantly and in turn so does the error.
Figure 12: (a) The first graph shows errors from the output of the implementation of task 2C for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. (d) The fourth graph shows the chi-2 value.
Figure 13: The graphs taken from iteration 1300 and shows approximately where loop closure occurs. (a) The first graph shows the map of the environment.
(b) The second shows the co-variance over time.
(c) The third shows the evolution of the error on x and y over time.
3 Question 3
In this section, we explore different ways to increase the performance of our SLAM system. As we know, the size of the graph keeps growing larger over time and thus requiring more storage and becoming harder to solve which leads to an expensive computational cost. This virtually means that infinite memory requirements are necessary [4]. By pruning the sector graph from certain parts, we attempt reduce the memory and computational cost needed while still maintaining an acceptable accuracy [5] [6].
3.a Removing vehicle prediction edges
i – Description of the strategy
By checking the Figure 10, we can see that if prediction edges become removed, the computations that the SLAM has to handle will become reduced. Once the computational burden of the graph becomes reduced, the SLAM system will perform better.
However, pruning these edges means lowering the amount of information within the graph. Which will lead to an increased error and a reduced accuracy as some crucial prediction information becomes deleted. The error will keep accumulating at every new observation.
This suggests it is very important to selectively remove edges. It is likely to succeed if we find an adequate balance between deleting and keeping edges, and it is likely to fail if we delete most of the edges. Deleting edges excessively means that the system would be dealing with very sparse and minimal data, hence leading to a great negative impact on the performance as the factor graph does not contain enough information to be accurate.
ii – Description of the edited methods
In function deleteVehiclePredictionEdges:
for every edge in all edges:
1. if current edge is a VehicleKinematicsEdge:
2. if current edge is the 1st edge and keep is true, keep the first edge.
3. else, removeEdge(curEdge) we remove the edge from the graph.
iii – Differences in performance and behaviour
• Case1: all the prediction edges maintained:
In this case, the system is just like the SLAM system in task 2. From 10 we know that the system keeps all the VehicleKinematicsEdge f, so the system will process a lot of informations and do computations.
The performance of the case 1 will be slow.
• Case2: all prediction edges removed. In this case, the SLAM system failed to run. This is because without prediction edge, the SLAM system will have no information to absorb at all.
• Case3: all but the first prediction edge is removed.
In this case, we only keep the first VehicleKinematicsEdge, by running the program we found that usually the system will drop hundreds of edges and vertices from the graph. From 10 we know that, abandon some of the edges f and vertex x will significantly improve the performance of the system, because we decreased the additional information and kept the essential ones.
iv – Comparison with full SLAM : optimization time, co-variance and chi2 values
From Figure 14 we found that the behavior of the System(maintain all edges) is similar to task 2. The range of error is very small (+- 0.2), and the co-variances is also very low (max 0.12). However, the performance of the SLAM system is not very promising. More optimization times on the vehicle will result in longer run times. When setRecommendOptimizationPeriod = 1000, the system will spend around 20 minutes to run the whole process.
The chi-2 values increased throughout the time and the gradient remains stable, that means the system error accumulation is stable.
(a) (b)
(c) (d)
Figure 14: (a) The first graph shows errors from the output of the implementation of task 3a (keeping all edges) for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. (d) The fourth graph shows the chi-2 value over time.
Figure 15 below shows the behavior of the System(keep the first edge), we found that: Compare to case 1, the range of error increased to +- 1.5, this is because after removing most of the edges, the system receives less information than before. This can also be applied to co-variance, the max co-variance increased to around 0.9.
As expected, the performance of the SLAM system increased significantly, When setRecommendOptimiza tionPeriod = 1000, the system only will take around 8 minutes to run the whole process. The chi-2 values increased throughout the time and the gradient remains stable as well, and because of the reason above, the overall error accumulation is half-ed, compare to case 1.
(a) (b)
(c) (d)
Figure 15: (a) The first graph shows errors from the output of the implementation of task 3a (keep the first edge) for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. (d) The fourth graph shows the chi-2 value.
3.b Graph simplification : sparse keyframe based SLAM and graph pruning i – Description of keyframe based SLAM and graph pruning
• In keyframe based SLAM, the system proceed with localization and mapping in two different steps. Once localization is completed over a set of frames distributed over time, it uses an optimization algorithm on these keyframes to reduce the number of frames.
Some versions of Keyframe based SLAM have been shown to sometimes outperform filter-based methods and be effective against scale-drift [7] compared to SLAM systems that use local map components and thus hinder global scale correction. On large scale mapping, errors made at every iteration will accumulate within large loops and this can be responsible for map distortion. The scale drift error is prevalent with single camera SLAM systems is scale drift as the platform is only able to estimate its speed depending on the scale of the world. Some calibration landmarks (of known sizes) would be needed to initialise a such a system properly. When the scale error accumulates, the position estimates is also wrong [8].
• As for graph pruning, it removes the unnecessary LandmarkRangeBearingEdge, which will make decision on computation. This will reduce the size of the graph and improve the overall performance. However, it is always a problem how to determine the which one and how many edges should we remove. Deleting too many edges may cause the graph become too sparse and decrease the accuracy, deleting a few edges may not improve the performance at all.
ii – Strategy explanation
We implemented graph pruning as it was a simpler option given our current implementation and it should make the system significantly faster.
iii – Implementation
We created a copy of q3_a.m, and did the modification based on it. Both GPS and Laser are set to true, the laserDetectionRange = 20 and gpsMeasurementPeriod = 1. We set setRecommendOptimizationPeriod = 1000. The graph pruning will be activated and will run once the boolean variable graphPruning in DriveBotSLAM System is switched to true.
In function startGraphPruning, we proceed as follows :
for every vertices:
1. get the edges connected to current vertices.
2. for every edge, if the edge is a LandmarkRangeBearingEdge, we increment the number of deleted edges. 3. if the number of deleted edges is greater than remain(In our case, remain = 2)
4. remove the current edge from the optimization graph.
iv – Results evaluation
In conclusion, from figure 16 we can see that the errors produced by the implementation. Were generally much higher than in the previous SLAM implementation and also presented a number of spikes. However, it is expected that the errors would be much greater for a more complex route since the toy problem we have is only a very basic test problem. The figure shows that that our co-variances are also slightly higher than the one in our previous implementation and the chi-2 value is lower due to the fact that the accumulated error of the SLAM system through executed period is lower than the methods previously described in 3A observable from figure 14. What is quiet important, however, is the fact that the execution time, view-able in the figure, is quiet low in comparison to our full implementations in 3A seen in figure 14.
(a) (b)
(c) (d)
Figure 16: (a) The first graph shows error rates from the output of the implementation of task 3b (keep the first edge) for x, y and the heading.
(b) The second graph shows the co-variances of x, y and the heading.
(c) The third graph shows the time taken for the optimization to finish after each iteration. (d) The fourth graph shows the chi-2 value.
References
[1] Simon Julier. “Lab Topic 2, Workshop 1: Kalman Filters and SLAM”. In: COMP0130 Robot Vision and Navigation (2022).
[2] Simon Julier. “Lecture 6B: Graphical Models and Factor Graphs”. In: COMP0130 Robot Vision and Navi gation (2022).
[3] Rainer Kümmerle et al. “G2o: A general framework for graph optimization”. In: 2011 IEEE International Conference on Robotics and Automation (2011), pp. 3607–3613.
[4] Gerhard Kurz, Matthias Holoch, and Peter Biber. “Geometry-based Graph Pruning for Lifelong SLAM”. In: 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2021, pp. 3313– 3320. doi: 10.1109/IROS51168.2021.9636530.
[5] Simon Julier. “Lecture 7A: Graphical Models and SLAM”. In: COMP0130 Robot Vision and Navigation (2022).
[6] Simon Julier. “Lecture 7B: Practical Issues”. In: COMP0130 Robot Vision and Navigation (2022).
[7] Hauke Strasdat, J. Montiel, and Andrew Davison. “Scale drift-aware large scale monocular SLAM”. In: vol. 2. June 2010. doi: 10.15607/RSS.2010.VI.010.
[8] Tom Botterill, Steven Mills, and Richard Green. “Correcting Scale Drift by Object Recognition in Single Camera SLAM”. In: IEEE Transactions on Cybernetics 43.6 (2013), pp. 1767–1780. doi: 10.1109/TSMCB .2012.2230164.
Written by James Harding, Yicheng Zhang, Jeanne Beyazian
Technology
What is SLAM processing?
