Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X...

transcript

3D Photography: Bundle Adjustment and SLAM

Marc Pollefeys, Torsten Sattler

30 March 2014

Structure-From-Motion

• Two views initialization:

– 5-Point algorithm (Minimal Solver)

– 8-Point linear algorithm

– 7-Point algorithm

E → (R,t)

Structure-From-Motion

• Triangulation: 3D Points

E → (R,t)

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 .

argminX

xij Pj ,Ci Wij

X j,C j

Cost Function:

Measurement error covariance

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 j,C j

Cost Function:

argminX

zijTWijzij

Measurement error covariance

X [P,C]7

Bundle Adjustment

• Minimize the cost function:

1. Gradient Descent

2. Newton Method

3. Gauss-Newton

4. Levenberg-Marquardt

argminX

Bundle Adjustment

1. Gradient Descent

Initialization:

Compute gradient:

Xk Xk g

g f X X

Update:

Slow convergence near minimum point!

Iterate until convergence

: Step size

X: Jacobian

Bundle Adjustment

2. Newton Method

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

Differentiate and set to 0 gives:

Computation of H is not trivial and might get stuck at saddle point!

Update:

f (X )X XK

f (X) g 12

THX XK

Bundle Adjustment

3. Gauss-Newton

H JTWJ ZijWij

H JTWJ

Normal equation:

JTWJ JTWZ

Update:

Might get stuck and slow convergence at saddle point !

Bundle Adjustment

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)

Structure of the Jacobian and Hessian Matrices

• Sparse matrices since 3D structures are locally observed.

Solving the Normal Equation

• Schur Complement

HLM JTWZ

3D Structures

Camera Parameters

HLM JTWZ

3D Structures

Camera Parameters

SCH CH

HLM JTWZ

HS HSC

HSCT HC

3D Structures

Camera Parameters

Multiply both sides by:

HSCT HS

HS HSC

0 HC HSCT HS

C SHSCT HS

HS HSC

0 HC HSCT HS

C SHSCT HS

(HC HSCT HS

1HSC)C C SHSCT HS

First solve for from:

Schur Complement (Sparse and Symmetric Positive Definite Matrix)

Easy to invert a block diagonal matrix

Solve for by backward substitution.

• Sparse matrix factorization 1. LU Factorization

2. QR factorization

3. Cholesky Factorization

• Iterative methods 1. Conjugate gradient

2. Gauss-Seidel

(HC HSCT HS

1HSC)C C SHSCT HS

Can be solved without inverting A since it is a sparse matrix!

Solve for x by forward backward substitutions.

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. …

PTAP PT x PTb

Permutation matrix to reorder A

Problem of Fill-In

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.

argminX

zijTWijzij

-argminX

ln p(Z | X)

-argminX

ln p(X |Z) -argminX

ln cij exp zijTWijzij

argminX

zijTWijzij

• Gaussian distribution assumption is not true in the presence of outliers!

• Causes wrong convergences.

argminX

ij zij ij

argminX

zijTSijzij

Robust Cost Function scaled with

• Similar to iteratively re-weighted least-squares.

• Weight is iteratively rescaled with the attenuating factor .

• Attenuating factor is computed based on current error.

Reduced influence from high errors

Gaussian Distribution

Cauchy Distribution

Influence from high errors

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/

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.

Full SLAM: Problem Definition

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

u1 u2 u3 Control Actions

argmaxX,L

p X,L |Z,U argmaxX,L

p(X0) p(xi | xi1,ui) p(zk | xik,l jk)k1

-argminX,L

ln p(xi | xi1,ui)i1

ln p(zk | xik,l jk)k1

Negative log-likelihood

Likelihoods:

p(xi | xi1,ui) exp{ f (xi1,ui) xi i

Process model

p(zk | xik,l jk) exp{ h(xik,l jk) zk k

Measurement model

argmaxX,L

p(X,L | Z,U) -argminX,L

ln p(xi | xi1,ui)i1

ln p(zk | xik,l jk)k1

Putting the likelihoods into the equation:

argmaxX,L

p(X,L |Z,U) argminX,L

f (xi1,ui) xi i

h(xik,l jk) zk k

Minimization can be done with Levenberg-Marquardt (similar to bundle adjustment)!

JTWJ I JTWZJacobian made up of , , ,

Weight made up of ,

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

p(xt,L |Z,U) K p(X,L |Z,U) dx1 dx2...dxt1

Previous poses are marginalized out

EKF SLAM

• Assumes: pose and map are random variables that follows Gaussian distribution.

• Hence,

p(xt,L |Z,U) ~ N(,)

mean Error covariance

EKF SLAM

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)

Ht h(t )

x tMeasurement Jacobian Process Jacobian

Structure of Mean and Covariance

2221222

1211111

NNNNNN

llllllylxl

ylylylyyxy

xlxlxlxxyx

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).

p(xt,L |Z,U)

FastSLAM

ptm {xt

m, 1,tm ,1,t

m , 2,tm ,2,t

m ... N ,tm ,N ,t

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.

Pose-Graph SLAM

argminX

zij h(vi,v j )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

Next Lectures

• Next week: Easter

• 13.04.: Project Updates (you!)

• 20.04.: Multi-view Stereo & Volumetric Modeling

Bundle Adjustment and SLAM - CVG @ ETHZ...Bundle Adjustment 1. Gradient Descent 9 Initialization: X...

Documents