Post on 30-May-2020
Inference in Factor Graphs Frank DellaertSchool of Interactive Computing @ Georgia TechCenter for Robotics & Intelligent Machines
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Monte Carlo Localization
Dellaert, Fox, Burgard & Thrun, ICRA 1999
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Biotracking
• Multi-Target Tracking with MCMC
3Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 4
SFM without Correspondence
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
4D Reconstruction, with Grant Schindler
Historical Image Collection
5
4D City Model
3D Structure + Time IntervalsCamera Pose + Time
4D Model
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
4D Structure over Time
6
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Large-Scale Structure from Motion
Photo Tourism
[Snavely et al. SIGGRAPH’06]
Build Rome in a Day
[Agarwal et al. ICCV’09]
Photosynth
[Microsoft]
Build Rome on a Cloudless Day
[Frahm et al. ECCV’10]
Iconic Scene Graphs
[Li et al. ECCV’08]
Discrete-Continuous Optim.
[Crandall et al. CVPR’11]
2 / 407Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
3D Models from Community Databases
• E.g., Google image search on “Dubrovnik”
8Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
3D Models from Community Databases
Agarwal, Snavely et al., University of Washingtonhttp://grail.cs.washington.edu/rome/
95K images, 3.5M points, >10M factors
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Motivation
10
• Robotics and Computer Vision• Large nonlinear problems• Claim: Factor Graphs Rock!• Insights into familiar algorithms
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 11Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Boolean Satisfiability
M I
A H G
¬M ! I
M ! ¬I
M ! A
¬I ! H
¬A ! H ¬H ! G
Variables
Factors
Factor Graph
12Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Constraint Satisfaction Problems
13Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
H
O 14
Constraint Satisfaction Problems
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
4.1 Representation
4.1.1 Bayesian Networks
A Bayesian network (Pearl, 1988) or Bayes net is a directed graphical model where the nodes representvariables x with conditional densities P(x|�) on them, and the edges indicate on which variables � we areconditioning. The joint probability density P(X) over all variables X = (x1 . . .xn) is then defined as theproduct of all the conditionals associated with the nodes:
P(X) =�iP(xi|�i) (4.1)
Bayes nets are an excellent language in which to think about and specify a generative model. They easilylend themselves to thinking about how a given measurement z depends on the parameters �. Importantly,they allow us to factor the joint density P(�,Z) into a product of simple factors. This is typically done byspecifying conditional densities of the form P(z|�), indicating that given the values for a subset � ! � ofthe parameters, the measurement z is conditionally independent of all other parameters.
A
D
T
S
L
BE
X
Figure 4.2: Bayes nets for example in Figure 4.1. The observed variables are shown as square nodes.
Example 11. The Bayes net for our running example from Figure 4.1 is shown in Figure 4.2. In the figureI have indicated which of the variables are assumed to be measurements or observed variables. For theASIA example we will assume below that A and D were observed and that we want to do inference on theremaining variables, but other combinations of observations are possible. The joint probability for the ASIAexample can simply be read off from the graph:
P(A,S,T,L,E,B,X ,D) = P(A)P(S)P(T |A)P(L|S)P(E|T,L)P(B|S)P(X |E)P(D|E,B)
where the conditional probability tables are given in Table 4.1.
Asia
15Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
P(¬a) P(a)
0.99 0.01
(a) Asia
P(¬s) P(s)
0.50 0.50
(b) Smoking
A P(¬t|A) P(t|A)
0 0.99 0.01
1 0.95 0.05
(c) Tuberculosis
S P(¬l|S) P(l|S)
0 0.99 0.01
1 0.90 0.10
(d) Lung Cancer
T L P(¬e|T,L) P(e|T,L)
0 0 1.00 0.00
0 1 0.00 1.00
1 0 0.00 1.00
1 1 0.00 1.00
(e) E = T !L
S P(¬b|S) P(b|S)
0 0.70 0.30
1 0.40 0.60
(f) Bronchitis
E P(¬x|E) P(x|E)
0 0.95 0.05
1 0.02 0.98
(g) X-Ray
E B P(¬d|E,B) P(d|E,B)
0 0 0.90 0.10
0 1 0.20 0.80
1 0 0.30 0.70
1 1 0.10 0.90
(h) Dyspnoea
Table 4.1: Conditional probability tables for the Bayes net in Figure 4.2.
4.1 Representation
4.1.1 Bayesian Networks
A Bayesian network (Pearl, 1988) or Bayes net is a directed graphical model where the nodes representvariables x with conditional densities P(x|�) on them, and the edges indicate on which variables � we areconditioning. The joint probability density P(X) over all variables X = (x1 . . .xn) is then defined as theproduct of all the conditionals associated with the nodes:
P(X) =�iP(xi|�i) (4.1)
Bayes nets are an excellent language in which to think about and specify a generative model. They easilylend themselves to thinking about how a given measurement z depends on the parameters �. Importantly,they allow us to factor the joint density P(�,Z) into a product of simple factors. This is typically done byspecifying conditional densities of the form P(z|�), indicating that given the values for a subset � ! � ofthe parameters, the measurement z is conditionally independent of all other parameters.
A
D
T
S
L
BE
X
Figure 4.2: Bayes nets for example in Figure 4.1. The observed variables are shown as square nodes.
Example 11. The Bayes net for our running example from Figure 4.1 is shown in Figure 4.2. In the figureI have indicated which of the variables are assumed to be measurements or observed variables. For theASIA example we will assume below that A and D were observed and that we want to do inference on theremaining variables, but other combinations of observations are possible. The joint probability for the ASIAexample can simply be read off from the graph:
P(A,S,T,L,E,B,X ,D) = P(A)P(S)P(T |A)P(L|S)P(E|T,L)P(B|S)P(X |E)P(D|E,B)
where the conditional probability tables are given in Table 4.1.
Asia Conditional Probability Tables (CPTs)
16Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 17
Simultaneous Localization and Mapping (SLAM)
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 18
Continuous Probability Densities
x0 x1 x2 xM...
lNl1 l2...
...
P(X,M) = k*
- Trajectory of Robot
- Landmarks
- Measurements
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 19
Factor Graph -> Smoothing and Mapping (SAM) !Variables are multivariate! Factors are non-linear!
Factors induce probability densities.-log P(x) = |E(x)|2
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Factor Graph Representation of Bundle Adjustment
l⇥ l⇤ l⌅ l⇧ l⌃
l⇥
l⇤
l⌅l⇧ l⌃
8 / 4020
Visual S(L)AM aka Bundle Adjustment
Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Example: Underwater 3D Reconstruction
21
Joint Work with with Stefan Williams, Ian Mahon, ACFR @ University of SydneyData collected by ACFR
Presented at Oceans’11, Santander
• Downward facing stereo pair with 7cm baseline
• 50 x 75m survey area, 2m altitude
• 1Hz, 50% frame overlap
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Resulting Factor Graph is Big
22
Factor graph contains 10K camera poses, 200K 3D points, and 350K factors
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 23Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
“Gaussian” Elimination?
24
The Nine Chapters on the Mathematical Art, 200 B.C.
Gauss re-invented elimination for determining the orbit of Ceres
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
H
O 25
Graph Coloring using Direct Elimination
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
Elimination Algorithm
• Choose ordering• Eliminate variables
one at a time
26Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• For every color of A (Kansai),
choose a color for C (Chugoku)• Show as directed edge from
parent (A) to child (C)
A CR G/BG R/BB R/G
27
Elimination Algorithm
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• For every color of A (Kansai),
choose a color for S (Shikoku)• Show as directed edge from
parent (A) to child (S)
28
Elimination Algorithm
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• For every color of U
(Chubu), choose a color for A (Kansai)
• Show as directed edge from parent (U) to child (A)
29
Elimination Algorithm
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• For every combination of T and
K, choose a color for U (Chubu)• Two directed edges from
parents T and K to child (U)
New Factor:Alldiff(T,U,K)replaced by Alldiff (T,K)
30
Elimination Algorithm
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
T K U R R R ∞R R G ∞R R B ∞R G R ∞R G G ∞R G B 0R B R ∞R B G 0R B B ∞G R R ∞G R G ∞G R B 0G G R ∞G G G ∞G G B ∞G B R 0G B G ∞G B B ∞B R R ∞B R G 0B R B ∞B G R 0B G G ∞B G B ∞B B R ∞B B G ∞B B B ∞
Original Factor
31
Elimination Detail
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
T K ! UR R ! -R G ! BR B ! GG R ! BG G ! -G B ! RB R ! GB G ! RB B ! -
T K U R R R ∞R R G ∞R R B ∞R G R ∞R G G ∞R G B 0R B R ∞R B G 0R B B ∞G R R ∞G R G ∞G R B 0G G R ∞G G G ∞G G B ∞G B R 0G B G ∞G B B ∞B R R ∞B R G 0B R B ∞B G R 0B G G ∞B G B ∞B B R ∞B B G ∞B B B ∞
Original Factor Lookup Table
argmin
32
Elimination Detail
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
T K ! UR R ! -R G ! BR B ! GG R ! BG G ! -G B ! RB R ! GB G ! RB B ! -
T K U R R R ∞R R G ∞R R B ∞R G R ∞R G G ∞R G B 0R B R ∞R B G 0R B B ∞G R R ∞G R G ∞G R B 0G G R ∞G G G ∞G G B ∞G B R 0G B G ∞G B B ∞B R R ∞B R G 0B R B ∞B G R 0B G G ∞B G B ∞B B R ∞B B G ∞B B B ∞
Original Factor Lookup Table
argmin
T KR R ∞R G 0R B 0G R 0G G ∞G B 0B R 0B G 0B B ∞
min
New Factor 33
Elimination Detail
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• For every color of T (Tohoku),
choose a color for K (Kanto)• Show as directed edge from
parent (T) to child (K)
34
Elimination
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• C,S,A,U,K,T• T (Tohoku) has no constraints left:
choose any color• Elimination finished: Directed
Acyclic Graph (DAG)
35
Elimination Last Step
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• Depends on variable ordering C,S,A,U,K,T
• Max number of parents of any node in the DAG
• Determines Computational Complexity T K U
R R -R G BR B GG R BG G -G B RB R GB G RB B -
A CR G/BG R/BB R/G
36
Tree Width
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• Try T,K,U,A,S,CTree width?Table sizes?
• Try C,S,U,T,K,ATree width?Table sizes?
37
Variable Ordering
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
U K
T
S
AC
• Try T,K,U,A,S,CTree width? 2,1,1,2,1,0Table sizes? 9,3,3,9,3,1
• Try C,S,U,T,K,ATree width? 1,1,3,2,1,0Table sizes? 3,3,27,9,3,1
38
Variable Ordering
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
A T K R R R ! -R R G ! BR R B ! GR G R ! BR G G ! -R G B ! -R B R ! GR B G ! -R B B ! -G R R ! -G R G ! BG R B ! -G G R ! BG G G ! -G G B ! RG B R ! -G B G ! RG B B ! -B R R ! -B R G ! -B R B ! GB G R ! -B G G ! -B G B ! RB B R ! GB B G ! RB B B ! -
U K
T
S
AC
39
Eliminating U
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 40Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 41
Continuous Probability Densities
x0 x1 x2 xM...
lNl1 l2...
...
P(X,M) = k*
- Trajectory of Robot
- Landmarks
- Measurements
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 42
Linearize: Linear Factor Graph
x0 x1 x2 xM...
lNl1 l2...
...
-log P(X,M|Z) =
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 43
Linear Factor Graph == mxn Matrix
A
Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 44334x334, 9606 nnz
R
Linear Least Squares
334x334, 7992 nnz
ATA
1129x334, 5917 nnz
A
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 45
Markov Random Field
ATA
Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 46
Square Root Factorization
A QR R
Cholesky RATA
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Intel Dataset
47
910 poses, 4453 constraints, 45s or 49ms/step
(Olson, RSS 07: iterative equation solver, same ballpark)
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
MIT Killian Court Dataset
48
1941 poses, 2190 constraints, 14.2s or 7.3ms/step
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 49
QR Factorization as Variable Elimination
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3
l1 l2
Basis = Chain Rule ! e.g. P(l1,x1,x2)=P(l1|x1,x2)P(x1,x2)
P(l1,x1,x2)
l1 l2 x1 x2 x3xx x x x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3
l1 l2l1
Small Example
50
P(l1|x1,x2)
P(x1,x2)
Basis = Chain Rule ! e.g. P(l1,x1,x2)=P(l1|x1,x2)P(x1,x2)
l1 l2 x1 x2 x3x x x x x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3
l1 l2l2
Small Example
51
l1 l2 x1 x2 x3x x x x x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3x1
l1 l2
Small Example
52
l1 l2 x1 x2 x3x x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3x2
l1 l2
Small Example
53
l1 l2 x1 x2 x3x x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
• Choose Ordering• Eliminate one node at a time
• Express node as function of adjacent nodes
x1 x2 x3x3
l1 l2
Small Example
54
l1 l2 x1 x2 x3x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
x1 x2 x3x3
l1 l2
Small Example
55
• End-Result = Chordal Bayes Net: l1 l2 x1 x2 x3x x x x x x x x x x
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 56
Elimination order is again CRUCIAL
• Order with least fill-in NP-complete to find
Square Root SAM: Simultaneous Location and Mapping via Square Root Information Smoothing, Frank Dellaert, Robotics: Science and Systems, 2005Exploiting Locality by Nested Dissection For Square Root Smoothing and Mapping, Peter Krauthausen, Frank Dellaert, and Alexander Kipp, Robotics: Science and Systems, 2006
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 57
Remember: Larger Example Factor Graph
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 58
Eliminate Poses then LandmarksR
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 59
Eliminate Landmarks then Poses
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 60
Minimum Degree FillYields Chordal Graph. Chordal graph + order = directedR
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 61
Filtering (EKF SLAM): Eliminate all but last pose
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Underwater SLAM Example
62
Factor graph contains 10K camera poses, 200K 3D points, and 350K factors
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Results
63
Filtering
SAM
Bundle adjustment in large-scale 3D reconstructions based on underwater robotic surveys, Chris Beall, Frank Dellaert, Ian Mahon, and Stephen Williams, Oceans '11, 2011
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Projection Errors
64
Filtering SAM
The root mean square errors (RMSE) for EIF and SAM solutions are 8.32and 0.26, respectively
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Tectonic SAM
65Multi-Level Submap Based SLAM Using Nested Dissection, Kai Ni and Frank Dellaert, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2010
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
DDF-SAM and Anti-factors
66
DDF-SAM 2.0: Consistent Distributed Smoothing and Mapping, Alexander Cunningham, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2013
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 67Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
As a first step to perform inference, we take a Bayes net corresponding to the generative model andconvert it to a factor graph. However, because inference assumes known measurements Z, we drop them asvariables. Instead, the conditional densities P(zk|�k) on them are converted into likelihood factors
fk(�k)�= L(�k;zk) � P(zk|�k)
where zk acts as a parameter, not a variable. After this conversion, the factor graph represents the posteriorP(�|Z) as the product of the prior P(�) and the likelihood factors L(�k;zk),
P(�|Z) � P(�)�kL(�k;zk) = f0(�0)�
kfk(�k) (4.2)
where the factor f0 corresponding to the prior P(�) typically factors as well, depending on the application.
T
S
L
BE
X
Figure 4.3: Factor graph for examples in Figure 4.2.
Example 12. As an example, the Bayes net from Figure 4.2 is converted into the factor graph shown inFigure 4.3. Note that the observed variables do not appear as variables in the factor graph. Instead, theconstraint induced on the other variables by the observations is represented by corresponding factors.
4.2 Inference
4.2.1 Computations Supported by Factor Graphs
Converting to a factor graph with measurements as parameters yields a representation of the posteriorP(�|Z), given by Equation 4.2, and it is natural to ask what we can do with this.
Given any factor graph defining an unnormalized density f (�) we can easily evaluate it for any givenvalue of the parameters �. However, it is not obvious how to sample from the corresponding density
4.1 Representation
4.1.1 Bayesian Networks
A Bayesian network (Pearl, 1988) or Bayes net is a directed graphical model where the nodes representvariables x with conditional densities P(x|�) on them, and the edges indicate on which variables � we areconditioning. The joint probability density P(X) over all variables X = (x1 . . .xn) is then defined as theproduct of all the conditionals associated with the nodes:
P(X) =�iP(xi|�i) (4.1)
Bayes nets are an excellent language in which to think about and specify a generative model. They easilylend themselves to thinking about how a given measurement z depends on the parameters �. Importantly,they allow us to factor the joint density P(�,Z) into a product of simple factors. This is typically done byspecifying conditional densities of the form P(z|�), indicating that given the values for a subset � ! � ofthe parameters, the measurement z is conditionally independent of all other parameters.
A
D
T
S
L
BE
X
Figure 4.2: Bayes nets for example in Figure 4.1. The observed variables are shown as square nodes.
Example 11. The Bayes net for our running example from Figure 4.1 is shown in Figure 4.2. In the figureI have indicated which of the variables are assumed to be measurements or observed variables. For theASIA example we will assume below that A and D were observed and that we want to do inference on theremaining variables, but other combinations of observations are possible. The joint probability for the ASIAexample can simply be read off from the graph:
P(A,S,T,L,E,B,X ,D) = P(A)P(S)P(T |A)P(L|S)P(E|T,L)P(B|S)P(X |E)P(D|E,B)
where the conditional probability tables are given in Table 4.1.
Factor Graph Form of Bayes Nets (after evidence)
68Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
T
S
L
BE
XX
T
S
L
BE
X
T T
S
L
BE
X
S
T
S
L
BE
X
E
T
S
L
BE
X
L T
S
L
BE
X
B
Figure 4.4: Elimination of the ASIA factor graph in Figure 4.3 into a Bayes net, using the ordering X , T , S,E , L, B. At each stage the shaded node xj is eliminated from the factor graph and the conditional densityP(x j|Sj) is added to the Bayes net, while a newly created factor fnew(Sj) (shown in red) summarizes thebelief about the separator Sj and is added to the graph.
Elimination
69Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
T
S
L
BE
X
Figure 4.5: Chordal Bayes nets for the ASIA example using less advantageous elimination ordering. Theordering used was E , L, S, T , B, X for the ASIA network.
T
S
L
BE
XX
T
S
L
BE
X
T T
S
L
BE
X
S
T
S
L
BE
X
E
T
S
L
BE
X
L T
S
L
BE
X
B
Figure 4.4: Elimination of the ASIA factor graph in Figure 4.3 into a Bayes net, using the ordering X , T , S,E , L, B. At each stage the shaded node xj is eliminated from the factor graph and the conditional densityP(x j|Sj) is added to the Bayes net, while a newly created factor fnew(Sj) (shown in red) summarizes thebelief about the separator Sj and is added to the graph.
X,T,S,E,L,B E,L,S,T,B,X
Ordering matters here as well !
70Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 71
Bayes Network Structure?
• Upper-triangular = DAG !A
E
D
T
X
L
S
D X E L T S AD X X XX X XE X X X XL X X XT X X XS X XA X
DAG
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 72
New: Bayes Tree
TSA
EL:TSD:ESX:E
P(D|ES)P(X|E)
P(EL|TS)
P(TSA)
Chapter 3
Bayes Trees
3.1 Bayes Trees Defined
Performing marginalization and optimization in Bayes nets is not easy in general, even forevidence-free Bayes nets. However, chordal Bayes nets have the property that they can easilybe converted into a tree-structured graphical model which I call a Bayes tree, in which theseoperations are easy.
A Bayes tree is a directed tree where the nodes represents cliques Ci of an underlying chordalBayes net. In this respect Bayes trees are similar to clique trees (Blair and Peyton, 1993) orjunction trees (Dechter and Pearl, 1989). However, a Bayes tree is directed and is closer to aBayes net in the way it encodes a factored probability density. In particular, as in Bayes nets wedefine one conditional density P(Fi|Si) per node, where we use the separator Si as the intersectionCi !�i of the clique Ci and its parent clique �i, and the frontal variables Fi as the remainingvariables, i.e., Fi
�=Ci \ Si. We write Ci = Fi : Si. This leads to the following expression for thejoint density P(�) on the variables � defined by a Bayes tree,
P(�) =�iP(Fi|Si) (3.1)
where for the root Fr the separator is empty, i.e., it is a simple prior P(Fr) on the root variables.The way Bayes trees are defined, the separator Si for a clique Ci is always a subset of the parentclique �i, and hence the directed edges in the graph have the same semantic meaning as in aBayes net: conditioning.
21
Frontal Nodes : SeparatorEach clique:
Tree of Cliques:A
E
D
T
X
L
S
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 73
Easy, Recursive Algorithms
TSA
EL:TS
D:ESX:E4.P(DES)3.P(XE)
2.P(ELTS)
1.P(TSA)
TSA
EL:TS
D:ESX:E
3.ELTS*
4.TSA*
Marginalization or sampling:
Optimization:
2.DES*1.XE*
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 74
Larger Example, EliminationYields Chordal Graph = Triangulated Graph
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 75
Bayes Tree = Structure of R
R ==
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 76
Marginals Easily Recovered
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 77
Bayes Tree and the Cholesky Factor
TSA
EL:TSD:ESX:E
P(D|ES)P(X|E)
P(EL|TS)
P(TSA)
• Bayes Tree clique == block-row in R• Frontal nodes:
• eliminated together (multifrontal QR or Cholesky)• leading triangles
Tree of Cliques:D X E L T S AD X X XX X XE X X X XL X X XT X X XS X XA X
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Bayes Tree vs. Junction Tree
78
• Junction trees are typically created to structure a sum-product/belief propagation computa-tion, which proceeds in two passes after the junction tree has been created.
• The creation of a Bayes tree through elimination and then MCS corresponds to one pass ofbelief propagation, whereas marginalization corresponds to the second pass.
• A variant of sum-product, max-product, is often discussed to perform optimization ratherthan marginalization. Similarly, elimination can be modified to create functions of theseparator rather than conditionals. These can then be collected in a tree of functions F !
i (Si)that were defined in Section (3.3.3).
• Bayes trees have a more concise factored joint given by (3.1), computed when the Bayestree is created from the chordal Bayes net.
• The creation of a Bayes tree from a factor graph corresponds exactly to factorization inlinear algebra.
3.2.1 Example: Visit to ASIA
Figure 3.3: Bayes tree and junction tree for the robotics example from Figure 3.2.
The Bayes tree and junction tree corresponding to the chordal Bayes net in Figure 2.2 areshown side-by-side in Figure 3.3. The posterior P(X ,L|Z) is factored in two ways:
P(X ,L|Z) = P(l1,x1|x2)P(l2|x3)P(x2,x3)
= P(l1,x1,x2)1
P(x2)P(x2,x3)
1P(x3)
P(l2,x3)
The former can be read off from the Bayes tree on the left of Figure 3.3, and the latter from thejunction tree on the right.
• Junction trees are typically created to structure a sum-product/belief propagation computa-tion, which proceeds in two passes after the junction tree has been created.
• The creation of a Bayes tree through elimination and then MCS corresponds to one pass ofbelief propagation, whereas marginalization corresponds to the second pass.
• A variant of sum-product, max-product, is often discussed to perform optimization ratherthan marginalization. Similarly, elimination can be modified to create functions of theseparator rather than conditionals. These can then be collected in a tree of functions F !
i (Si)that were defined in Section (3.3.3).
• Bayes trees have a more concise factored joint given by (3.1), computed when the Bayestree is created from the chordal Bayes net.
• The creation of a Bayes tree from a factor graph corresponds exactly to factorization inlinear algebra.
3.2.1 Example: Visit to ASIA
Figure 3.3: Bayes tree and junction tree for the robotics example from Figure 3.2.
The Bayes tree and junction tree corresponding to the chordal Bayes net in Figure 2.2 areshown side-by-side in Figure 3.3. The posterior P(X ,L|Z) is factored in two ways:
P(X ,L|Z) = P(l1,x1|x2)P(l2|x3)P(x2,x3)
= P(l1,x1,x2)1
P(x2)P(x2,x3)
1P(x3)
P(l2,x3)
The former can be read off from the Bayes tree on the left of Figure 3.3, and the latter from thejunction tree on the right.
Figure 3.2: Bayes tree for the chordal Bayes net from Figure 2.3. Note that the maximum cliquesize is larger.
In our running robot mapping example, the chordal Bayes net in Figure 2.2 is converted usingMCS into the Bayes tree shown in Figure 3.2, left. The posterior P(X ,L|Z) is factored by theBayes tree as
P(X ,L|Z) = {P(l1|x1,x2)P(x1|x2)}{P(l2|x3)}{P(x2|x3)P(x3)}
= P(l1,x1|x2)P(l2|x3)P(x2,x3)
The latter can be easily read off Figure 3.2, and the equality above shows the correspondenceof each clique conditional to the finer chordal Bayes net factors from Figure 2.2. In Figure 3.2,right, I show the Bayes tree for the chordal Bayes net corresponding to the reverse ordering, fromFigure 2.3. Note that the tree width, defined as the maximum clique size minus one, is now equalto three.
3.2 Relationship to Junction Trees
Since in the marginalization step above we obtain the marginals P(S i) over every separator Si, aswell as the clique marginals P(Ci), we can express the Bayes tree joint probability (3.1) in termsof those marginals:
P(�) =�iP(Fi|Si) =�
i
P(Fi,Si)P(Si)
=�i
P(Ci)P(Si)
The last form above is exactly the joint probability of a junction tree, introduced for probabilisticreasoning in Dechter and Pearl (1989). Junction trees are undirected trees of the cliques C i ina chordal graph, with the edges Ei j between overlapping cliques Ci and Cj annotated by theseparatorsCi!Cj. The differences between a Bayes tree and a junction tree are given below.
• The Bayes tree arises naturally from the elimination algorithm, which has an inherent or-dering associated with it that is maintained in the Bayes tree.
Also: elimination + marginalization == exact belief propagation (sum product)
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 79Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Of Eiffel Towers....
80Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Sudoku’s
• Tree width ?
81Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Iterative Methods
Better than direct methods for large problems because theyPerform only simple operations and no variable eliminationRequire constant (linear) memory
But they may converge slowlywhen the problem is ill-conditioned
The conjugate gradient method: The convergence speed is related tothe condition number (AtA) = �
max
/�min
13 / 4082Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Recap.
So far we know that ...
Direct methods work well for sparse graphs
Iterative methods are simple and memory-e�cient
Can we combine their advantages?
15 / 4083Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Linear Reparametrization is Preconditioning
The preconditioned conjugate gradient (PCG) method will convergefaster if the new problem becomes well-conditioned
18 / 4084Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 85
Boolean Factor Graphs Revisited
“If a unicorn is mythical, then it is immortal, but if it is not mythical, then it is a mortal mammal. If a unicorn is either immortal or a mammal, then it is horned. A unicorn is magical if it is horned.”
M I
A H G
M ! I
¬M ! (¬I " A)
(I # A) ! H H ! G
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Conjuctive Normal Form (CNF)
M I
A H G
¬M ! I
M ! ¬I
M ! A
¬I ! H
¬A ! H ¬H ! G
86Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Boolean Satisfiability as Elimination, start w M
M I
A H G
(¬M ! I) " (M ! ¬I) " (M ! A)
I ! A ¬I ! H
¬A ! H ¬H ! G
87Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Elimination of I
M I
A H G
(¬M ! I) " (M ! ¬I) " (M ! A)
(I ! A) " (¬I ! H)
A ! H
¬A ! H ¬H ! G
88Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Elimination of A
M I
A H G
(¬M ! I) " (M ! ¬I) " (M ! A)
(I ! A) " (¬I ! H)
HH
¬H ! G
89Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Elimination of H
M I
A H G
(¬M ! I) " (M ! ¬I) " (M ! A)
(I ! A) " (¬I ! H)
H HG
90Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Elimination of G
M I
A H G
(¬M ! I) " (M ! ¬I) " (M ! A)
(I ! A) " (¬I ! H)
H H G
91Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Render readable again (convert from CNF)
M I
A H G
(I ! M) " (¬A # M)
(¬A # I) " (¬H # ¬I)
H H G
92Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Cutset Conditioning (from CSP Literature)
M I
A H G
¬M ! I
M ! ¬I
M ! A
¬I ! H
¬A ! H ¬H ! G
93Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Cutset Conditioning
M I
A H G
I = false
¬M
M ! A
¬A ! H ¬H ! G
94Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Cutset Conditioning
M I
A H G
¬M I = false
A H G
95Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Cutset Conditioning
M I
A H G
I = true
M
M ! A H
¬A ! H ¬H ! G
96Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Cutset Conditioning
M I
A H G
I = trueM
A ! ¬A H G
97Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Main Idea (with Viorela Ila and Doru Balcan)
0 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 3151
0 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 286240 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 25473
The whole problem The easy part (subgraph)
The hard part(loop closures)
Direct
Method
Iterat
ive M
ethod
= +
98Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Variable Reparametrization (1)
Variable reparametrization could result in faster convergence
Incremental pose reparametrization on the odometry chain reducesthe time to propagate the loop closure constraints [Olson et al. ICRA’06]
16 / 4099Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Variable Reparametrization (2)
Reparametrizing the problem with a spanning tree works even better[Grisetti et al. RSS’07, Dellaert et al. IROS’10]
y=Rx
17 / 40100Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Eiffel Tower Problem
101
!"
#!"
$!!"
$#!"
%!!"
%#!"
&!!"
&#!"
#!" $!!"$#!"%!!"%#!"&!!"&#!"'!!"'#!"#!!"##!"(!!"(#!")!!")#!"*!!"
+,-."
+,/0"
10000 poses, up to 800 Eiffel towers (SPCG averages 1.2 seconds)
Subgraph-preconditioned Conjugate Gradients for Large Scale SLAM, Frank Dellaert, Justin Carlson, Viorela Ila, Kai Ni, and Charles E. Thorpe, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2010
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Combine the Best of Direct and Iterative Methods:Subgraph Preconditioners
Key idea: use a sparse subgraph as the preconditioner
l⇥ l⇤ l⌅ l⇧ l⌃
l⇥ l⇤ l⌅ l⇧ l⌃
Sparsity ensures the e�ciency of direct methods
The e↵ectiveness of subgraph preconditioners can be characterized bythe Support Theory [Boman et al. JMAA’03]
19 / 40102Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Generalized Subgraph Preconditioners for Large-Scale Bundle Adjustment, Yong-Dian Jian, Doru Balcan, and Frank Dellaert, IEEE International Conference on Computer Vision (ICCV), 2011
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Original, max
103Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Original, min
104Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Preconditioner
105Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Preconditioned, max
106Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Preconditioned, min
107Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Illustration with Real Data
Reconstruction results of a street scene dataset [Agarwal et al. ECCV’10]
Solution to a sparse subgraphAn approximated solutionE�cient to compute
Solution to the entire graphThe optimal solutionExpensive to compute directly
Use the subgraph solution to reparametrize the original problem21 / 40108Frank Dellaert: Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Solution from Subgraph Solution from the Original Graph
Data from Noah Snavely
Notre Dame
109Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Solution from Subgraph Solution from the Original Graph
Data from Sameer Agarwal
Piazza San Marco
110Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Solution from Subgraph Solution from the Original Graph
Data from Grant Schindler111
Chicago Downtown
Friday, May 10, 13
Questions?
Intro
Factor Graphs
Elimination Algorithm
Factorization & SLAM
The Bayes Tree
Subgraphs
GTSAM
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 112Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
GTSAM: Georgia Tech Smoothing and Mapping (2.3.0)
• BSD-Licensed
113
tinyurl.com/gtsam
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Pose SLAM
114Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Pose SLAM (MATLAB version)
115Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Pose SLAM (optimize and marginalize)
116Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
Planar SLAM (with landmarks)
117Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
SPCG
118
// optimize using SPCGresult = optimizeSPCG(graph, initialEstimate);
Subgraph-preconditioned Conjugate Gradients for Large Scale SLAM, Frank Dellaert, Justin Carlson, Viorela Ila, Kai Ni, and Charles Thorpe, IROS 2010
0 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 3151
0 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 28624 0 0.5 1 1.5 2 2.5
x 104
0
0.5
1
1.5
2
2.5
x 104
nz = 25473
=
=
+
+
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013
And just in case you’re wondering....
119
Sudoku sudoku(9, 9,5,0, 0,0,6, 0,0,0, 0,8,4, 0,7,0, 0,0,0, 6,2,0, 5,0,0, 4,0,0,
0,0,0, 2,9,0, 6,0,0, 0,9,0, 0,0,0, 0,2,0, 0,0,2, 0,6,3, 0,0,0,
0,0,9, 0,0,7, 0,6,8, 0,0,0, 0,3,0, 2,9,0, 0,0,0, 1,0,0, 0,3,7);
// Do BP sudoku.runArcConsistency(9,10,PRINT);
//sudoku.printSolution(); // don't do it
Yes, GTSAM does sudokus, but direct elimination does not fare well :-)
(Currently unreleased)
Friday, May 10, 13
Frank Dellaert: Inference in Factor Graphs, Tutorial at ICRA 2013 120Friday, May 10, 13