37

# Computer Vision Project 3: SLAM

SLAM stands for Simultaneous Localization And Mapping. As a part of Udacity's Computer Vision Nanodegree, I generated a random world with randomly placed landmarks, and made a robot perform random movements within that world. Measurements between the robot location and nearby landmarks were performed at each step, and using the robot's motion, and measurements to the landmarks, its location was tracked over time.

This PPT was submitted as a part of my PhD coursework to detail my project process and reflection.

June 10, 2021

## Transcript

1. ### Computer Vision Project 3: SLAM ( ) NAME: Aaron Snowberger

(정보통신공학과, 30211060) DATE: 2021.06.10 SUBJECT: Computer Vision (Dong-Geol Choi) CURRICULUM: Udacity Computer Vision | Udacity GitHub Project Repo | Certiﬁcate of Completion Simultaneous Localization And Mapping
2. ### CONTENTS 01 Problem Overview 02 Setting up the World a.

Robot Class b. Visualize the World c. Sense Landmarks d. Constraint Matrices, 𝝮 & 𝝽 (#1-5) e. Motion Constraints 03 Implement (Graph) SLAM a. Generate Environment & data b. Initialize Constraints c. Update measurements d. Run SLAM & Visualize results e. Visualize the World 04 Test Model Accuracy 05 Project Reﬂection 06 Conclusion & Future Research 2 SLAM: Simultaneous Localization And Mapping The idea is to detect the motion of a robot in an environment by tracking measurements from its sensors over time. (A similar concept to how a self-driving car works.)

4. ### Problem Overview In this project, I set up a 2-dimensional

world and perform SLAM (Simultaneous Localization And Mapping) on a robot in order to track its location and recreate a map of the environment based on the robot’s motion and sensor measurements of landmarks over time. In order to do this, a square matrix (𝝮) and a vector (𝝽) are used to track all robot poses (xi) and landmark locations (Li). Since this localization and map-building relies on the visual sensing of landmarks, this is a computer vision problem. 4
5. ### Setting up the World Build 2D world and the robot

with initial position & measurements of uncertainty 2
6. ### Apart from instantiating a new instance of the Robot and

initializing its internal variables based on the parameters that are passed in, there are three important methods as well. Robot Class make_landmarks(num) : Creates a given number of randomly located landmarks by selecting a random x and random y location within the world_size 6 move(dx, dy): Attempts to move the robot by delta x (dx) and delta y (dy). If outside the world boundary, then nothing happens and the function returns failure. This function also includes motion_noise which is a small random value between [-1.0, 1.0] that accounts for data loss recorded by motion sensors as the robot moves around the environment. sense(): Computes dx and dy distances to landmarks (referencing internal variables it keeps track of, i.e. self.landmarks) within the visibility range and returns the landmark index along with the measurements. This function also includes measurement_noise which is a small random value between [-1.0, 1.0] that accounts for data loss in landmark measurements.
7. ### Visualize the World Instantiate the Robot Class with some small

values just to visualize it (later it will be 10x bigger). 7 r.move(-2, 4) #(x,y) r.make_landmarks (6) display_world(*args)
8. ### Once the measurements are taken, these measurements, along with the

motion that occurred, can be added to a data array which will be used for tracking. SLAM works in reverse - using the tracking data array to recreate the world. Sense Landmarks Based on the landmarks created previously, now we get the robot to sense() how far away from it each landmark is. The measurements it records are only those that fall within its visibility measurement_range . 8
9. ### Constraint Matrices, Omega (𝝮) & Xi (𝝽) #1 9 The

constraint matrix 𝝮 keeps track of the relationships between each robot location and the landmarks it detects. x represents the position of the robot (in one-dimensional space). L represents the landmarks it detects. Motion Constraints The robot moves 5 steps from x0 → x1: x1 = x0 + 5 x0 - x1 = -5 x1 - x0 = 5
10. ### Constraint Matrices, Omega (𝝮) & Xi (𝝽) #2 10 The

constraint matrix 𝝮 keeps track of the relationships between each robot location and the landmarks it detects. x represents the position of the robot (in one-dimensional space). L represents the landmarks it detects. Motion Constraints The robot moves -4 steps from x1 → x2: x2 = x1 - 4 x1 - x2 = (original 5) + 4 = 9 x2 - x1 = -4 -4 9 2 -1 -1 1
11. ### Constraint Matrices, Omega (𝝮) & Xi (𝝽) #3 11 The

constraint matrix 𝝮 keeps track of the relationships between each robot location and the landmarks it detects. x represents the position of the robot (in one-dimensional space). L represents the landmarks it detects. Measurement Constraints The robot detects landmark L0 @ x1 and measures it to be 9 steps away: This means the 4 intersections of: (x1,x1), (x1,L0), (L0,x1), (L0,L0) must be updated. x1: L0 distance = 9 x1 - L0 = -9 L0 - x1 = 9 0 3 -1 -1 1 9
12. ### Constraint Matrices, Omega (𝝮) & Xi (𝝽) #4 12 The

constraint matrix 𝝮 keeps track of the relationships between each robot location and the landmarks it detects. x represents the position of the robot (in one-dimensional space). L represents the landmarks it detects. Measurement Constraints The robot detects landmark L0 @ x1 and measures it to be 9 steps away: This means the 4 intersections of: (x1,x1), (x1,L0), (L0,x1), (L0,L0) must be updated. x2: L0 distance = -3, L1 distance = 7 x2 - L0 = 3 x2 - L1 = -7 L0 - x2 = -3 L1 - x2 = 7 -1 -1 -1 6 -8 7
13. ### The best solution for ﬁnding all the motion positions and

all the landmark positions is to multiply the inverse of 𝝮 x 𝝽. For 2D space, we can either: 1. use two 𝝻 = 𝝮-1 x 𝝽 functions (one for x values and one for y) 2. OR double the size of our matrices (x0, y0, x1, y1, … Lx0, Ly0, Lx1, Ly1) Constraint Matrices, Omega (𝝮) & Xi (𝝽) #5 13

15. ### Generate Environment & data make_data() generates a world grid with

landmarks and places a robot in the center of the world. Then it generates motion and measurement data by moving the robot and sensing the landmarks over some number of time steps. 15
16. ### Initialize Constraints (𝝮 & 𝝽) For testing purposes, we begin

with a 10x10 2D grid with only 2 landmarks. 16 𝝮 𝝽
17. ### Update Measurements (Implement SLAM) slam() takes 6 parameters and returns

𝝻 1. data - measurements & motions collected for a number of time steps 2. N - number of time steps 3. num_landmarks - # of landmarks 4. world_size - size (w/h) of the world 5. motion_noise - random noise associated with motion. Update conﬁdence is 1.0/motion_noise 6. measurement_noise - random noise associated with sensing. Update weight is 1.0/measurement_noise 𝝻 is the entire path traversed by the robot (all x,y poses) *and* all landmark locations. 17 Possibly the most important function is the helper function to update the matrices. It needs to take into account: 1. Both indices that must be updated 2. In both matrices 3. As well as motion_noise 4. And measurement_noise 5. And the delta (diﬀerence in x or y)

=𝝻
19. ### Visualize the World The diﬀerence between the robot’s estimated ﬁnal

position and its true ﬁnal position is: dx = 8.24910 - 9.52082 = -1.27172 dy = 7.35233 - 6.98117 = 0.37116 Total = 1.32478 ( ) And estimated landmark positions are close to the true landmark positions, so our implementation works well! 19

4
21. ### Test Case 1 21 Test Case 2 The more closely

my results match the results from the “oﬃcial answer results,” the better my algorithm is.

23. ### Kalman Filter: keeps track of the estimated state of the

system and uncertainty of the estimated event. It is a recursive iterator and consists of two steps: Predict and Update. Baye’s Rule, Gaussian Distributions, & Kalman Filters In working on this project, I was introduced to many new concepts that helped me understand how prediction models work in AI and robotics. Baye’s Rule: addresses the probability of an event, based on initial data & collecting more data to improve the probability Gaussian: addresses the uncertainty related to an event’s probability 23
24. ### Kalman Filter & Graph SLAM Notes 24 Graph SLAM: uses

information matrices that (𝝮 & 𝝽) that are generated with a factor graph of interdependent observations (two observations are related if they contain information about the same landmark) in order to perform Simultaneous Localization And Mapping (SLAM).

26. ### Kalman Filter uses & SLAM Algorithms SLAM in a nutshell

(Wikipedia) Given a series of controls u t and sensor observations o t over a discrete number of time steps t, compute an estimate of the agent’s state x t and a map of the environment m t . With Baye’s Rule: Sequentially: Future Research needs to be done into both additional uses for the Kalman Filter, as well as more in-depth research into the various other SLAM algorithms. SLAM itself is a broad topic and has many wide uses including ➀ self-driving cars, ➁ unmanned aerial vehicles, ➂ autonomous underwater vehicles, ➃ planetary rovers, and ➄ robotics. Therefore, there are many approaches to address this problem, and much to explore in many diﬀerent domains. The Kalman Filter is one approach (also known as EFK SLAM in robotics), but additionally, Acoustic SLAM, Audio-Visual SLAM, Collaborative SLAM, FastSLAM, Graph SLAM (studied in this project), SLAM with DATMO, Active SLAM, and RatSLAM to name a few. 26
27. ### THANKS! The Jupyter Notebook used in this project including code

and output can be found at: https://github.com/jekkilekki/computer-vision/tree/ master/Graph%20SLAM 27