DEPARTMENT OF COMPUTER SCIENCE, UNIVERSITY COLLEGE LONDON
COMP0130 Coursework 02: Graph-based Optimisation and SLAM
1 Overview
Assignment Release Date: Saturday 5th March, 2022
Assignment Submission Date: 16:00 Friday 25th March, 2022
Weighting: 33% of module total
Final Submission Format: a PDF file containing a report, and a zip file containing source code.
Coursework Description
Current Revision: 20220305-001220
The goal of this coursework is to experiment with how a graphical-model based SLAM system is implemented and assess its properties. The system will be implemented using the MiniSLAM
CS Help, Email: tutorcs@163.com
Figure 2.1: The scenario. The vehicle (small blue triangle,starting from (0,0) at bottom left) drives a path (purple line) through an environment populated by landmarks (blue crosses). Var- ious landmark estimates, with their associated covariance ellipses, are shown.
event-based framework you met in Topic 02 Lab 01 and the MATLAB port of the g2o port you met in Topic 02 Lab 02. We assume you have familiarity with the questions and worked answers to those workshops.
The scenario is shown in Figure 2.1. A wheeled robot (DriveBot) drives through a two-dimensional environment which is populated by a set of landmarks. The number of landmarks and their lo- cations are not known, and must be estimated by the SLAM system.
The vehicle is equipped with three types of sensors:
1. A vehicle odometry sensor, which records speed and heading.
2. A “GPS” sensor which measures vehicle position.
3. A2Drangebearingsensorwhichmeasurestherangeandbearingtothelandmarksin2D.
In addition, an external is provided to specify the initial position and orientation of the robot. The mathematical models for these data sources and sensors are described in Appendix A.
A simulation system will create a stream of events which the SLAM system will need to pro- cess. Data association has already been addressed. Therefore, each landmark measurement you receive will have a unique (and correct) landmark ID.
The coursework consists of three questions:
Q1: Implement the prediction and update steps using the vehicle process model and the GPS sensor. You will also investigate some general properties about how the graph optimizers work. (28%)
Q2: Extend your implementation to undertake SLAM. (34%)
Q3: Discuss and implement a couple of ways improve the performance of the SLAM system (38%).
Please note that different subparts of different questions have their own launcher scripts of the form qx y.m. Although these scripts look very similar to one another, there are subtle differences between them to support the different subquestions (for example, some datasets are considerably longer than others). Furthermore, to mark your coursework, we will execute each script in turn and so the changes required to answer each question should not interfere with us being able to run the launcher code for a previous question.
3 Questions
1. In this question you will implement, within the DriveBotSLAMSystem class , the prediction step (using the process model in Subsection A.2) and the GPS sensor (using the observation model in Subsection A.3). Note that the GPS data arrives less frequently than the vehicle odometry data, and you must ensure that timestamps are handled correctly.
a. Describethestructureofthegraphwhichwillbeusedtosupportpredictingtherobot through time and updating its position periodically with the GPS sensor. Your de- scription should list the different types of vertices and edges. Provide the equations for the different types of edges and vertices.
Hint: Think about how angular discontinuities affect your solution and how these are related to the ⊞ operator.
[6 marks] 3
b. Implement the prediction step in the DriveBotSLAMSystem.
i. Inyourreportdescribewhichmethodsyoueditedtocompletethefunctionality. In the code, please provide comments which explain what your implementation does. These should be prefixed by “Q1b”.
[10 marks]
ii. Using the script q 1b.m, plot the graphs of the state error and the covariances computed by the estimator. Describe the behaviours you see and explain why you think they arise.
c. Implement the GPS update step in the DriveBotSLAMSystem class.
i. Inyourreportdescribewhichmethodsyoueditedtocompletethefunctionality. In the code, please provide comments which explain what your implementation does. These should be prefixed by “Q1c”.
ii. Using the script q1 c.m generate plots of the state error and the covariance. Present these graphs. Discuss the behaviours you see and why they arise.
d. Modify the script q1 d.m so that the optimizer runs once every timestep instead of at the end of the run. Plot the curves of the optimization time and the chi2 over the run. Describe the behaviours you observe and propose an explanation for why they arise.
[10 marks] [Total 49 marks]
2. In this question you will extend the DriveBotSLAMSystem to support SLAM. The observation model for these landmarks is given in Subsection A.4. The GPS sensor is not used.
a. Describe the structure of the graph for the SLAM system. Your description should include a discussion on the different types of edges and vertices and you should include the error models and Jacobians needed to implement them.
Hint: Think about how angular discontinuities affect your solution and how these are related to the ⊞ operator.
[11 marks]
b. Implement SLAM in DriveBotSLAMSystem.
i. Inyourreportdescribewhichmethodsyoueditedtocompletethefunctionality. In the code, please provide comments which explain what your implementation does. These should be prefixed by “Q2b”.
ii. Run the script q2 b.m which runs a SLAM example on a single closed loop. Plot the vehicle covariances, chi2 values and the amount of time to complete an optimization step. Discuss the behaviours you see and why you think they arise.
[15 marks]
程序代写 CS代考 加QQ: 749389476
c. Wewillnowlookatsomemoredetailsaboutthepropertiesoftheconstructedgraph.
i. By modifying the script q2 c.m, investigate the properties of the structure of the graph. You should obtain information on: the number of vehicle poses stored, the number of landmarks initalized, the average number of observations made by a robot at each timestep, and the average number of observations re- ceived by each landmark.
In your report, describe how you calculated these quantities. In the code, please prefix your changes by “Q2c”.
Hint: You can obtain all the necessary information from querying the con- structed graph, Matlab’s isa function, and using methods in the BaseVertex and BaseEdge classes.
ii. Present the results you obtained from analysing the graph and discuss what these quantities imply.
d. One of most important advantages of SLAM is the ability for it to perform loop closure.
Using the script q2 d.m, generate figures which show the map, vehicle and land- mark states just before and just after the loop closure event. Explain the form of the covariance matrices you see on the landmark and vehicle estimates immediately before and after the update. By looking at the determinant of the covariance of each landmark, compute the amount by which the uncertainty has changed for each landmark.
[10 marks]
[Total 60 marks]
e. This is a purely optional activity for those who are interested. No marks are awarded for it.
Run the script q2 large map.m. This tests the code on a much more substantial example. The landmark covariances matrices look like they are oriented as tangents on a set of concentric circles. Where do you think the origin might be, and what do you think might cause it?
3. This question explores several ways to improve the performance of a SLAM system by deleting parts of the factor graph. Again, the GPS is not used.
a. OnewaytoimprovetheperformanceofaSLAMsystemistomaintainallthesensor observation data, but to remove all the vehicle prediction edges.
i. By referring to the structure of the factor graph, why do you think this might be a sensible strategy to take? When do you think this is likely to be a successful strategy? When is it likely to be unsuccessful?
ii. Modify the DriveBotSLAMSystem system so that, when requested, it will delete the vehicle edges before optimization. Your code should support two cases: when all of the prediction edges are deleted, and when all but the first prediction edge should be deleted.
In your report, describe which methods you edited to provide this functionality. In the code, please provide comments which explain what your implementation does. These should be prefixed by “Q3a”.
iii. Using the script q3 a.m, present the three cases: all the prediction edges main- tained, all prediction edges removed, and all but the first prediction edge is re- moved. By referring to the structure of the graph, explain the differences in performance and behaviour.
Hint: you should find that the algorithm will fail if all the prediction edges are removed.
iv. Plot the optimization time, covariance and chi2 values for the working case where the edges have been removed. How do the results differ from the full SLAM system? Given the difference in performance, discuss if you this strat- egy on its own successful.
Computer Science Tutoring
b. Two approaches for simplifying graphs have been proposed: sparse keyframe based SLAM, and graph pruning.
i. Describe how keyframe-based SLAM and graph pruning methods work. Dis- cuss their strengths and weaknesses.
ii. Chose one strategy — either sparse key frames or graph pruning — to imple- ment. Explain which one you chose and why.
iii. Implement your solution. You can either modify the existing code (but ensure that it will run the earlier questions correctly) or you can create new classes to implement it (either by subclassing by simply copying the existing code to cre- ate a new class). To accompany it, you should write your own script q3 b.m, following the pattern of the other scripts, to run your revised SLAM system with the necessary settings.
In your report, describe how you implemented the functionality. In the code, please provide comments which explains what your implementation does. These should be prefixed by “Q3b”.
[15 marks]
iv. Evaluatetheresultsofyouralgorithm.Howyouchosethiswilldependuponthe algorithm implemented. However, key characteristic to include are the graph properties (such as number of vertices), chi2 performance, and error perfor- mance. Your evaluation should show whether your changes have improved performance, and provide an explanation of why you think you achieved the results presented.
[12 marks] [Total 66 marks] [Coursework Total 175 marks]
A System Models A.1 State Description
The vehicle state is described by its position and orientation in 2D:
xk =yk.
A standard right handed coordinate system is used: x points to the right, y points up and a positive rotation is in an anticlockwise direction. Angles are in radians.
Landmarks are in 2D. The state of the ith landmark is given by
mi =yi. (A.2)
A.2 Process Model
The vehicle process model is the similar to the one you saw in Workshop 04. The model has the form,
The matrix
xk+1 =xk +∆TkMk(uk +vk).
cosψk −sinψk 0
Mk =sinψk cosψk 0
is the rotation from the vehicle-fixed frame to the world-fixed frame and ∆Tk is the length of
the prediction interval. Note that the prediction interval is governed by when various sensors are available, and can vary through the simulation.
The control input consists of the speed of the vehicle (which is oriented along a body-fixed x axis) together with an angular velocity term,
The process noise is zero mean, Gaussian and additive on all three dimensions,
vk =vy.
The noise in the body-fixed y direction allows for slip and the fact, as discussed in the lectures, that the velocity is related to the front wheel and not the body orientation. The process noise covariance Qk is diagonal.
A.3 GPS Observation Model
The GPS sensor is a highly idealized one which provides direct measurements of the position of the robot. The observation model is
zG=xk +wG kk
yk The covariance of wkG, RGk is diagonal and constant.
A.4 Landmark Observation Model
The landmark observation model measures the range, azimuth and elevation of a landmark relative to the platform frame,
z Lk = β ki + w kL ,
rki =(xi −xk)2 +(yi −yk)2
i −1 yi − yk
βk = tan xi − x
The covariance of wkL, RLk is diagonal and is assumed to be time invariant and the same for all landmarks.