Upload
others
View
12
Download
0
Embed Size (px)
Citation preview
3D Photography: Bundle Adjustment and SLAM
Marc Pollefeys, Torsten Sattler
30 March 2014
1
Structure-From-Motion
• Two views initialization:
– 5-Point algorithm (Minimal Solver)
– 8-Point linear algorithm
– 7-Point algorithm
E → (R,t)
2
Structure-From-Motion
• Triangulation: 3D Points
E → (R,t)
3
Incremental Structure-From-Motion
• Subsequent views: Perspective pose estimation
(R,t) (R,t)
(R,t) 4
Incremental SfM with Bundle Adjustment
Bundle Adjustment
• Final 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 .
x ij
argminX
xij Pj ,Ci Wij
2
j
i
ijz
z
Pj
X j,C j
Ci
zij
Cost Function:
Measurement error covariance
Wij
1 :
X [P,C]6
Bundle Adjustment
• Final 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 .
x ij
z
Pj
X j,C j
Ci
zij
Cost Function:
argminX
zijTWijzij
j
i
Measurement error covariance
Wij
1 :
X [P,C]7
f X
Bundle Adjustment
• Minimize the cost function:
1. Gradient Descent
2. Newton Method
3. Gauss-Newton
4. Levenberg-Marquardt
argminX
f X
8
Bundle Adjustment
1. Gradient Descent
9
Initialization:
Xk X0
Compute gradient:
Xk Xk g
g f X X
X X k
ZTWJ
Update:
Slow convergence near minimum point!
Iterate until convergence
: Step size
J
X: Jacobian
Bundle Adjustment
2. Newton Method
10
2nd order approximation (Quadratic Taylor Expansion):
f (X )X XK
f (X) g 12
THX XK
Find that minimizes !
f (X )X XK
:H 2 f X
2X X k
Hessian matrix
Bundle Adjustment
2. Newton Method
11
Differentiate and set to 0 gives:
H1g
Computation of H is not trivial and might get stuck at saddle point!
Update:
Xk Xk
f (X )X XK
f (X) g 12
THX XK
Bundle Adjustment
3. Gauss-Newton
12
H JTWJ ZijWij
2 ij
X 2j
i
H JTWJ
Normal equation:
JTWJ JTWZ
Update:
Xk Xk
Might get stuck and slow convergence at saddle point !
Bundle Adjustment
13
4. Levenberg-Marquardt
Regularized Gauss-Newton with damping factor .
JTWJ I JTWZ
0 : Gauss-Newton (when convergence is rapid)
: Gradient descent (when convergence is slow)
HLM
Structure of the Jacobian and Hessian Matrices
• Sparse matrices since 3D structures are locally observed.
14
Solving the Normal Equation
• Schur Complement
15
HLM JTWZ
HLM
3D Structures
Camera Parameters
Solving the Normal Equation
• Schur Complement
16
HLM JTWZ
3D Structures
Camera Parameters
sH
HLM
SCH
T
SCH CH
Solving the Normal Equation
• Schur Complement
17
HLM JTWZ
HS HSC
HSCT HC
S
C
S
C
3D Structures
Camera Parameters
Multiply both sides by:
I 0
HSCT HS
1 I
HS HSC
0 HC HSCT HS
1HSC
S
C
S
C SHSCT HS
1
Solving the Normal Equation
• Schur Complement
18
HS HSC
0 HC HSCT HS
1HSC
S
C
S
C SHSCT HS
1
(HC HSCT HS
1HSC)C C SHSCT HS
1
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.
S
Solving the Normal Equation
• Sparse matrix factorization 1. LU Factorization
2. QR factorization
3. Cholesky Factorization
• Iterative methods 1. Conjugate gradient
2. Gauss-Seidel
19
(HC HSCT HS
1HSC)C C SHSCT HS
1
Ax b
Can be solved without inverting A since it is a sparse matrix!
A QR
A LU
A LLT
Solve for x by forward backward substitutions.
Problem of Fill-In
20
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. …
21
PTAP PT x PTb
Permutation matrix to reorder A
Problem of Fill-In
22
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.
23
argminX
zijTWijzij
ij
-argminX
ln p(Z | X)
-argminX
ln p(X |Z) -argminX
ln cij exp zijTWijzij
ij
argminX
zijTWijzij
ij
Robust Cost Function
• Gaussian distribution assumption is not true in the presence of outliers!
• Causes wrong convergences.
24
Robust Cost Function
25
argminX
ij zij ij
argminX
zijTSijzij
ij
Robust Cost Function scaled with
W ij
"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
26
(.)
"(.)
Reduced influence from high errors
Gaussian Distribution
Cauchy Distribution
Influence from high errors
Robust Cost Function
27
Outliers are taken into account in Cauchy!
State-of-the-Art Solvers
• Google Ceres:
– http://ceres-solver.org/
• g2o:
– https://openslam.org/g2o.html
• GTSAM:
– https://collab.cc.gatech.edu/borg/gtsam/
28
Simultaneous Localization and Mapping (SLAM)
• For a robot to estimate its own pose and acquire a map model of its environment.
• Chicken-and-Egg problem:
– Map is needed for localization.
– Pose is needed for mapping.
29
Full SLAM: Problem Definition
30
Robot Poses
Observations
Map landmarks
argmaxX,L
p X,L |Z,U argmaxX,L
p(X0) p(xi | xi1,ui) p(zk | xik,l jk)k1
K
i1
M
u1 u2 u3 Control Actions
31
argmaxX,L
p X,L |Z,U argmaxX,L
p(X0) p(xi | xi1,ui) p(zk | xik,l jk)k1
K
i1
M
-argminX,L
ln p(xi | xi1,ui)i1
M
ln p(zk | xik,l jk)k1
K
Negative log-likelihood
Simultaneous Localization and Mapping (SLAM)
Likelihoods:
p(xi | xi1,ui) exp{ f (xi1,ui) xi i
2}
Process model
p(zk | xik,l jk) exp{ h(xik,l jk) zk k
2
}
Measurement model
Simultaneous Localization and Mapping (SLAM)
32
argmaxX,L
p(X,L | Z,U) -argminX,L
ln p(xi | xi1,ui)i1
M
ln p(zk | xik,l jk)k1
K
Putting the likelihoods into the equation:
argmaxX,L
p(X,L |Z,U) argminX,L
f (xi1,ui) xi i
2
i1
M
h(xik,l jk) zk k
2
k1
K
Minimization can be done with Levenberg-Marquardt (similar to bundle adjustment)!
Simultaneous Localization and Mapping (SLAM)
33
JTWJ I JTWZJacobian made up of , , ,
f
x
f
u
h
x
h
l
Weight made up of ,
i
k
Normal Equations:
Can be solved with sparse matrix factorization or iterative methods
• Estimate current pose and full map .
• Inference with:
1. Kalman Filtering (EKF SLAM)
2. Particle Filtering (FastSLAM)
Online SLAM: Problem Definition
34
p(xt,L |Z,U) K p(X,L |Z,U) dx1 dx2...dxt1
tx L
Previous poses are marginalized out
EKF SLAM
• Assumes: pose and map are random variables that follows Gaussian distribution.
• Hence,
35
tx L
p(xt,L |Z,U) ~ N(,)
mean Error covariance
EKF SLAM
36
t f (ut,t1)
t Ftt1FtT Rt
Kt tHtT (HttHt
T Qt )1
t t Ktyt
t (I KtHt )t
Prediction:
Correction:
Process model
Error propagation with process noise
Kalman gain
Update mean
yt zt h(t ) Measurement residual (innovation)
Update covariance
Ft f (ut,t1)
xt1
Ht h(t )
x tMeasurement Jacobian Process Jacobian
Structure of Mean and Covariance
37
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 (not necessarily Gaussian).
38
p(xt,L |Z,U)
p(xt,L |Z,U)
FastSLAM
39
ptm {xt
m, 1,tm ,1,t
m , 2,tm ,2,t
m ... N ,tm ,N ,t
m }
Each particle represents:
Robot state Landmark state (mean and covariance)
xtm ~ p(xt | xt1,ut )
Sample the robot state from the process model
p(Ln,tm | xt
m,zt )N Kalman filter
measurement updates
wtm p(zt |Lt
m,xtm) Weight update
Re-sampling
FastSLAM
• Many particles needed for accurate results.
• Computationally expensive for high state dimensions.
40
Pose-Graph SLAM
41
argminX
zij h(vi,v j )ij
2
ij
• 3D structures are removed (fixed).
• Constraints are relative pose estimates from 3D structures.
• Minimizes loop-closure errors.
Conclusion
• Bundle Adjustment
– See also Triggs et al., Bundle Adjustment – A Modern Synthesis, Vision, LNCS 1883, Springer 2000
• Simultaneous Localization and Mapping
– Full SLAM
– Online SLAM
• EKF SLAM
• FastSLAM
• Pose-Graph SLAM
42
Next Lectures
• Next week: Easter
• 13.04.: Project Updates (you!)
• 20.04.: Multi-view Stereo & Volumetric Modeling
43