Distributed Very Large Scale Bundle Adjustment by Global ...
Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs,...
Transcript of Bundle Adjustment and SLAM · 3D Vision –Class 6 Bundle Adjustment and SLAM •[Triggs,...
3D Vision
Marc Pollefeys, Viktor Larsson
Spring 2020
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
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
Lecture Overview
• Bundle Adjustment in Structure-from-Motion
• Simultaneous Localization & Mapping (SLAM)
4
Recap: Structure-From-Motion
• Two views initialization:
– 5-Point algorithm (Minimal Solver)
– 8-Point linear algorithm
– 7-Point algorithm
E → (R,t)
5
Recap: Structure-From-Motion
• Triangulation: 3D Points
E → (R,t)
6
Recap: Structure-From-Motion
• Subsequent views: Perspective pose estimation
(R,t)(R,t)
(R,t)7
Recap: Structure-From-Motion
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
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
Bundle Adjustment
• Minimize the cost function:
1. Gradient Descent
2. Newton Method
3. Gauss-Newton
4. Levenberg-Marquardt
( )XfX
argmin
11
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
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
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
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!
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
Structure of the Jacobianand Hessian Matrices
• Sparse matrices since 3D structures are locally observed.
17
Efficiently Solving the Normal Equation
• Schur Complement: Exploit structure of H
18
ZWJH T
LM −=
=LMH
3D Structures
Camera Parameters
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
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
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
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
Problem of Fill-In
24
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
Problem of Fill-In
26
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
Robust Cost Function
• Gaussian distribution assumption is not true in the presence of outliers!
• Causes wrong convergences.
28
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
Cauchy Distribution
30
(.) (.)"
Reduced influence from high errors
Gaussian Distribution
Influence from high errors
Robust Cost Function
Robust Cost Function
31
Outliers are taken into account in Cauchy!
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
Lecture Overview
• Bundle Adjustment in Structure-from-Motion
• Simultaneous Localization & Mapping (SLAM)
33
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
Full SLAM: Problem Definition
35
Robot poses
Observations
Map landmarks
u1 u2 u3Control actions
Full SLAM: Problem Definition
36
( ) = =
−=M
i
K
k
jkikkiiiX,LX,L
lxzpuxxpXpUZLXp1 1
10 ),|(),|()(argmax,|,argmax
• Maximum a Posteriori (MAP) solution:
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
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)!
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
• 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
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
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
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!
Particle Filtering: FastSLAM
• Particles represents samples from the posterior distribution .
• can be any distribution (need not be Gaussian).
44
),|,( UZLxp t
),|,( UZLxp t
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
FastSLAM
• Many particles needed for accurate results.
• Computationally expensive for high state dimensions.
46
• 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
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
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
Next week: Midterm Presentations
Reminder:Prepare short presentation
(3-5min) for Monday!