inference in factor graphs - college of computingdellaert/pub/2013-05-10-icra-tutorial.pdf · frank...

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


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


4D Reconstruction, with Grant Schindler

Historical Image Collection

5

4D City Model

3D Structure + Time IntervalsCamera Pose + Time

4D Model

Friday, May 10, 13


4D Structure over Time

6

Friday, May 10, 13


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


3D Models from Community Databases

• E.g., Google image search on “Dubrovnik”

8Friday, May 10, 13


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


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


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


Constraint Satisfaction Problems



U K

T

S

AC

H

O 14

Constraint Satisfaction Problems

Friday, May 10, 13


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



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



P(X) =�iP(xi|�i) (4.1)


A

D

T

S

L

BE

X





Asia Conditional Probability Tables (CPTs)



Simultaneous Localization and Mapping (SLAM)

Friday, May 10, 13


Continuous Probability Densities

x0 x1 x2 xM...

lNl1 l2...

...

P(X,M) = k*

- Trajectory of Robot

- Landmarks

- Measurements

Friday, May 10, 13


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


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


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


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



The Bayes Tree

Subgraphs

GTSAM



“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


U K

T

S

AC

H

O 25

Graph Coloring using Direct Elimination

Friday, May 10, 13


U K

T

S

AC


• Choose ordering• Eliminate variables

one at a time



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


Friday, May 10, 13


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


Friday, May 10, 13


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


Friday, May 10, 13


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


Friday, May 10, 13


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


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 ! -


Original Factor Lookup Table

argmin

32

Elimination Detail

Friday, May 10, 13


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 ! -


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


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


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


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


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


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


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



The Bayes Tree

Subgraphs

GTSAM



Continuous Probability Densities

x0 x1 x2 xM...

lNl1 l2...

...

P(X,M) = k*

- Trajectory of Robot

- Landmarks

- Measurements

Friday, May 10, 13


Linearize: Linear Factor Graph

x0 x1 x2 xM...

lNl1 l2...

...

-log P(X,M|Z) =

Friday, May 10, 13


Linear Factor Graph == mxn Matrix

A


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


Markov Random Field

ATA


Friday, May 10, 13


Square Root Factorization

A QR R

Cholesky RATA

Friday, May 10, 13


Intel Dataset

47

910 poses, 4453 constraints, 45s or 49ms/step

(Olson, RSS 07: iterative equation solver, same ballpark)

Friday, May 10, 13


MIT Killian Court Dataset

48

1941 poses, 2190 constraints, 14.2s or 7.3ms/step

Friday, May 10, 13


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




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




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




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




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




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


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


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


Remember: Larger Example Factor Graph

Friday, May 10, 13


Eliminate Poses then LandmarksR

Friday, May 10, 13


Eliminate Landmarks then Poses

Friday, May 10, 13


Minimum Degree FillYields Chordal Graph. Chordal graph + order = directedR

Friday, May 10, 13


Filtering (EKF SLAM): Eliminate all but last pose

Friday, May 10, 13


Underwater SLAM Example

62

Factor graph contains 10K camera poses, 200K 3D points, and 350K factors

Friday, May 10, 13


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

http://frank.dellaert.com/pubs/Beall11oceans.pdf




http://borg.cc.gatech.edu/people/cbeall3

http://borg.cc.gatech.edu/people/cbeall3

http://frank.dellaert.com/


http://www.stephenwilliams.info/

http://www.stephenwilliams.info/


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


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

http://frank.dellaert.com/pubs/Ni10iros.pdf

http://frank.dellaert.com/pubs/Ni10iros.pdf

http://www.cc.gatech.edu/~nikai




http://www.iros2010.org.tw/



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

http://www.prism.gatech.edu/~acunningham7/papers/Cunningham13icra.pdf

http://www.prism.gatech.edu/~acunningham7/papers/Cunningham13icra.pdf

http://www.prism.gatech.edu/~acunningham7

http://www.prism.gatech.edu/~acunningham7

http://www.cc.gatech.edu/~vindelma/

http://www.cc.gatech.edu/~vindelma/



http://www.icra2011.org/

http://www.icra2011.org/

Questions?

Intro

Factor Graphs



The Bayes Tree

Subgraphs

GTSAM



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



P(X) =�iP(xi|�i) (4.1)


A

D

T

S

L

BE

X





Factor Graph Form of Bayes Nets (after evidence)



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



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 !



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


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


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


Larger Example, EliminationYields Chordal Graph = Triangulated Graph

Friday, May 10, 13


Bayes Tree = Structure of R

R ==

Friday, May 10, 13


Marginals Easily Recovered

Friday, May 10, 13


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


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



The Bayes Tree

Subgraphs

GTSAM



Of Eiffel Towers....



Sudoku’s

• Tree width ?



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


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?


Friday, May 10, 13


Linear Reparametrization is Preconditioning

The preconditioned conjugate gradient (PCG) method will convergefaster if the new problem becomes well-conditioned


Friday, May 10, 13


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


Conjuctive Normal Form (CNF)

M I

A H G

¬M ! I

M ! ¬I

M ! A

¬I ! H

¬A ! H ¬H ! G



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



Elimination of I

M I

A H G

(¬M ! I) " (M ! ¬I) " (M ! A)

(I ! A) " (¬I ! H)

A ! H

¬A ! H ¬H ! G



Elimination of A

M I

A H G

(¬M ! I) " (M ! ¬I) " (M ! A)

(I ! A) " (¬I ! H)

HH

¬H ! G



Elimination of H

M I

A H G

(¬M ! I) " (M ! ¬I) " (M ! A)

(I ! A) " (¬I ! H)

H HG



Elimination of G

M I

A H G

(¬M ! I) " (M ! ¬I) " (M ! A)

(I ! A) " (¬I ! H)

H H G



Render readable again (convert from CNF)

M I

A H G

(I ! M) " (¬A # M)

(¬A # I) " (¬H # ¬I)

H H G



Cutset Conditioning (from CSP Literature)

M I

A H G

¬M ! I

M ! ¬I

M ! A

¬I ! H

¬A ! H ¬H ! G



Cutset Conditioning

M I

A H G

I = false

¬M

M ! A

¬A ! H ¬H ! G



Cutset Conditioning

M I

A H G

¬M I = false

A H G



Cutset Conditioning

M I

A H G

I = true

M

M ! A H

¬A ! H ¬H ! G



Cutset Conditioning

M I

A H G

I = trueM

A ! ¬A H G



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

= +



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]


Friday, May 10, 13


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


Friday, May 10, 13


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

http://frank.dellaert.com/pubs/Dellaert10iros.pdf




http://www.iri.upc.edu/people/vila/Welcome.html

http://www.iri.upc.edu/people/vila/Welcome.html



http://www.ri.cmu.edu/people/thorpe_chuck.html

http://www.ri.cmu.edu/people/thorpe_chuck.html






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]


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

http://frank.dellaert.com/pubs/Jian11iccv.pdf

http://frank.dellaert.com/pubs/Jian11iccv.pdf

http://www.cc.gatech.edu/~yjian6/

http://www.cc.gatech.edu/~yjian6/

http://www.cc.gatech.edu/~dbalcan/

http://www.cc.gatech.edu/~dbalcan/



http://research.microsoft.com/iccv2005

http://research.microsoft.com/iccv2005


Original, max



Original, min



Preconditioner



Preconditioned, max



Preconditioned, min



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


Solution from Subgraph Solution from the Original Graph

Data from Noah Snavely

Notre Dame




Data from Sameer Agarwal

Piazza San Marco




Data from Grant Schindler111

Chicago Downtown

Friday, May 10, 13

Questions?

Intro

Factor Graphs



The Bayes Tree

Subgraphs

GTSAM



GTSAM: Georgia Tech Smoothing and Mapping (2.3.0)

• BSD-Licensed

113

tinyurl.com/gtsam

Friday, May 10, 13

https://collab.cc.gatech.edu/borg/gtsam/

https://collab.cc.gatech.edu/borg/gtsam/


Pose SLAM



Pose SLAM (MATLAB version)



Pose SLAM (optimize and marginalize)



Planar SLAM (with landmarks)



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










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

inference in factor graphs - college of computingdellaert/pub/2013-05-10-icra-tutorial.pdf · frank...

Documents