Universiteit van Amsterdam - Graduate School of Social Sciences (GSSS)
Chapter 1 Introduction - Universiteit van Amsterdam
Transcript of Chapter 1 Introduction - Universiteit van Amsterdam
![Page 1: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/1.jpg)
Probabilistic Robotics Graph SLAM
MSc course Artificial Intelligence 2017
https://staff.fnwi.uva.nl/a.visser/education/ProbabilisticRobotics/
Arnoud Visser
Intelligent Robotics Lab
Informatics Institute
Universiteit van Amsterdam
Images courtesy of Sebastian Thrun, Wolfram Burghard, Dieter Fox, Michael Montemerlo, Dick Hähnel, Pieter Abbeel and others.
![Page 2: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/2.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 2
Simultaneous Localization and Mapping
A robot acquires a map while localizing itself relative to this map.
Online SLAM problem Full SLAM problem
Estimate map m and current position xt Estimate map m and driven path x1:t
),|,( :1:1 ttt uzmxp ),|,( :1:1:1 ttt uzmxp
![Page 3: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/3.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 3
Graph SLAM
GraphSLAM extends the state vector y with the path x0:t
Example: Groundhog in abandoned mine:
every 5 meters a local map
TNyNxNyxtt smmsmmxxxy ,,1,1,110:0
![Page 4: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/4.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 4
State estimate
GraphSLAM requires inference to estimate the state
The state is estimated from the information matrix Ω and vector , the canonical representation of the covariance and mean.
Benefits:
Uncertainty is easy represented (Ω=0)
Information can be integrated by addition, without direct inference
The state estimated μt requires inversion of the information matrix Ω, which is done off-line
~~~ 1
:0
t
![Page 5: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/5.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 5
Acquisition of the information matrix
The observation of a landmark m1 introduces an constraint:
The constraint is of the type:
Where h(xt,mj) is the measurement model and Qt the covariance of the
measurement noise.
)),(()),((1
jt
i
tt
T
jt
i
t mxhzQmxhz
![Page 6: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/6.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 6
Acquisition of the information matrix
The movement of the robot from x1 to x2 also introduces an constraint:
The constraint is of the type:
Where g(ut,xj-1) is the motion model and Rt the covariance of
the motion noise.
)),(()),(( 1
1
1
tttt
T
ttt xugxRxugx
![Page 7: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/7.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 7
Acquisition of the information matrix
After several steps, a dependence graph appears with several
constraints:
The resulting information matrix is quite sparse.
The sum of all constraints
in the graph has the form:
![Page 8: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/8.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 8
Simplifying acquisition
By a Taylor expansion of the motion and measurement
model, the equations can be approximated:
![Page 9: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/9.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 9
Reducing the dependence graph
Removal of the observation of a landmark m1 changes the constraint
between x1 to x2 :
The constraint is changed by the following subtraction:
This is a form of variable elimination algorithm for matrix inversion
j
xjjj
j
jxxx tttt :0:0:0:0 ,
1
,,,
~
![Page 10: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/10.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 10
Reducing the dependence graph
Removal of the observation of a landmark m2 introduces a new
constraint between x2 to x4:
![Page 11: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/11.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 11
Reducing the dependence graph
The final result:
The resulting information matrix is much smaller.
This reduction can be done in time linear in size N
![Page 12: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/12.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 12
Updating the full state estimate from the path
There is now an estimate of the path robot
This requires to solve a system of linear equations,
which is not linear in size t due to cycles (loop closures!).
When found, the map can be recovered. For each landmark mj:
In addition, an estimate of the covariance Σ0:t over the robot path is
known (but not over the full state y)
~~~ 1
:0:0
tt
)~( :0:0,
1
, ttjjjjj
![Page 13: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/13.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 13
Full Algorithm
The previous steps should be iterated to get a reliable state estimate μ:
![Page 14: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/14.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 14
Full Algorithm
The algorithm can be extended for unknown correspondences:
![Page 15: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/15.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 15
Correspondence test
Based on the probability that mj corresponds to mk:
![Page 16: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/16.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 16
GroundHog in abandoned mine
A robot deployed in a previous flooded coal mine:
Sebastian Thrun et al., Autonomous Exploration and Mapping of Abandoned Mines, IEEE Robotics and
Automation Magazine 11(4), 2005.
![Page 17: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/17.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 17
GroundHog in abandoned mine
A robot created a 3D model of the coal mine:
Sebastian Thrun et al., Autonomous Exploration and Mapping of Abandoned Mines, IEEE Robotics and
Automation Magazine 11(4), 2005.
![Page 18: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/18.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 18
GroundHog in abandoned mine
Correspondences are discovered:
Sebastian Thrun et al., Autonomous Exploration and Mapping of Abandoned Mines, IEEE Robotics and
Automation Magazine 11(4), 2005.
![Page 19: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/19.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 19
GroundHog in abandoned mine
Correspondences are propagated and dissolved:
![Page 20: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/20.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 20
GroundHog in abandoned mine
Iterations stops when data associations induce no further
changes:
![Page 21: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/21.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 21
Segway RMP at Stanford
Segway exploring outdoors:
Sebastian Thrun and Micheal Montemerlo, The Graph SLAM Algorithm with Applications to Large-Scale Mapping
of Urban Structures, International Journal on Robotics Research 25(5/6), p. 403-430, 2005
![Page 22: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/22.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 22
Segway RMP at Stanford
Segway with vertically mounted laserscanner:
Green is ground, red obstacles, white structures above the robot
Sebastian Thrun and Micheal Montemerlo, The Graph SLAM Algorithm with Applications to Large-Scale Mapping
of Urban Structures, International Journal on Robotics Research 25(5/6), p. 403-430, 2005
![Page 23: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/23.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 23
Segway RMP at Stanford
3D map of the Stanford campus:
![Page 24: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/24.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 24
Segway RMP at Stanford
Color coded 3D map of the Stanford campus:
![Page 25: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/25.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 25
Segway RMP at Stanford
Top view of 3D map of the Stanford campus:
![Page 26: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/26.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 26
Segway RMP at Stanford
Effect of GPS on indoor mapping:
![Page 27: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/27.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 27
Resumé
Use a graph to represent the problem:
Every node in the graph corresponds to a pose or
an observation of the robot during mapping
Every edge between two nodes corresponds to the spatial
constraints between them
![Page 28: Chapter 1 Introduction - Universiteit van Amsterdam](https://reader031.fdocuments.in/reader031/viewer/2022012102/6169f9b811a7b741a34d7284/html5/thumbnails/28.jpg)
Probabilistic Robotics Course at the Universiteit van Amsterdam 28
Conclusion
GraphSLAM:
Solves the Full SLAM problem as post-processing step
Creates a graph of soft constraints from the data-set
By minimizing the sum of all constraints the maximum likelihood
estimate of both the map and the robot path is found
The algorithm works in iterating three steps:
construction, reduction, solving remaining equations
),|,( :1:1:1 ttt uzmxp