![Page 1: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/1.jpg)
1
SLAM: Simultaneous Localization and Mapping
Instructor: Chris Mavrogiannis
TAs: Kay Ke, Gilwoo Lee, Matt Schmittle
*Slides based on or adapted from Sanjiban Choudhury, Dieter Fox, Michael Kaess
![Page 2: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/2.jpg)
The SLAM problem
2
Robot is moving through a static unknown environment
u1 u2
z1
z2
z0
Given a series of controls and measurements, estimate state and map
Origin
![Page 3: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/3.jpg)
What if I just integrate controls?
3
![Page 4: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/4.jpg)
What we want …
4
FastSLAM–Best particle13.10Grid-basedFastSLAM477
Figure13.19OccupancygridmapcorrespondingtotheparticlewiththehighestaccumulatedimportanceweightobtainedbythealgorithmlistedinTable13.4fromthedatadepictedinFigure13.18.Thenumberofparticlestocreatethisexperimentwas500.Alsodepictedinthisimageisthepathrepresentedbytheparticlewiththemaximumaccumulatedimportanceweight.
(a)(b)
Figure13.20Trajectoriesofallsamplesshortlybefore(left)andafter(right)closingtheouterloopoftheenvironmentdepictedinFigure13.19.ImagescourtesyofDirkHähnel,UniversityofFreiburg.
Need to figure out two things:
Correct relative movements between successive measurements
Closing the loop globally
![Page 5: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/5.jpg)
Spilling the beans on SLAM
5Ground truth
Let’s assume this was the ground truth….
![Page 6: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/6.jpg)
Spilling the beans on SLAM
6Ground truth Odometry
Odometry is really really noisy!
![Page 7: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/7.jpg)
Spilling the beans on SLAM
7Ground truth Odometry
Measurements can help correct this somewhat
Measurement correction
![Page 8: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/8.jpg)
Spilling the beans on SLAM
8Ground truth Odometry
Relative error accumulates …
Measurement correction
![Page 9: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/9.jpg)
Spilling the beans on SLAM
9Ground truth Odometry
Eventually robot comes back to a familiar place …. loop closure!
Measurement correction
![Page 10: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/10.jpg)
Spilling the beans on SLAM
10Ground truth Odometry
If we are filtering, we can only fix the last pose estimate
Measurement correction
![Page 11: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/11.jpg)
Spilling the beans on SLAM
11Ground truth Odometry
If we are optimizing, we can backprop errors in time!
Measurement correction
![Page 12: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/12.jpg)
Today’s objective
12
1. How do we solve the chicken-or-egg problem in SLAM?
2. SLAM as an optimization instead of filtering
![Page 13: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/13.jpg)
Bayes filter is a powerful tool
13Localization Mapping SLAM POMDP
![Page 14: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/14.jpg)
Assembling Bayes filter
14
TasksLocalization
P(pose | data)
Mapping P(map | data)
SLAM P(pose, map | data)
Belief Representations
Probabilistic Models
Bayes Filter
![Page 15: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/15.jpg)
Tasks that we will cover
15
Tasks Belief Representation Probabilistic Models
Localization P(pose | data)
Gaussian / ParticlesMotion model
Measurement model
Mapping P(map | data)
Discrete (binary) Inverse measurement model
SLAM P(pose, map |
data)
Particles+GridMap (pose, map)
Motion model, measurement model,
correspondence model
(Week 3)
(Week 4)
(Week 4)
![Page 16: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/16.jpg)
16
SLAM as just another Bayes filtering problem
Task:SLAM
P(map, pose | data)
zt�1
What is the data?
Stream of controls and measurements
zt
ut
u1:t, z1:t
What is the belief representation?
16-833, Spring 20189
Occupancy Grids: From scans to maps
False “hits” from people in the environment
16-833, Spring 20189
Occupancy Grids: From scans to maps
False “hits” from people in the environment
P (xt,m|u1:t, z1:t)
![Page 17: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/17.jpg)
Different map representations
17
We are free to use any of the map representations we discussed
Map representations
¨ Typical representations are:¤ Feature-based¤ Grid maps (occupancy maps)¤ 3D representations (voxels, surfels, octrees etc)
4
Typical models are: ! Feature maps ! Grid maps (occupancy or reflection probability
maps)
Map Representations
today
4
Typical models are: ! Feature maps ! Grid maps (occupancy or reflection probability
maps)
Map Representations
today
Feature maps Grid maps Surface maps
![Page 18: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/18.jpg)
18
Why is SLAM hard?
Chicken-or-egg problem:
Given a map, we can localizeGiven the pose, we can build map
![Page 19: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/19.jpg)
Graphical model of SLAM
19
xt�1 xt xt+1
zt�1 zt zt+1
m
ut�1 ut ut+1
![Page 20: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/20.jpg)
20
If SLAM can be expressed as a Bayes filtering problem,
let’s use our favorite Bayes filter…
Particle Filter!
![Page 21: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/21.jpg)
Generalized particle filters
21
Step 0: Start with a set of particles
Step 1: Sample particles from proposal distribution
Step 2: Compute importance weights
Step 3: Resampling
bel(xt�1) = {x1t�1, x
2t�1, . . . , x
Mt�1}
bel(xt) = {x̄1t , . . . , x̄
Mt }
wkt =
bel(xt)
bel(xt)
Draw M samples from weighted distribution
r =bel(xt)
bel(xt)= ⌘P (zt|xt)( )
![Page 22: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/22.jpg)
Problem: Space of maps too large!!
22
How big is this space?
P (xt,m|u1:t, z1:t)
If we were to sample particles, what is the likelihood that they would explain the measurements??
( )2M
P (zt|m,xt) → 0
![Page 23: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/23.jpg)
Key idea: Exploit dependencies
23
Even if a space is absurdly large, dependencies within the space can significantly shrink the space of possibilities we should consider.
A B
(need to store 5x12 = 60 numbers)
r.v.A B
(need to store 5 + 9 = 14 numbers)
![Page 24: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/24.jpg)
Key idea: Exploit dependencies
24
Is there a dependency in this gigantic combined state space?
Yes!
The map depends on the history of poses of the robot
![Page 25: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/25.jpg)
Rao-Blackwellization
25
Factorization to exploit dependencies between variables
P (a, b) = P (b|a)P (a)
If P(b|a) can be computed efficiently
P (a) P (b|a)Represent
with samples
Compute
for every sample
![Page 26: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/26.jpg)
Applying the factorization trick
26
state history
map data
P (x1:t,m|z1:t, u1:t)
P (x1:t|z1:t, u1:t)state
historydata datamap
P (m|x1:t, z1:t, u1:t)state
history
=
= P (x1:t|z1:t, u1:t)state
historydata
NY
i=1
P (mi|x1:t, z1:t)
(Particle filter to estimate this) (Occupancy map)
![Page 27: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/27.jpg)
How do we compute a PF over state history?
27
For simplicity, each particle only stores the current state at timestep t
However the weights of the particle are computed based on it’s path through time.
(just like you did in you assignment)
![Page 28: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/28.jpg)
How do we compute a PF over state history?
28
Let’s jump straight to the importance sampling step
wt =bel(x1:t)
bel(x1:t)=
P (x1:t|zt, z1:t�1, u1:t)
P (x1:t|z1:t�1, u1:t)
/ P (zt|x1:t, z1:t�1, u1:t)P (x1:t|z1:t�1, u1:t)
P (x1:t|z1:t�1, u1:t)
/ P (zt|xt, x1:t�1, z1:t�1, u1:t)
/X
m
P (zt|xt,m)P (m|x1:t�1, z1:t�1, u1:t)
(most likely map from prev timestep)(meas)
⇡ P (zt|xt, m̂)
where m̂ = argmaxm
P (m|x1:t�1, z1:t�1, u1:t�1)
![Page 29: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/29.jpg)
FastSLAM
29
Proposed by Montemerlo 2002
FastSLAM¨ Proposed by Montemerlo et al. in 2002 (for
landmark based SLAM)¨ Each particle has a pose and a map
Occupancy grid mapParticle1
Particle2
ParticleN
…
Occupancy grid map
Occupancy grid map
![Page 30: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/30.jpg)
FastSLAM Algorithm
30
FastSLAM Algorithm478 13 The FastSLAM Algorithm
1: Algorithm FastSLAM_occupancy_grids(Xt−1, ut, zt):
2: X̄t = Xt = ∅3: for k = 1 to M do
4: x[k]t = sample_motion_model(ut, x
[k]t−1)
5: w[k]t = measurement_model_map(zt, x
[k]t ,m[k]
t−1)
5: m[k]t = updated_occupancy_grid(zt, x
[k]t ,m[k]
t−1)
6: X̄t = X̄t + ⟨x[k]t ,m[k]
t , w[k]t ⟩
7: endfor
8: for k = 1 to M do
9: draw i with probability ∝ w[i]t
10: add ⟨x[i]t ,m[i]
t ⟩ to Xt
11: endfor
12: return Xt
Table 13.4 The FastSLAM algorithm for learning occupancy grid maps.
lution of the map is 10cm. To learn this map, as few as 500 particles wereused. During the overall process the robot encountered two loops. A mapcalculated from pure odometry data is shown in Figure 13.18, illustrating theamount of error in the robot’s odometry.
The importance of using multiple particles becomes evident in Fig-ure 13.20, which visualizes the trajectories of the samples shortly before andafter closing a loop. As the left image illustrates, the robot is quite uncertainabout its position relative to the starting position, hence the wide spread ofparticles at the time of loop closure. However, a few resampling steps afterthe robot re-enters known terrain suffice to reduce the uncertainty drastically(right image).
![Page 31: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/31.jpg)
FastSLAM in action!
31Haehenl et al.
![Page 33: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/33.jpg)
33
Do we really need a probability distribution?
P (x1:t,m)
Or are does a maximum a posteriori estimate suffice?
x̂1:t, m̂ = arg maxx1:t,m
P (x1:t,m)
![Page 34: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/34.jpg)
34
SLAM as a pure optimization problem
u1:t, z1:t
Data x̂1:t, m̂ = arg maxx1:t,m
P (x1:t,m)optimize
![Page 35: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/35.jpg)
SLAM as a set of relationships
3516-833, Spring 20182
The SLAM Problem (t=0)
Robot
Landmark
Landmarkmeasurement
Onboard sensors:– Wheel odometry– Inertial measurement unit
(gyro, accelerometer)– Sonar– Laser range finder– Camera– RGB-D sensors
Courtesy M.Kaess
![Page 36: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/36.jpg)
16-833, Spring 20183
The SLAM Problem (t=1)
Robot
Landmark 1 Landmark 2
Odometry measurement
Landmarkmeasurement
36
SLAM as a set of relationships
Courtesy M.Kaess
![Page 37: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/37.jpg)
3716-833, Spring 20184
The SLAM Problem (t=n-1)
Robot
Landmark 1 Landmark 2
Odometry measurement
Landmarkmeasurement
SLAM as a set of relationships
Courtesy M.Kaess
![Page 38: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/38.jpg)
3816-833, Spring 20185
The SLAM Problem (t=n)
Odometry measurement
Landmarkmeasurement
SLAM as a set of relationships
Courtesy M.Kaess
![Page 39: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/39.jpg)
Factor Graph representation of SLAM
3916-833, Spring 20186
Factor Graph Representation of SLAM
Bipartite graph with variable nodes and factor nodes
Robot pose
Landmark positionLandmarkmeasurement
Odometry measurement
Courtesy M.Kaess
![Page 40: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/40.jpg)
Factor Graph representation of SLAM
40
The variables in the optimization are poses of robot at all time steps and all landmarks
✓ = [x1, x2, . . . , xn, l1, . . . , lm]
![Page 41: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/41.jpg)
Factorization of probability density
4116-833, Spring 201817
Factorization of Probability Density
•Conditional independence:
argmaxΘෑ𝑧∈𝑍
𝑝 (𝑧|Θ)
argmaxΘ 𝑝 𝑝|Θ 𝑝 (𝑢1|Θ)⋯𝑝(𝑢𝑛|Θ) 𝑝 (𝑚1|Θ) ⋯𝑝(𝑚4|Θ)
𝑝 𝑧1𝑧2|Θ = 𝑝 𝑧1 Θ 𝑝 (𝑧2|Θ)
Courtesy M.Kaess
![Page 42: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/42.jpg)
How do we solve such optimization?
42
Large scale sparse
non-linear least squares optimization
+incrementally
growing factors
Option 1: Using sparse matrix algebra(Kaess et al. 2008)
Option 2: Using probabilistic graphical models(Kaess et al. 2011)
![Page 43: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/43.jpg)
43iSAM2, Kaess et al. 2012
![Page 44: SLAM: Simultaneous Localization and Mapping · 2020. 1. 31. · 1 SLAM: Simultaneous Localization and Mapping Instructor: Chris Mavrogiannis TAs: Kay Ke, Gilwoo Lee, Matt Schmittle](https://reader033.vdocuments.site/reader033/viewer/2022060814/60932b23e00f630d287a33d8/html5/thumbnails/44.jpg)
Application: Kintinuous 2.0 (Whelan et al.)
44