Department of Computer Science, University College London COMP0130 Coursework 02: Graph-based Optimisation and SLAM February 21, 2020 1. Overview • Assignment Release Date: Thursday 27th February, 2020 • Assignment Submission Date: 12:00 Monday 23rd March, 2020 • Weighting: 33% of module total • Final Submission Format: a PDF file containing a report, and a zip file containing code. 2. Coursework Description The goal of this coursework is to develop a graphical-model based SLAM system and assess its properties. The system will be implemented using the MATLAB g2o port. The scenario is shown in Figure 2.1. A wheeled robot drives through a two-dimensional envi- ronment which is populated by a set of landmarks. 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. A 2D range bearing sensor which measures the range and bearing to the landmarks in 2D. The mathematical models for these sensors are described in Appendix A. We assume that data association has already been addressed. Therefore, each landmark mea- surement you receive will have a unique (and correct) landmark ID. You will implement the system using the g2o MATLAB implementation from Workshop 04, within an event-driven framework called MiniSLAM. The code for the event-driven framework is described in Appendix . The coursework consists of four tasks: 1 Figure 2.1: The scenario. The vehicle (small blue triangle) drives a path (purple line) through an environment populated by landmarks (blue crosses). Task 1: Implement vehicle odometry in both a Kalman filter and the g2o within MiniSLAM. Compare the performance of both. (10%) Task 2: Extend your implementation to include the GPS for both the Kalman and g2o systems. (15%) Task 3: Extend your implementation with g2o to implement SLAM and evaluate its properties. (40%) Task 4: Explore a number of ways to improve the performance of the SLAM system (35%). 3. Task 1: Implement and Analyse Platform Prediction (10%) You should use the script task1TestPrediction to launch the system. For both the Kalman and g2o SLAM systems, implement the code to predict the future state of the platform by implementing the methods handleNoPrediction and handlePredictionToTime in +minislam.slam.g2o.G2OSLAMSystem and the methods in minislam.slam.g2o.VehicleKinematicsEdge. Describe your implementation and, when describing your g2o implementation, provide a dia- gram to show the structure of the graph you create. Compare the performance of the Kalman and g2o systems. For your comparison, you should examin both the computed trajectories and the covariances. Explain the results you obtain. 2 4. Task 2: Implement and Analyse GPS Measurements (15%) You should use the script task2TestGPS to launch the system. Extend your implementations of the Kalman and the g2o SLAM systems to use the GPS obser- vations by implementing handleGPSObservationEvent and developing any edges that you need to support the new observation type. Describe your implementation and, when describing your g2o implementation, provide a diagram to show the structure of the graph you create. Your diagram should show the case where the GPS observation events arrive less frequently than the odometry events. Compare the performance of the Kalman and g2o systems for GPS measurement periods (time between GPS measurements) of 1 s, 2 s and 5 s (you can set this by changing the value of gpsMeasurementPeriod in the simulation parameter object created in line 4 of task2TestGPS). For your comparison, you should compare both the computed trajectories and the covariances. Explain the results you obtain. 5. Task 3: Implement SLAM (40%) Extend your implementation of the g2o SLAM system to implement a full SLAM system by providing an implementation of the handleLandmarkObservationEvent method. You will need to create your own vertex type to represent the landmarks and any edges to support observa- tions. Describe your implementation and, as before, show the structure of the graph you create. You should also provide information about the size of the graph. This includes the number of landmarks initialized, the number of vehicle poses stored, and the average number of observa- tions made by a robot at each timestep, and the average number of observations received by a landmark. In developing your system, two scripts are provided. The script task3DevelopSLAM runs the SLAM system on a small scenario of only a few landmarks. The script task3TestSLAM runs a more extensive example with 200 landmarks and 5000 timesteps. 6. Task 4: Improving Performance (35%) This contains a number of sub-tasks to explore the performance further. The script task4ReviseSLAM as based on task3TestSLAM, but provides one or two extra parameters you might find useful. 6.1. Task 4.1: Adding GPS (5%) One way to improve the performance of a SLAM system is to use additional measurements such as GPS. Experiment with different values of GPS measurement periods and see how the performance of the SLAM system changes. (You should find no changes should be required to your code.) You can start with the values in Task 2, but you are not restricted to those choices. Assuming you want to minimize the number of GPS measurements taken, what do you think would be optimal rate will be? Provide evidence of your choice. 6.2. Task 4.2: Removing Odometry (10%) One way to improve the performance of a SLAM system is to use a keyframe based approach in which the vehicle prediction edges are not kept. Briefly describe how a keyframe-based SLAM 3 system differs from a regular SLAM system. Modify the g2o SLAM system to delete the vehicle prediction edges before optimization and explain your implementation. Compare two cases: when you delete all vehicle prediction edges, and all but the first vehicle prediction edges. By considering the structure of the graph, explain why you think the two behave differently. H int: The algorithm will fail in one of these two cases. 6.3. Task 4.3: Graph Reduction (20%) Two approaches for simplifying graphs have been proposed: sparse keyframe based SLAM, and graph pruning. Explain what these are. Chose one strategy — either sparse key frames or graph pruning. Explain which one you chose and why. Modify the g2o SLAM system to support your strategy and present the results. Your results should compare the size of the graph, its runtime, and performance as compared with a graph which contains no pruning. A. System Models A.1. State Description The vehicle state is described by its position and orientation in 2D: xk = xkyk ψk . (A.1) 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 = [ xi 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, xk+1 = xk + ∆TkMk (uk + vk) . (A.3) The matrix Mk = cosψk − sinψk 0sinψk cosψk 0 0 0 1 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, uk = sk0 ψ˙k . 4 The process noise is zero mean, Gaussian and additive on all three dimensions, vk = vkvy vψ˙ . 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. Observation Models The GPS sensor is a highly idealized one which provides direct measurements of the position of the robot. The observation model is zGk = [ xk yk ] +wGk The covariance of wGk , R G k is diagonal and constant. The landmark
observation model measures the range, azimuth and elevation of a landmark relative to the platform frame, zLk = [ rik βik ] +wLk , where rik = √ (xi − xk)2 + (yi − yk)2 βik = tan −1 ( yi − yk xi − xk ) − φk The covariance of wLk , R L k is diagonal and is assumed to be time invariant and the same for all landmarks. B. MiniSLAM Framework Modern robotic systems fuse data from a variety of sensing systems with different properties and update rates. You have already seen how these can be implemented in ROS. For this coursework, we introduce a small (self-contained) framework for event-driven estimation called MiniSLAM. The code for MiniSLAM is provided in the Coursework_02/+minislam directory. A set of scripts are provided for the different tasks which launch the MiniSLAM scenario. MiniSLAM consists of the following: 1. A light weight event driven system to support multiple event types. 2. A scenario generator, which simulates or creates an environment. 3. A set of one or more SLAM systems which process this data to create results. 5 B.1. Event Driven System Communication to the SLAM systems is provided by a set of events. Events are the analogy of ROS messages. The event types can be found in the Coursework_02/+minislam/+event_types subdirectory. The event types inherit from the Event class. Further documentation on the different events can be found in this class. B.2. SLAM Systems The main class which handles the processing the events is the VehicleSLAMSystem class. This provides logic to handle the different incoming event types. These are eventually passed onto a series of event handlers which carry out operations dependent upon the SLAM systems. The main ones you need to define are: 1 % Handle a GPS measurement 2 handleInitialConditionEvent(this , event); 3 4 % Handle not predicting 5 handleNoPrediction(this); 6 7 % Handle everything needed to predict to the current state. 8 handlePredictToTime(this , time); 9 10 % Handle a GPS measurement 11 handleGPSObservationEvent(this , event); 12 13 % Handle a set of laser measurements 14 handleLandmarkObservationEvent(this , event); 6
