sss 06 graphical slam and sparse linear algebra frank dellaert
TRANSCRIPT
SSS 06SSS 06Graphical SLAM and Sparse Linear AlgebraGraphical SLAM and Sparse Linear Algebra
Frank DellaertFrank Dellaert
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 2
Learning OutcomesLearning Outcomes
• Understand:– Three Graphical Models for S(L)AM– Inference by Variable Elimination– Gaussian: Sparse Linear Algebra– Variable Ordering Methods
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 3
Reference MaterialReference Material• Probabilistic Robotics, Sebastian Thrun, wolfram Burgard, and
Dieter Fox. MIT Press 2005• Square Root SAM: Simultaneous Location and Mapping via Square
Root Information Smoothing, Frank Dellaert and Michael Kaess, Invited submission to special issue of IJRR on RSS, 2006
• A Multifrontal QR Factorization Approach to Distributed Inference applied to Multi-robot Localization and Mapping, Frank Dellaert, Alexander Kipp, and Peter Krauthausen, National Conference on Artificial Intelligence (AAAI), 2005
• Exploiting Locality by Nested Dissection For Square Root Smoothing and Mapping, Peter Krauthausen, Frank Dellaert, and Alexander Kipp, Robotics: Science and Systems, 2006
• Pothen and Sun 1992. Distributed multifrontal factorization using clique trees. In Proceedings of the Fifth SIAM Conference on Parallel Processing for Scientific Computing.
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 4
OutlineOutline
Three Graphical Models for SAM
Inference by Variable Elimination
Gaussian: Sparse Linear Algebra
Variable Ordering Methods
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 5
Sam I AmSam I Am
• EKF-Based SLAM is N2
• Inherent to Filtering• SAM: Smoothing and Mapping• All the buzz:
– Graphical SLAM (Folkesson & Christensen)
– Relaxation-based SLAM (Frese & Duckett)
– Delayed-State Filters (Eustice et al)– Full SLAM (Thrun, Burgard & Fox book)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 6
Bayes NetsBayes Nets
A
E
D
T
X
L
S
Directed Acyclic Graph
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 7
SAM Bayes NetSAM Bayes Net
x0 x1 x2 xM...
- Trajectory of Robot Poses + odometry
lNl1 l2
...
- “Landmarks”
z3z1 zKz2 z4
...
- Landmark Measurements
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 8
SAM Joint ProbabilitySAM Joint Probability
x0 x1 x2 xM...
P(X,M,Z) =
lNl1 l2
...
z3z1 zKz2 z4
...
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 9
Factor GraphsFactor Graphs
A
E
D
T
X
L
S
P(A)
P(T|A)
P(X|E)
P(E|TL)
P(L|S)
P(D|ES)
P(S)
D X E L T S AP(A) XP(T|A) X XP(S) XP(L|S) X XP(E|TL) X XP(D|ES) X X XP(X|E) X X
Bipartite
X
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 10
SAM Factor GraphSAM Factor Graph
x0 x1 x2 xM...
lNl1 l2
...
...
P(X,M,Z) =
- Trajectory of Robot
- “Landmarks”
- Landmark Measurements
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 11
Simulated ExampleSimulated Example
QuickTime™ and aAnimation decompressor
are needed to see this picture.
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 12
Example Factor GraphExample Factor Graph
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 13
Markov Random FieldMarkov Random Field
A
E
D
T
X
L
SD X E L T S A
D X X XX X XE X X X X X XL X X X XT X X X XS X X X XA X X
Undirected
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 14
SAM Markov Random FieldSAM Markov Random Field
x0 x1 x2 xM...
lNl1 l2
...
...
P(X,M,Z) =
- Trajectory of Robot
- “Landmarks”
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 15
Markov Random Field, ExampleMarkov Random Field, Example
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 16
OutlineOutline
Three Graphical Models for SAM
Inference by Variable Elimination
Gaussian: Sparse Linear Algebra
Variable Ordering Methods
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 17
Junction TreeJunction Tree
Belief Propagation• Cumbersome• Not a natural metaphor
TSA
ELTS
DESXE
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 18
Elimination GamesElimination Games
Undirected or MRF Elimination:
for j=1 to n dofind all potentials with xi
determine separator Sj
calculate P(xj|Sj)add clique (Sj)=∑P(xj|Sj)
done
Bipartite or Factor Graph Elimination:
for j=1 to n doremove all fi(xj..) with xj
determine separator Sj
calculate P(xj|Sj)=kfi(xj..) add factor f(Sj)=∑P(xj|Sj)
done
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 19
Bayes NetworkBayes Network
• Upper-triangular =DAG !
A
E
D
T
X
L
SD X E L T S A
D X X XX X XE X X X XL X X XT X X XS X XA X
DAG
works for arbitrary factors !
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 20
New: Bayes TreeNew: Bayes Tree
Inference = Easy• Marginalization• Optimization• sampling
TSA
EL:TSD:ESX:EP(D|ES)P(X|E)
P(EL|TS)
P(TSA)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 21
Easy, Recursive AlgorithmsEasy, Recursive Algorithms
TSA
EL:TS
D:ESX:E
4.P(DES)3.P(XE)
2.P(ELTS)
1.P(TSA)
TSA
EL:TS
D:ESX:E
2.ELTS*
1.TSA*
Marginalization or sampling:
Optimization:
4.DES*3.XE*
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 22
Variable EliminationYields Chordal Graph = Triangulated Graph
Variable EliminationYields Chordal Graph = Triangulated Graph
QuickTime™ and aAnimation decompressor
are needed to see this picture.
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 23
Bayes Tree= Directed Junction Tree = Directed Clique Tree
Bayes Tree= Directed Junction Tree = Directed Clique Tree
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 24
Marginals Easily RecoveredMarginals Easily Recovered
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 25
Filtering (EKF SLAM):Eliminate all but last poseFiltering (EKF SLAM):Eliminate all but last pose
QuickTime™ and aAnimation decompressor
are needed to see this picture.
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 26
Visual SLAM ExampleVisual SLAM Example
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 27
Visual SLAM Example Filled Graph
Visual SLAM Example Filled Graph
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 28
Visual SLAM Example EKF Graph = Dense !
Visual SLAM Example EKF Graph = Dense !
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 29
OutlineOutline
Three Graphical Models for SAM
Inference by Variable Elimination
Gaussian: Sparse Linear Algebra
Variable Ordering Methods
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 30
Gaussian Factor GraphGaussian Factor Graphx0 x1 x2 xM...
lNl1 l2
...
...
P(X,M,Z) =
-log P(X,M|Z) =
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 31
Gaussian Factor GraphGaussian Factor Graph
A
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 32
Gaussian MRFGaussian MRF
x0 x1 x2 xM...
lNl1 l2
... ...
- Trajectory of Robot
- “Landmarks”
Canonical form:
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 33
GMRFGMRF
ATA
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 34
Sparse Linear AlgebraSparse Linear Algebra
QR Factorization = Bipartite elimination
Cholesky Factorization = MRF elimination:
• Built for least-squares
• Corresponds to Gaussian factors/cliques
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 35
334x334, 9606 nnz
R
Linear Least SquaresLinear Least Squares
334x334, 7992 nnz
ATA
1129x334, 5917 nnz
A
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 36
Square Root FactorizationSquare Root Factorization
• QR on A: Numerically Stable
• Cholesky on A’A: Faster
A QR R
Cholesky RATA
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 37
OutlineOutline
Three Graphical Models for SAM
Inference by Variable Elimination
Gaussian: Sparse Linear Algebra
Variable Ordering Methods
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 38
Variable OrderingVariable Ordering
Ordering• NP-complete• Minimum degree methods (mmd,amd)• Nested dissection
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 39
Fill Poses then LandmarksFill Poses then Landmarks
R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 40
Eliminate Landmarks then PosesEliminate Landmarks then Poses
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 41
Minimum Degree FillYields Chordal Graph. Chordal graph + order = directed
Minimum Degree FillYields Chordal Graph. Chordal graph + order = directedR
QuickTime™ and aAnimation decompressor
are needed to see this picture.
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 42
The importance of being OrderedThe importance of being Ordered
500 Landmarks, 1000 steps, many loops
Bottom line: even a tiny amount of domain knowledge helps.
Bottom line 2: this is EXACT !!!
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 43
Simulation ResultsSimulation Results
0.000
0.100
0.200
0.300
0.400
0.500
0.600
factorization cost
#landmarks / #poses
Different column orderings
SAM 0.008 0.008 0.027 0.028 0.072 0.084
colamd 0.030 0.032 0.135 0.116 0.527 0.464
X-L 0.075 0.066 0.000 0.000 0.000 0.000
L-X 0.244 0.317 0.000 0.000 0.000 0.000
180/200 500/200 180/500 500/500 180/1000 500/1000
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 44
Nested Dissection (ND) (1)Nested Dissection (ND) (1)
• Recursive n-section
• Stop if subparts < β const.
• Separator sizes crucial!
l15 l28
l29 l42
x1 x14
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 45
Nested Dissection (ND) (2)Nested Dissection (ND) (2)
• Ordering = tree in post-fix order
• Poses: 1-14; measurements: 15-28 and 29-42
40-26-42-28-41-27-14-13-39-25-36-22-37-23-38-24-9-10-12-11-32-18-35-21-34-20-33-19-6-5-31-17-29-15-30-16-1-2-4-3-8-7
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 46
Simulated Block WorldSimulated Block World
Robot Pose Landmark
0 1 2 3 4
5
6
7
8910
11
12
13
14
15
k x k block world
K block hallway
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 47
Factorization times ND vs. AMD (1)Factorization times ND vs. AMD (1)
• Simulated block world
• Infinite hallway
• Sensing 8-12 landmarks per block
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 48
Factorization times ND vs. AMD (2)Factorization times ND vs. AMD (2)
• k x k block world
• k steps
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 49
Factorization times ND vs. AMD (3)Factorization times ND vs. AMD (3)
• k x k block world
• k² steps
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 50
Advantages of ND orderingsAdvantages of ND orderings
• Unlike MD algorithms ND elimination trees allow parallel computation
• Complexity bounds derivable
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 51
OutlineOutlineThree Graphical Models for SAM
Inference by Variable Elimination
Gaussian: Sparse Linear Algebra
Variable Ordering Methods
BONUS: Distributed SAM
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 52
Distributed SAM: Factor GraphDistributed SAM: Factor Graph
A
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 53
Distributed SAM: MRFDistributed SAM: MRF
ATAR
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 54
Reorder the Unknowns !Reorder the Unknowns !
A
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 55
Structure of R Changes!Structure of R Changes!
R
- Sparser !
- Independent Parts
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 56
R as a Directed GraphResults from elimination order
R as a Directed GraphResults from elimination order
R
- R is not symmetric
- Extra edges = fill
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 57
R = Clique Tree = Junction TreeR = Clique Tree = Junction Tree
- R is not symmetric
- Extra edges = fill
S2
F2
R
S1
F1
S3
F3
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 58
Example Clique TreeExample Clique Tree
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 59
•Solve:–Back-substitution from root to leaves
Multifrontal QRMultifrontal QR
•Symbolic Phase:–Eliminate–Build Junction Tree S2
F2
R
S1F1
S3F3
•Numeric Phase:–QR on leaves–Propagate to parents–Recurse
A QR R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 60
Multifrontal QRMultifrontal QR
Separator
Frontal Clique
AS2
F2
R
S1
F1
F1 F2 R
RF1 F2 R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 61
Multifrontal QRMultifrontal QR
AS2
F2
R
S1
F1
F1 R F2 R
F1 F2 R
Step 1: Gather Frontal Matrices
RF1 F2 R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 62
Multifrontal QRMultifrontal QR
AS2
F2
R
S1
F1
F1 R F2 R
F1 F2 R
Step 2: Factorize Frontal Matrices
RF1 F2 R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 63
Multifrontal QRMultifrontal QR
AS2
F2
R
S1
F1
F1 R F2 R
F1 F2 R
Step 1: Gather Frontal Matrices
R
RF1 F2 R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 64
Multifrontal QRMultifrontal QR
AS2
F2
R
S1
F1
F1 R F2 R
F1 F2 R
Step 2: Factorize Frontal Matrices
R
RF1 F2 R
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 65
AdvantagesAdvantages• Fast + Parallel
– Sparse structure of R– Robots = parallel computer
• Local Solution– Full rank frontal matrix– Singular frontal matrix
• Incremental QR– Linear or local linearization– Swap in new measurements– Rank 1 updates or Givens rotations
F1 R
F2 R
S2F2
R
S1F1
S3F3