A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 ·...
Transcript of A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 ·...
![Page 1: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/1.jpg)
A brief introductionto Factor Graphs
José Luis Blanco ClaracoEngineering Department
University of Almería
January, 9th-10th 2020
![Page 2: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/2.jpg)
Talk Schedule
Thursday Friday
10:00-11:40Terrain awareness
for planetary roversFactor graphs: Theory.
Intro to GTSAM
11:40-12:00 Coffee break
12:00-14:00 Factor graphs: Theory MATLAB exercises
Introduction to Factor Graphs - Jose Luis Blanco - UAL 2
![Page 3: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/3.jpg)
References
Introduction to Factor Graphs - Jose Luis Blanco - UAL 3
“Pattern Recognition and Machine Learning”Christopher M. Bishop, 2006
C.M. Bishop(Microsoft)
Frank Dellaert(Georgia Tech)
Michael Kaess(Carnegie Mellon University)
“Factor Graphs for Robot Perception”Frank Dellaert, Michael Kaess, 2017
![Page 4: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/4.jpg)
Probability and estimation bases
Introduction to Factor Graphs - Jose Luis Blanco - UAL 4
![Page 5: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/5.jpg)
Probability spaces
“Under both the frequentist and Bayesian paradigms,probabilities are numbers associated to certain phenomena inthe real world, in particular to the possible outcomes of somestochastic process.”
• W: Sample space.
• E: s-algebra of W.
• P: Probability function
P: E→ [0,1]
e.g. P(W)=1
Introduction to Factor Graphs - Jose Luis Blanco - UAL 5
![Page 6: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/6.jpg)
Probability Mass and Density Functions (PMF/PDF)
Discrete random variable: Normalized histogram as PMF.
Continuous random variable: Any function
Introduction to Factor Graphs - Jose Luis Blanco - UAL 6
![Page 7: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/7.jpg)
Probability Density Function (PDF) examples
Model of a PT-100 temperature probe
y: output [V]
x: actual temperature [K]
n: random additive noise
Wheeled robot motion model
Introduction to Factor Graphs - Jose Luis Blanco - UAL 7
( )y f x n= +
( )20,n N s
![Page 8: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/8.jpg)
PDFs of several variables: concepts
• Marginal pdfs:
• Joint pdf:
• Conditional pdf:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 8
Pose (x)
Velocity (v)
( )p x
( )p v
( ),p x v
( )0|p =x v v
![Page 9: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/9.jpg)
PDFs of several variables: concepts
Introduction to Factor Graphs - Jose Luis Blanco - UAL 9
Matos, Jose. (2008). UNCERTAINTY TREATMENT IN CIVIL ENGINEERING NUMERICAL MODELS.
![Page 10: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/10.jpg)
Multivariate Gaussian PDFs
Profusely used everywhere. Moment and information matrix forms.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 10
Moment form:
Information (or precision) form:
(Information matrix)
( )1
constant exp2
p
= − −
Σx x μ
![Page 11: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/11.jpg)
Relationship between joint and conditional
It can be verified that:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 11
( )( )( )
,|
p a bp b a
p a=
a b
( ) ( ) ( ), |p a b p b a p a =
Conditional
Joint Marginal
![Page 12: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/12.jpg)
PDF factorization
Given a joint pdf:
To factor it: splitting it into a product of smaller pdfs, e.g:
In large problems, we easily end up having 100s or 1000s of variables, so the dimensionality of the functions matter.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 12
( ), , c,d,ep a b
( ) ( ) ( ) ( ), ,c,d,e | d | ep a b p a p b c p=
![Page 13: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/13.jpg)
PDF factorization
Tools and rules to factorize:
• Independence:
• Conditional independence:
• Bayes’ rule:
• Law of total probability:
(“Marginalize b out”)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 13
( ) ( ) ( ),p a b p a p b a b= ⊥
( ) ( ) ( ), | | b | |p a b c p a c p c a b c= ⊥
( ) ( )( )( )
||
p b ap a b p a
p b=
( ) ( ) ( )|b
p a p a b p b
=
a b
( ) ( ) ( ), |p a b p b a p a=
![Page 14: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/14.jpg)
Likelihood function
From the Bayes’ rule:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 14
( ) ( )( )( )
||
p b ap a b p a
p b= ( ) ( ) ( )| | ;p a b p b a l b a =
Prior Posterior
Marginal
LikelihoodLikelihoodPosterior
![Page 15: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/15.jpg)
Maximum a posterior (MAP) estimation
Find the set of parameters X that maximize the likelihood of a set of observations Z.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 15
( )arg max |p =x
x x z
( )arg max ;l=x
z x
( )( )arg min log ;l= −x
z x
Log-likelihood
( )arg min f =x
x x➔ Estimation as a numerical optimization problem!
![Page 16: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/16.jpg)
Types of estimators for dynamic systems
• A dynamic system has a state vector x that evolves over time:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 16
x4 x5 x6 x7x1 x2 x3
time
![Page 17: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/17.jpg)
Types of estimators for dynamic systems
• Filters: They just estimate the last (“current”) system state.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 17
x4 x5 x6 x7x1 x2 x3
x1
REAL SYSTEM
x4 x5 x6 x7x2 x3ESTIMATION
![Page 18: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/18.jpg)
Types of estimators for dynamic systems
• Fixed-lag smoother: They estimate the last “n” system states.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 18
x4 x5 x6 x7x1 x2 x3
x1
REAL SYSTEM
x4 x5 x6 x7x2 x3ESTIMATION
![Page 19: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/19.jpg)
Types of estimators for dynamic systems
• Batch estimator: Obtain results after processing the entire dataset.
19
x4 x5 x6 x7x1 x2 x3
x1
REAL SYSTEM
x4 x5 x6 x7x2 x3ESTIMATION
Introduction to Factor Graphs - Jose Luis Blanco - UAL
![Page 20: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/20.jpg)
Graphical models. Bayes net
Introduction to Factor Graphs - Jose Luis Blanco - UAL 20
![Page 21: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/21.jpg)
Why using graphs?
Algebraic manipulation Graphical models
Introduction to Factor Graphs - Jose Luis Blanco - UAL 21
![Page 22: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/22.jpg)
Definition
The formalism of graphical models allows us to represent:
• a set of r.v.s and
• their conditional independence assumptions.
It was born as a fusion of probability theory and graph theory, andplays a central role in many machine learning techniques (Bishop, 2006).
Introduction to Factor Graphs - Jose Luis Blanco - UAL 22
![Page 23: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/23.jpg)
Taxonomy
(Non-exhaustive) taxonomy of graphical models: • Bayesian Network (BN)
• Dynamic BN (BN)
• Markov Random Field (MRF)
• Factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 23
![Page 24: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/24.jpg)
Purpose
Probabilistic graphical modelsprovide a mechanism tocompactly describe complexprobability densities by exploitingthe structure in them.
→ Efficient factorizations of PDFs
Edges in a graph are important, but the lack of them is what iseven more relevant.
Main application in our scope: inference.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 24
![Page 25: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/25.jpg)
Bayesian Network (BN)
• A directed acyclic graphical model.
• Nodes represent variables: both, knowns and unknowns.
• Directed edges carry a semantic meaning of causality.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 25
Allow encoding a human expert’s knowledge as a highly-efficient, sparse probabilistic model
![Page 26: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/26.jpg)
Bayesian Network (BN)
Mathematically, a BN encodes a factorization:
X={x1,…,xn}: all variables
xi: the i’th variable
pi: parents of the i’th variable
Introduction to Factor Graphs - Jose Luis Blanco - UAL 26
( ) ( )|i i
i
p p x=X p
![Page 27: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/27.jpg)
Bayesian Network (BN)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 27
( ) ( )|i i
i
p p x=X p
( ), , , , ?p A B G W F =
![Page 28: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/28.jpg)
Dynamic Bayesian Network (DBN)
A BN over variables that have a dynamic state over time. Implicit Markov property.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 28
![Page 29: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/29.jpg)
DBN example: vehicle localization
Introduction to Factor Graphs - Jose Luis Blanco - UAL 29
![Page 30: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/30.jpg)
Graphical models. Markov random field
Introduction to Factor Graphs - Jose Luis Blanco - UAL 30
![Page 31: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/31.jpg)
Markov Random Field (MRF)
• An undirected graphical model. May (and often will) contain loops.
• Nodes represent variables: both, knowns and unknowns.
• Edges does not carry information about causality, only about some “relationship”.
A DBN can be converted into a MRF (not covered here).
Introduction to Factor Graphs - Jose Luis Blanco - UAL 31
![Page 32: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/32.jpg)
Markov Random Field (MRF): example
Spatial Markov condition.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 32
![Page 33: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/33.jpg)
Markov Random Field (MRF) factorization
Graph clique and maximal clique Factorization of a MRF
Introduction to Factor Graphs - Jose Luis Blanco - UAL 33
![Page 34: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/34.jpg)
MRF applications: image de-noising
Noisy input Restored
Introduction to Factor Graphs - Jose Luis Blanco - UAL 34
![Page 35: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/35.jpg)
Factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 35
![Page 36: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/36.jpg)
Factor graph (FG)
• A bi-partite graphical model. Can contain loops.
• Two types of nodes: variablesand factors.
• Undirected edges: only between variables and factors.
Unary, binary,… n-ary factors.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 36
“Factor Graphs for Robot Perception”Frank Dellaert, Michael Kaess, 2017
![Page 37: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/37.jpg)
Bayes Network ➔ Factor graph
• “every node in a Bayes net denotes a conditional density on thecorresponding variable and its parent nodes. Hence, the conversion isquite simple: every Bayes net node splits in both a variable node anda factor node in the corresponding factor graph. The factor isconnected to the variable node, as well as the variable nodescorresponding to the parent nodes in the Bayes net. If some nodes inthe Bayes net are evidence nodes, i.e., they are given as knownvariables, we omit the corresponding variable nodes: the knownvariable simply becomes a fixed parameter in the correspondingfactor.” [Dellaert&Kaess, 2017]
Introduction to Factor Graphs - Jose Luis Blanco - UAL 37
![Page 38: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/38.jpg)
Bayes Network ➔ Factor graph
Bayes Network Factor graph
Introduction to Factor Graphs - Jose Luis Blanco - UAL 38
![Page 39: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/39.jpg)
Factor graph (FG) factorization
Mathematically, a FG encodes a factorization:
X={x1,…,xn}: all variables
xi: all variables touching the i’th factor
fi: the i’th factor
Introduction to Factor Graphs - Jose Luis Blanco - UAL 39
( ) ( )i i
i
p f=X x
![Page 40: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/40.jpg)
Factor graph (FG) factorization
Introduction to Factor Graphs - Jose Luis Blanco - UAL 40
( ) ( )i i
i
p f=X x
( ) ...p =X
![Page 41: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/41.jpg)
Uses of FGs
• Sampling.
• Evaluation.
• Optimization: • MAP estimator. Gradient-based (Gauss-Newton).
• How to do efficiently? (➔ Literature)
• Message-based optimization (not discussed here)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 41
![Page 42: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/42.jpg)
Error models
• Each factor model a constraint, and must include a measure of uncertainty, confidence. The “strength” in the mass-spring model.
• Most common model: Gaussian.
y = f(x) + n ➔
Problem ➔ Too strict with outliers!
Introduction to Factor Graphs - Jose Luis Blanco - UAL 42
( ) ( ) ( )1( ) ( ) ( )T
e f f f−= − = − −Σ
x x z x z Σ x z
![Page 43: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/43.jpg)
Error models: robust kernels
• Just one outlier is enough to ruin a least-squares estimator.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 43
![Page 44: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/44.jpg)
Error models: robust kernels
Robust M-estimators: based on modified cost functions.
Well-known kernels:
• Pseudo-Huber,
• Cauchy,
• Geman-McClure,
• Tukey.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 44
![Page 45: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/45.jpg)
Relationship between graphs and sparse matrix Algebra
Introduction to Factor Graphs - Jose Luis Blanco - UAL 45
![Page 46: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/46.jpg)
Motivation
• Linearization → Jacobian of factors.
• Variable ordering → Important.
• Learn about the most common nonlinear algorithms: GN, LM, DL, etc.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 46
![Page 47: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/47.jpg)
Nonlinear factor graphs
MAP of nonlinear factor graphs becomes iteratively solving Ax=b
Introduction to Factor Graphs - Jose Luis Blanco - UAL 47
“Factor Graphs for Robot Perception” Frank Dellaert, Michael Kaess, 2017
![Page 48: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/48.jpg)
Nonlinear factor graphs
Linearization:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 48
“Factor Graphs for Robot Perception” Frank Dellaert, Michael Kaess, 2017
![Page 49: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/49.jpg)
Nonlinear factor graphs
Whitening:
Introduction to Factor Graphs - Jose Luis Blanco - UAL 49
“Factor Graphs for Robot Perception” Frank Dellaert, Michael Kaess, 2017
Direct methods for LS:▪ Cholesky (+ forward and back-substitution)▪ QR
Iteration: Gauss-Newton, DogLeg, Lev-Marq.
![Page 50: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/50.jpg)
Sparsity
• Sparsity of Jacobians (and hence, Hessians H=J’J) are key for efficiency.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 50
![Page 51: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/51.jpg)
The elimination algorithm
• For a particular ordering, it recovers the Bayes Net for variables to be estimated.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 51
“Factor Graphs for Robot Perception” Frank Dellaert, Michael Kaess, 2017
![Page 52: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/52.jpg)
The elimination algorithm
• Example. Ordering: L1,L2,X1,X2,X3.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 52
![Page 53: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/53.jpg)
The importance of ordering
• Dominating cost: factorization of sparse matrices for local factors while eliminating.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 54
![Page 54: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/54.jpg)
The importance of ordering
Introduction to Factor Graphs - Jose Luis Blanco - UAL 55
J =
TJ J = =
( )chol = ( )AMDchol =
nnz=9399 nnz=4168
![Page 55: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/55.jpg)
Inference via message passing in factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 59
![Page 56: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/56.jpg)
Inference in FGs
Sum-product Algorithm (SPA): find the marginal of one variable p(xi).
Exact if tree-like. Approximate if the FG has loops.
Max-sum algorithm: find the most likely state (Viterbi decoder).
Introduction to Factor Graphs - Jose Luis Blanco - UAL 60
![Page 57: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/57.jpg)
The sum-product algorithm
Introduction to Factor Graphs - Jose Luis Blanco - UAL 61
![Page 58: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/58.jpg)
Example applications of FGs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 62
![Page 59: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/59.jpg)
Linear–quadratic regulator (LQR)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 63
(Based on GTSAM blog post by Gerry Chen and Yetong Zhang)
![Page 60: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/60.jpg)
Linear–quadratic regulator (LQR)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 64
(Based on GTSAM blog post by Gerry Chen and Yetong Zhang)
![Page 61: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/61.jpg)
Linear–quadratic regulator (LQR)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 65
(Based on GTSAM blog post by Gerry Chen and Yetong Zhang)
![Page 62: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/62.jpg)
Linear–quadratic regulator (LQR)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 66
(Based on GTSAM blog post by Gerry Chen and Yetong Zhang)
![Page 63: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/63.jpg)
Linear–quadratic regulator (LQR)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 67
(Based on GTSAM blog post by Gerry Chen and Yetong Zhang)
Equivalent to the well-known Ricatti equation solution.
![Page 64: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/64.jpg)
Nonintrusive load monitoring(NILM)• Identifying which appliances are ON/OFF from electric signals, e.g.
“load disaggregation”. W: real power, R: reactive power. Li =ON/OFF.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 68
Goutam, Y. G., Chandra, M. G., Srinivasarengan, K., &
Kadhe, S. (2013, August). On electrical load disaggregation
using factor graphs. In 2013 International Conference on
Advances in Computing, Communications and Informatics
(ICACCI) (pp. 1759-1764). IEEE.
W R
L1 L2 LN…
![Page 65: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/65.jpg)
Applications to robotics & vision
• Large systems of keyframes and observations.
• SLAM vs localization.
• SE(3) vs Euclidean
Introduction to Factor Graphs - Jose Luis Blanco - UAL 69
![Page 66: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/66.jpg)
Applications to robotics & vision
wx0wx1
wx2wx3
wx4wx5
wf1
wf2
wf3wf4
w
Introduction to Factor Graphs - Jose Luis Blanco - UAL 70
wx0
fprior(x0)
wx1wx2
wx3wx4
ffeat(f1)
wx5
ffeat(f2)
ffeat(f3)
ffeat(f4)
wx0
fprior(x0)
wx1wx2
wx3wx4
ffeat(f1)
wx5
ffeat(f2)
ffeat(f3)
ffeat(f4)
fkin fkin fkin fkin fkin
![Page 67: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/67.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 71
ax0
ax1ax2
ax3
bx4bx5
af1
af2
bf3bf4
a
b
apbax0
fprior(ax0)
ax1ax2
ax3bx4
ffeat(af1)
bx5
ffeat(af2)
ffeat(bf4)
apb
ffeat(bf3)
fprior(bx4)
![Page 68: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/68.jpg)
Applications to robotics & vision
C++ libraries for factor graphs (tailored to SLAM):
• g2o: A General Framework for Graph Optimization [Grisetti et al.]https://github.com/RainerKuemmerle/g2o
• GTSAM: GeorgiaTech Smoothing and Mapping [Dellaert et al.]
https://github.com/borglab/gtsam
Introduction to Factor Graphs - Jose Luis Blanco - UAL 72
![Page 69: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/69.jpg)
Bundle adjustment, poses-only, structure-only optimization
Introduction to Factor Graphs - Jose Luis Blanco - UAL 73
![Page 70: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/70.jpg)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 74
![Page 71: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/71.jpg)
Applications to robotics & vision
• Poses-only: “factorizing out” some variables comes with the cost of a denser information matrix (in principle).
• The “mass-spring” model: “Removing a mass and all springs attached to it is equivalent to adding new springs between all affected masses”.
(Next slides from: Thrun, S., & Montemerlo, M. (2006). The graph SLAM algorithm with applications to large-scale mapping of urban structures. The International Journal of Robotics Research, 25(5-6), 403-429.)
Introduction to Factor Graphs - Jose Luis Blanco - UAL 75
![Page 72: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/72.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 76
![Page 73: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/73.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 77
![Page 74: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/74.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 78
![Page 75: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/75.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 79
![Page 76: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/76.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 80
![Page 77: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/77.jpg)
Applications to robotics & vision
Introduction to Factor Graphs - Jose Luis Blanco - UAL 81
![Page 78: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/78.jpg)
Time-varying gas mapping with a mobile robot• Model the environment as a discrete set of cells with gas
concentrations as the “unknown”. Include obstacles information.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 82
Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J.
(2016). Time-variant gas distribution mapping with
obstacle information. Autonomous Robots, 40(1), 1-16.
![Page 79: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/79.jpg)
Time-varying gas mapping with a mobile robot
Introduction to Factor Graphs - Jose Luis Blanco - UAL 83
![Page 80: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/80.jpg)
Time-varying gas mapping with a mobile robot
Introduction to Factor Graphs - Jose Luis Blanco - UAL 84
![Page 81: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/81.jpg)
Gas mapping in dynamic environments• Exploring a gas diffusion process using a
multi-robot system. The physical behavior of the diffusion process is modeled using a Partial Differential Equation (PDE).
Introduction to Factor Graphs - Jose Luis Blanco - UAL 85
Wiedemann, T., Shutin, D., & Lilienthal, A. J. (2019).
Model-based gas source localization strategy for a
cooperative multi-robot system—A probabilistic
approach and experimental validation incorporating
physical knowledge and modeluncertainties. Robotics and Autonomous Systems.
![Page 82: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/82.jpg)
Manipulator dynamics with factor graphs
• Mechanical systems: kinematics can be also formulated as FGs.
• “This paper describes a unified method solving for inverse, forward, and hybrid dynamics problems for robotic manipulators with either open kinematic chains or closed kinematic loops based on factor graphs”.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 86
Xie, M., & Dellaert, F. (2019). A Unified Method for Solving
Inverse, Forward, and Hybrid Manipulator Dynamics using
Factor Graphs. arXiv preprint arXiv:1911.10065.
![Page 83: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/83.jpg)
Manipulator dynamics with factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 87
Xie, M., & Dellaert, F. (2019). A Unified Method for Solving
Inverse, Forward, and Hybrid Manipulator Dynamics using
Factor Graphs. arXiv preprint arXiv:1911.10065.
![Page 84: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/84.jpg)
Manipulator dynamics with factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 88
Xie, M., & Dellaert, F. (2019). A Unified Method for Solving
Inverse, Forward, and Hybrid Manipulator Dynamics using
Factor Graphs. arXiv preprint arXiv:1911.10065.
Elimination algorithm for a particular ordering (3-DOF simplified case):
![Page 85: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/85.jpg)
Manipulator dynamics with factor graphs
Introduction to Factor Graphs - Jose Luis Blanco - UAL 89
Xie, M., & Dellaert, F. (2019). A Unified Method for Solving
Inverse, Forward, and Hybrid Manipulator Dynamics using
Factor Graphs. arXiv preprint arXiv:1911.10065.
Elimination algorithm for a particular ordering (3-DOF simplified case):
![Page 86: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/86.jpg)
Introduction to GTSAM
Introduction to Factor Graphs - Jose Luis Blanco - UAL 106
![Page 87: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/87.jpg)
GTSAM
• C++ library.
• Frank Dellaert, Georgia Tech “Institute for Robotics and Intelligent Machines”.
• Tailored to SLAM but coded as a general purpose library.
• Matlab and Python bindings.
https://github.com/borglab/gtsam/
Introduction to Factor Graphs - Jose Luis Blanco - UAL 107
![Page 88: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/88.jpg)
GTSAM
• Main C++ classes: • NonlinearFactorGraph : The Factor graph.
• FactorXXX: Factors. Arguments: • N Keys. Can be built with “Symbol” (e.g. “X1”, “V2”).
• Observed value.
• Noise model, e.g. “noiseModel::Diagonal::Sigmas()”
• Values : “Variant” container for initial and final values of all keys. Associative container: Key → Value.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 108
![Page 89: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/89.jpg)
GTSAM: PlanarSLAMExample.cpp
Introduction to Factor Graphs - Jose Luis Blanco - UAL 109
![Page 90: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/90.jpg)
GTSAM: PlanarSLAMExample.cpp
Introduction to Factor Graphs - Jose Luis Blanco - UAL 110
![Page 91: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/91.jpg)
GTSAM: PlanarSLAMExample.cpp
Introduction to Factor Graphs - Jose Luis Blanco - UAL 111
![Page 92: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/92.jpg)
GTSAM: PlanarSLAMExample.cpp
Introduction to Factor Graphs - Jose Luis Blanco - UAL 112
![Page 93: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/93.jpg)
GTSAM: Matlab wrapper
Introduction to Factor Graphs - Jose Luis Blanco - UAL 113
![Page 94: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/94.jpg)
GTSAM MATLAB: Installation
• From sources: • Enable building the MATLAB wrapper in CMake.
• Set an installation directory.
• Build the INSTALL target.
• Precompiled version for Windows and MATLAB 64bits:• https://github.com/jlblancoc/2020-ual-factor-graphs-course
• Clone or download as ZIP and uncompress.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 114
![Page 95: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/95.jpg)
GTSAM MATLAB: Installation
• From MATLAB: • “Add to path” →
gtsam_toolbox
• (If built from sourcesmanually, also add the“bin” directory to thesytem PATH).
Introduction to Factor Graphs - Jose Luis Blanco - UAL 115
![Page 96: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/96.jpg)
GTSAM MATLAB: Exercise 1
• Run Pose2SLAMExample.m
• Analyze the code and the results.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 116
![Page 97: A brief introduction to Factor Graphsjlblanco/papers/2020-introduction... · 2020-01-09 · References Introduction to Factor Graphs - Jose Luis Blanco - UAL 3 “Pattern Recognition](https://reader034.fdocuments.us/reader034/viewer/2022050323/5f7cf83defcc3325ec214682/html5/thumbnails/97.jpg)
GTSAM MATLAB: Exercise 2
• Modify the file Pose2SLAMExample.m to:• Remove the observation between Keyframe 5 & 2. Observe the final
covariances.
• Restore the removed edge, and introduce small errors in the relative poses.
• After that, define a different covariance and create a new edge between 1 & 5. Experiment with different information matrices and edge observed values.
Introduction to Factor Graphs - Jose Luis Blanco - UAL 117