bundle adjustment and slam · 3d vision –class 6 bundle adjustment and slam •[triggs,...
TRANSCRIPT
![Page 1: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/1.jpg)
3D Vision
Marc Pollefeys, Viktor Larsson
Spring 2020
![Page 2: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/2.jpg)
Feb 17 Introduction
Feb 24 Geometry, Camera Model, Calibration
Mar 2 Features, Tracking / Matching
Mar 9 Project Proposals by Students
Mar 16 Structure from Motion (SfM) + papers
Mar 23 Dense Correspondence (stereo / optical flow) + papers
Mar 30 Bundle Adjustment & SLAM + papers
Apr 6 Student Midterm Presentations
Apr 13 Easter break
Apr 20 Multi-View Stereo & Volumetric Modeling + papers
Apr 27 3D Modeling with Depth Sensors + papers
May 4 3D Scene Understanding + papers
May 11 4D Video & Dynamic Scenes + papers
May 18 papers
May 25 Student Project Demo Day = Final Presentations
Schedule
![Page 3: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/3.jpg)
3D Vision – Class 6
Bundle Adjustment and SLAM
• [Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment – A Modern Synthesis, Int. Workshop on Vision Algorithms, 1999]
• [Montemerio, Thrun, Koller, Wegbreit, FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem, AAAI 2002]
• Section 2.5 from [Lee, Visual Mapping and Pose Estimation for Self-Driving Cars, PhD Thesis, ETH Zurich, 2014]
Slides : Gim Hee Lee
![Page 4: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/4.jpg)
Lecture Overview
• Bundle Adjustment in Structure-from-Motion
• Simultaneous Localization & Mapping (SLAM)
4
![Page 5: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/5.jpg)
Recap: Structure-From-Motion
• Two views initialization:
– 5-Point algorithm (Minimal Solver)
– 8-Point linear algorithm
– 7-Point algorithm
E → (R,t)
5
![Page 6: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/6.jpg)
Recap: Structure-From-Motion
• Triangulation: 3D Points
E → (R,t)
6
![Page 7: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/7.jpg)
Recap: Structure-From-Motion
• Subsequent views: Perspective pose estimation
(R,t)(R,t)
(R,t)7
![Page 8: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/8.jpg)
Recap: Structure-From-Motion
![Page 9: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/9.jpg)
Bundle Adjustment
• Refinement step in Structure-from-Motion.
• Refine a visual reconstruction to produce jointly optimal 3D structures P and camera poses C.
• Minimize total re-projection errors .
ijx
( ) −i j
WijijX ij
CPx2
,argmin
ijz
z
jP
( )jj CX ,
iC
ijz
Cost Function:
],[ CPX =9
![Page 10: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/10.jpg)
Bundle Adjustment
• Refinement step in Structure-from-Motion.
• Refine a visual reconstruction to produce jointly optimal 3D structures P and camera poses C.
• Minimize total re-projection errors .
ijx
z
jP
( )jj CX ,
iC
ijz
Cost Function:
i j
ijij
T
ijX
zWzargmin
Measurement error covariance:1−
ijW
],[ CPX =10
( )Xf
![Page 11: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/11.jpg)
Bundle Adjustment
• Minimize the cost function:
1. Gradient Descent
2. Newton Method
3. Gauss-Newton
4. Levenberg-Marquardt
( )XfX
argmin
11
![Page 12: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/12.jpg)
Bundle Adjustment
1. Gradient Descent
12
Initialization: 0XX k =
Compute gradient:
gXX kk −
( )WJZ
X
Xfg T
XX k
=
=
=
Update:
Slow convergence near minimum point!
Iterate until convergence
: Step sizeX
J
=
: Jacobian
![Page 13: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/13.jpg)
Bundle Adjustment
2. Newton Method
13
2nd order approximation (Quadratic Taylor Expansion):
KK XX
T
XXHgXfXf
==+++
21)()(
Find that minimizes !KXX
Xf=
+ )(
( )
kXX
XfH
=
+=
2
2
:
Hessian matrix
![Page 14: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/14.jpg)
Bundle Adjustment
2. Newton Method
14
Differentiate and set to 0 gives:
gH 1−−=
Computation of H is not trivial and might get stuck at saddle point!
Update: + kk XX
![Page 15: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/15.jpg)
Bundle Adjustment
3. Gauss-Newton
15
+=
i j
ij
ijij
T
XWZWJJH
2
2
WJJH T
Normal equation:
ZWJWJJ TT −=
Update: + kk XX
Might get stuck and slow convergence at saddle point!
![Page 16: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/16.jpg)
Bundle Adjustment
16
4. Levenberg-Marquardt
Regularized Gauss-Newton with damping factor .
( ) ZWJIWJJ TT −=+
:0→ Gauss-Newton (when convergence is rapid)
:→ Gradient descent (when convergence is slow)
LMH
Adapt λ during optimization:• Decrease λ when function value decreases• Increase λ otherwise
![Page 17: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/17.jpg)
Structure of the Jacobianand Hessian Matrices
• Sparse matrices since 3D structures are locally observed.
17
![Page 18: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/18.jpg)
Efficiently Solving the Normal Equation
• Schur Complement: Exploit structure of H
18
ZWJH T
LM −=
=LMH
3D Structures
Camera Parameters
![Page 19: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/19.jpg)
Efficiently Solving the Normal Equation
• Schur Complement: Exploit structure of H
19
ZWJH T
LM −=
3D Structures
Camera Parameters
sH
=LMH
SCH
T
SCH CH
![Page 20: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/20.jpg)
Efficiently Solving the Normal Equation
• Schur Complement: Obtain reduced system
20
ZWJH T
LM −=
=
C
S
C
S
C
T
SC
SCS
HH
HH
3D Structures
Camera Parameters
Multiply both sides by:
− − IHH
I
S
T
SC
1
0
−=
− −− 110 S
T
SCSC
S
C
S
SCS
T
SCC
SCS
HHHHHH
HH
![Page 21: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/21.jpg)
Efficiently Solving the Normal Equation
• Schur Complement: Obtain reduced system
21
−=
− −− 110 S
T
SCSC
S
C
S
SCS
T
SCC
SCS
HHHHHH
HH
11 )( −− −=− S
T
SCSCCSCS
T
SCC HHHHHH
First solve for from:C
Schur Complement(Sparse and Symmetric Positive Definite Matrix)
Easy to invert a block diagonal matrix
Solve for by backward substitution.SC
![Page 22: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/22.jpg)
Efficiently Solving the Normal Equation
• Use sparse matrix factorization to solve system
1. LU Factorization
2. QR factorization
3. Cholesky Factorization
• Iterative methods
1. Conjugate gradient
2. Gauss-Seidel
22
11 )( −− −=− S
T
SCSCCSCS
T
SCC HHHHHH bAx =
Don’t solve as x=A-1b: A is sparse, but A-1 is not!
QRA =
LUA =
TLLA =
Solve for x by forward-backward substitutions
![Page 23: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/23.jpg)
Problem of Fill-In
24
![Page 24: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/24.jpg)
Problem of Fill-In
• Reorder sparse matrix to minimize fill-in.
• NP-Complete problem.
• Approximate solutions:1. Minimum degree
2. Column approximate minimum degree permutation
3. Reverse Cuthill-Mckee.
4. …
25
( )( ) bPxPAPP TTT =
Permutation matrix to reorder A
![Page 25: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/25.jpg)
Problem of Fill-In
26
![Page 26: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/26.jpg)
Robust Cost Function
• Non-linear least squares:
• Maximum log-likelihood solution:
• Assume that:1. X is a random variable that follows Gaussian distribution.
2. All observations are independent.
27
ij
ijij
T
ijX
zWzargmin
)|(ln-argmin XZpX
( )
−−=− ij
ijij
T
ijijXX
zWzcZXp explnargmin)|(lnargmin
=ij
ijij
T
ijX
zWzargmin
![Page 27: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/27.jpg)
Robust Cost Function
• Gaussian distribution assumption is not true in the presence of outliers!
• Causes wrong convergences.
28
![Page 28: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/28.jpg)
29
( ) ij
ijij
T
ijXij
ijijX
zSzz argminargmin
Robust Cost Function scaled with ijW ij"
• Similar to iteratively re-weighted least-squares.
• Weight is iteratively rescaled with the attenuating factor .
• Attenuating factor is computed based on current error.
ij"
Robust Cost Function
![Page 29: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/29.jpg)
Cauchy Distribution
30
(.) (.)"
Reduced influence from high errors
Gaussian Distribution
Influence from high errors
Robust Cost Function
![Page 30: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/30.jpg)
Robust Cost Function
31
Outliers are taken into account in Cauchy!
![Page 31: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/31.jpg)
State-of-the-Art Solvers
• Google Ceres:
– https://code.google.com/p/ceres-solver/
• g2o:
– https://openslam.org/g2o.html
• GTSAM:
– https://collab.cc.gatech.edu/borg/gtsam/
• Multicore Bundle Adjustment
– http://grail.cs.washington.edu/projects/mcba/
32
![Page 32: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/32.jpg)
Lecture Overview
• Bundle Adjustment in Structure-from-Motion
• Simultaneous Localization & Mapping (SLAM)
33
![Page 33: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/33.jpg)
Simultaneous Localization & Mapping (SLAM)
• Robot navigates in unknown environment:
– Estimate its own pose
– Acquire a map model of its environment.
• Chicken-and-Egg problem:
– Map is needed for localization (pose estimation).
– Pose is needed for mapping.
• Highly related to Structure-From-Motion.
34
![Page 34: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/34.jpg)
Full SLAM: Problem Definition
35
Robot poses
Observations
Map landmarks
u1 u2 u3Control actions
![Page 35: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/35.jpg)
Full SLAM: Problem Definition
36
( ) = =
−=M
i
K
k
jkikkiiiX,LX,L
lxzpuxxpXpUZLXp1 1
10 ),|(),|()(argmax,|,argmax
• Maximum a Posteriori (MAP) solution:
![Page 36: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/36.jpg)
37
( ) = =
−=M
i
K
k
jkikkiiiX,LX,L
lxzpuxxpXpUZLXp1 1
10 ),|(),|()(argmax,|,argmax
−−= ==
−
K
k
jkikk
M
i
iiiX,L
lxzpuxxp11
1 ),|(ln),|(lnargminNegative log-likelihood
Likelihoods:
}),(exp{),|(2
11i
iiiii xuxfuxxp−− −−
Process model
}),(exp{),|(2
kkjkikjkikk zlxhlxzp
−−
Measurement model
Full SLAM
![Page 37: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/37.jpg)
Full SLAM
38
−−= ==
−
K
k
ikikk
M
i
iiiX,LX,L
lxzpuxxpUZLXp11
1 ),|(ln),|(lnargmin),|,(argmax
Putting the likelihoods into the equation:
−+−= =
=
−
K
k
kikik
M
i
iiiX,LX,L ki
zlxhxuxfUZLXp1
2
1
2
1 ),(),(argmin),|,(argmax
Minimization can be done with Levenberg-Marquardt (similar to bundle adjustment problem)!
![Page 38: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/38.jpg)
39
( ) ZWJIWJJ TT −=+
Jacobian made up of , , ,x
f
u
f
x
h
l
h
Weight made up of ,i
k
Normal Equations:
Can be solved with sparse matrix factorization or iterative methods
Full SLAM
Solving the full SLAM problem rather expensive for larger scenes
![Page 39: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/39.jpg)
• Estimate current pose and full map :
• Inference with:
1. (Extended) Kalman Filter (EKF SLAM)
2. Particle Filter (FastSLAM)
Online SLAM: Problem Definition
40
121 ...),|,(),|,( − = tt dxdxdxUZLXpUZLxp
tx L
Previous poses are marginalized out
![Page 40: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/40.jpg)
EKF SLAM
• Assumes: pose and map are random variables that follow Gaussian distributions.
• Hence,
• (Extended) Kalman Filter iteratively– Predicts pose & map based on process model
– Corrects prediction based on observations
41
tx L
),Ν(~),|,( UZLxp t
Mean Error covariance
![Page 41: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/41.jpg)
42
),( 1−= ttt uf
t
T
tttt RFF += −1
Prediction:Process model
Error propagation with process noise
Kalman gain
Update mean
1)( −+= t
T
ttt
T
ttt QHHHK
tttt yK+=
tttt HKI −= )(
Correction:)( ttt hzy −= Measurement residual (innovation)
Update covariance
t
tt
x
hH
=
)(Measurement Jacobian
1
1),(
−
−
=
t
ttt
x
ufF
Process Jacobian
EKF SLAM
![Page 42: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/42.jpg)
Structure of Mean and Covariance
43
=
=
2
2
2
2
2
2
2
1
21
2221222
1211111
21
21
21
,
NNNNNN
N
N
N
N
N
llllllylxl
llllllylxl
llllllylxl
lllyx
ylylylyyxy
xlxlxlxxyx
t
N
t
l
l
l
y
x
Covariance is a dense matrix that grows with increasing map features!
True robot and map states might not follow unimodal Gaussian distribution!
![Page 43: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/43.jpg)
Particle Filtering: FastSLAM
• Particles represents samples from the posterior distribution .
• can be any distribution (need not be Gaussian).
44
),|,( UZLxp t
),|,( UZLxp t
![Page 44: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/44.jpg)
FastSLAM
45
},...,,,,{ ,,,2,2,1,1 = m
tN
m
tN
m
t
m
t
m
t
m
t
m
t
m
t xp
Each particle represents:
Robot state Landmark state (mean and covariance)
),|(~ 1 ttt
m
t uxxpx −Sample the robot state from
the process model
),|( , t
m
t
m
tn zxLp N Kalman filter Landmark updates
),|( m
t
m
tt
m
t xLzpw Weight update
Resampling based on current state
![Page 45: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/45.jpg)
FastSLAM
• Many particles needed for accurate results.
• Computationally expensive for high state dimensions.
46
![Page 46: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/46.jpg)
• Constraints: Relative pose estimates from 3D structure.
• Don’t update 3D structure (fixed wrt. to some pose).
• Optimizes poses as
• Can be used to minimize loop-closure errors.47
−ij
jiijX ij
vvhz2
),(argmin
Pose-Graph SLAM
Robot pose
Measured constraint
Relative transformation between poses
![Page 47: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/47.jpg)
Summary
• Bundle Adjustment– Refine 3D points and poses in Structure-From-Motion.– Efficient computation by exploiting structure & sparsity.– Core step in every Structure-From-Motion (SFM) pipeline.
• Simultaneous Localization and Mapping – Very similar to Incremental SFM.– Typically includes some motion model.– Two general approaches to SLAM:
• (Local) Bundle Adjustment (not discussed in lecture)• Filter-based techniques (EKF SLAM, FastSLAM)
– Pose-Graph SLAM (loop-closure handling)
48
![Page 48: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/48.jpg)
Feb 17 Introduction
Feb 24 Geometry, Camera Model, Calibration
Mar 2 Features, Tracking / Matching
Mar 9 Project Proposals by Students
Mar 16 Structure from Motion (SfM) + papers
Mar 23 Dense Correspondence (stereo / optical flow) + papers
Mar 30 Bundle Adjustment & SLAM + papers
Apr 6 Student Midterm Presentations
Apr 13 Easter break
Apr 20 Multi-View Stereo & Volumetric Modeling + papers
Apr 27 3D Modeling with Depth Sensors + papers
May 4 3D Scene Understanding + papers
May 11 4D Video & Dynamic Scenes + papers
May 18 papers
May 25 Student Project Demo Day = Final Presentations
Schedule
![Page 49: Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs, McLauchlan, Hartley, Fitzgibbon, Bundle Adjustment –A Modern Synthesis, Int. Workshop on](https://reader035.vdocument.in/reader035/viewer/2022063006/5fb72a36fb648d75902ae8e9/html5/thumbnails/49.jpg)
Next week: Midterm Presentations
Reminder:Prepare short presentation
(3-5min) for Monday!