Aaron Snowberger
June 10, 2021
58

# 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.)

3. Problem Overview
To track the motion of a robot from sensor readings
1

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

14. Implement (Graph) SLAM
From data, determine the robot’s location
3

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)

18. Run SLAM (𝝻 = 𝝮-1 x 𝝽)
18
𝝮-1 *𝝽 =𝝻

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

20. Test Model Accuracy
How closely do our predictions match reality?
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.

22. Project Reﬂection
Complex simplicity OR simple complexity
5

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.
probability of an event, based
on initial data & collecting more
data to improve the probability
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
same landmark) in order to
perform Simultaneous
Localization And Mapping (SLAM).

25. Conclusion & Future Research
Mapping, Sensing, Predicting
6

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