SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.
-
Upload
jacob-morris -
Category
Documents
-
view
223 -
download
0
Transcript of SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.
![Page 1: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/1.jpg)
SSS 06SSS 06Graphical SLAM and Sparse Linear AlgebraGraphical SLAM and Sparse Linear Algebra
Frank DellaertFrank Dellaert
![Page 2: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/2.jpg)
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
![Page 3: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/3.jpg)
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.
![Page 4: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/4.jpg)
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
![Page 5: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/5.jpg)
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)
![Page 6: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/6.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 6
Bayes NetsBayes Nets
A
E
D
T
X
L
S
Directed Acyclic Graph
![Page 7: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/7.jpg)
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
![Page 8: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/8.jpg)
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
...
![Page 9: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/9.jpg)
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
![Page 10: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/10.jpg)
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
![Page 11: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/11.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 11
Simulated ExampleSimulated Example
QuickTime™ and aAnimation decompressor
are needed to see this picture.
![Page 12: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/12.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 12
Example Factor GraphExample Factor Graph
![Page 13: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/13.jpg)
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
![Page 14: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/14.jpg)
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”
![Page 15: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/15.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 15
Markov Random Field, ExampleMarkov Random Field, Example
![Page 16: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/16.jpg)
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
![Page 17: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/17.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 17
Junction TreeJunction Tree
Belief Propagation• Cumbersome• Not a natural metaphor
TSA
ELTS
DESXE
![Page 18: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/18.jpg)
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
![Page 19: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/19.jpg)
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 !
![Page 20: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/20.jpg)
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)
![Page 21: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/21.jpg)
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*
![Page 22: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/22.jpg)
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.
![Page 23: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/23.jpg)
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
![Page 24: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/24.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 24
Marginals Easily RecoveredMarginals Easily Recovered
![Page 25: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/25.jpg)
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.
![Page 26: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/26.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 26
Visual SLAM ExampleVisual SLAM Example
![Page 27: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/27.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 27
Visual SLAM Example Filled Graph
Visual SLAM Example Filled Graph
![Page 28: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/28.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 28
Visual SLAM Example EKF Graph = Dense !
Visual SLAM Example EKF Graph = Dense !
![Page 29: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/29.jpg)
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
![Page 30: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/30.jpg)
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) =
![Page 31: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/31.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 31
Gaussian Factor GraphGaussian Factor Graph
A
![Page 32: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/32.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 32
Gaussian MRFGaussian MRF
x0 x1 x2 xM...
lNl1 l2
... ...
- Trajectory of Robot
- “Landmarks”
Canonical form:
![Page 33: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/33.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 33
GMRFGMRF
ATA
![Page 34: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/34.jpg)
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
![Page 35: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/35.jpg)
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
![Page 36: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/36.jpg)
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
![Page 37: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/37.jpg)
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
![Page 38: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/38.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 38
Variable OrderingVariable Ordering
Ordering• NP-complete• Minimum degree methods (mmd,amd)• Nested dissection
![Page 39: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/39.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 39
Fill Poses then LandmarksFill Poses then Landmarks
R
![Page 40: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/40.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 40
Eliminate Landmarks then PosesEliminate Landmarks then Poses
![Page 41: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/41.jpg)
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.
![Page 42: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/42.jpg)
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 !!!
![Page 43: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/43.jpg)
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
![Page 44: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/44.jpg)
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
![Page 45: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/45.jpg)
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
![Page 46: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/46.jpg)
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
![Page 47: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/47.jpg)
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
![Page 48: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/48.jpg)
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
![Page 49: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/49.jpg)
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
![Page 50: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/50.jpg)
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
![Page 51: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/51.jpg)
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
![Page 52: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/52.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 52
Distributed SAM: Factor GraphDistributed SAM: Factor Graph
A
![Page 53: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/53.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 53
Distributed SAM: MRFDistributed SAM: MRF
ATAR
![Page 54: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/54.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 54
Reorder the Unknowns !Reorder the Unknowns !
A
![Page 55: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/55.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 55
Structure of R Changes!Structure of R Changes!
R
- Sparser !
- Independent Parts
![Page 56: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/56.jpg)
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
![Page 57: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/57.jpg)
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
![Page 58: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/58.jpg)
Graphical SLAM and Sparse Linear Algebra, Frank Dellaert 58
Example Clique TreeExample Clique Tree
![Page 59: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/59.jpg)
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
![Page 60: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/60.jpg)
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
![Page 61: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/61.jpg)
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
![Page 62: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/62.jpg)
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
![Page 63: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/63.jpg)
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
![Page 64: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/64.jpg)
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
![Page 65: SSS 06 Graphical SLAM and Sparse Linear Algebra Frank Dellaert.](https://reader035.fdocuments.net/reader035/viewer/2022062314/56649e2a5503460f94b189c9/html5/thumbnails/65.jpg)
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