![Page 1: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/1.jpg)
3D Photography: Bundle Adjustment and SLAM
Marc Pollefeys, Torsten Sattler
30 March 2014
1
![Page 2: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/2.jpg)
Structure-From-Motion
• Two views initialization:
– 5-Point algorithm (Minimal Solver)
– 8-Point linear algorithm
– 7-Point algorithm
E → (R,t)
2
![Page 3: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/3.jpg)
Structure-From-Motion
• Triangulation: 3D Points
E → (R,t)
3
![Page 4: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/4.jpg)
Incremental Structure-From-Motion
• Subsequent views: Perspective pose estimation
(R,t) (R,t)
(R,t) 4
![Page 5: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/5.jpg)
Incremental SfM with Bundle Adjustment
![Page 6: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/6.jpg)
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
![Page 7: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/7.jpg)
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
![Page 8: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/8.jpg)
Bundle Adjustment
• Minimize the cost function:
1. Gradient Descent
2. Newton Method
3. Gauss-Newton
4. Levenberg-Marquardt
argminX
f X
8
![Page 9: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/9.jpg)
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
![Page 10: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/10.jpg)
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
![Page 11: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/11.jpg)
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
![Page 12: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/12.jpg)
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 !
![Page 13: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/13.jpg)
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
![Page 14: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/14.jpg)
Structure of the Jacobian and Hessian Matrices
• Sparse matrices since 3D structures are locally observed.
14
![Page 15: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/15.jpg)
Solving the Normal Equation
• Schur Complement
15
HLM JTWZ
HLM
3D Structures
Camera Parameters
![Page 16: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/16.jpg)
Solving the Normal Equation
• Schur Complement
16
HLM JTWZ
3D Structures
Camera Parameters
sH
HLM
SCH
T
SCH CH
![Page 17: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/17.jpg)
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
![Page 18: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/18.jpg)
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
![Page 19: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/19.jpg)
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.
![Page 20: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/20.jpg)
Problem of Fill-In
20
![Page 21: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/21.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. …
21
PTAP PT x PTb
Permutation matrix to reorder A
![Page 22: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/22.jpg)
Problem of Fill-In
22
![Page 23: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/23.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.
23
argminX
zijTWijzij
ij
-argminX
ln p(Z | X)
-argminX
ln p(X |Z) -argminX
ln cij exp zijTWijzij
ij
argminX
zijTWijzij
ij
![Page 24: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/24.jpg)
Robust Cost Function
• Gaussian distribution assumption is not true in the presence of outliers!
• Causes wrong convergences.
24
![Page 25: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/25.jpg)
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
![Page 26: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/26.jpg)
Robust Cost Function
26
(.)
"(.)
Reduced influence from high errors
Gaussian Distribution
Cauchy Distribution
Influence from high errors
![Page 27: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/27.jpg)
Robust Cost Function
27
Outliers are taken into account in Cauchy!
![Page 28: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/28.jpg)
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
![Page 29: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/29.jpg)
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
![Page 30: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/30.jpg)
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
![Page 31: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/31.jpg)
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
![Page 32: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/32.jpg)
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)!
![Page 33: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/33.jpg)
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
![Page 34: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/34.jpg)
• 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
![Page 35: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/35.jpg)
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
![Page 36: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/36.jpg)
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
![Page 37: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/37.jpg)
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!
![Page 38: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/38.jpg)
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)
![Page 39: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/39.jpg)
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
![Page 40: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/40.jpg)
FastSLAM
• Many particles needed for accurate results.
• Computationally expensive for high state dimensions.
40
![Page 41: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/41.jpg)
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.
![Page 42: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/42.jpg)
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
![Page 43: Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X k X 0 Compute gradient: X k m X k Kg g T wf X wX X X k ' Z WJ Update: Slow convergence](https://reader036.vdocuments.site/reader036/viewer/2022071223/60853313e54b5a0f8b677ecb/html5/thumbnails/43.jpg)
Next Lectures
• Next week: Easter
• 13.04.: Project Updates (you!)
• 20.04.: Multi-view Stereo & Volumetric Modeling
43