3D Vision Andreas Geiger, Torsten Sattler Spring 2017 Schedule - - PowerPoint PPT Presentation

3D Vision

Andreas Geiger, Torsten Sattler

Spring 2017

Feb 20 Introduction Feb 27 Geometry, Camera Model, Calibration Mar 6 Features, Tracking / Matching Mar 13

Project Proposals by Students

Mar 20 Structure from Motion (SfM) + papers Mar 27 Dense Correspondence (stereo / optical flow) + papers Apr 3

Bundle Adjustment & SLAM + papers

Apr 10

Student Midterm Presentations

Apr 17 Easter Break Apr 24 Multi-View Stereo & Volumetric Modeling + papers May 1 Labor Day May 8 3D Modeling with Depth Sensors + papers May 15 3D Scene Understanding + papers May 22 4D Video & Dynamic Scenes + papers May 29

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
• ptimal 3D structures P and camera poses C.
• Minimize total re-projection errors .

ij

x

( )

∑∑

−

i j W i j ij X

ij

C P x

2

, argmin π

ij

z ∆ z ∆

j

P

( )

j j C

X , π

i

C

ij

z ∆

Cost Function: ] , [ C P X =

9

Bundle Adjustment

• Refinement step in Structure-from-Motion.
• Refine a visual reconstruction to produce jointly
• ptimal 3D structures P and camera poses C.
• Minimize total re-projection errors .

ij

x

z ∆

j

P

( )

j j C

X , π

i

C

ij

z ∆

Cost Function:

∑∑

∆ ∆

i j ij ij T ij X

z W z argmin

Measurement error covariance

:

1 − ij

W ] , [ C P X =

10

( )

X f

Bundle Adjustment

• Minimize the cost function:
• 1. Gradient Descent
• 2. Newton Method
• 3. Gauss-Newton
• 4. Levenberg-Marquardt

( )

X f

X

argmin

11

Bundle Adjustment

• 1. Gradient Descent

12

Initialization:

X X k =

Compute gradient:

g X X

k k

η − ←

( )

WJ Z X X f g

T X X

k

∆ = ∂ ∂ =

=

Update:

Slow convergence near minimum point!

Iterate until convergence : Step size

η

X J ∂ ∂ = π

: Jacobian

Bundle Adjustment

• 2. Newton Method

13

2nd order approximation (Quadratic Taylor Expansion):

K K

X X T X X

H g X f X f

= =

+ + ≈ + δ δ δ δ

2 1

) ( ) ( Find that minimizes !

δ

K

X X

X f

=

+ ) ( δ

( )

k

X X

X f H

=

∂ + ∂ =

2 2

: δ δ

Hessian matrix

Bundle Adjustment

• 2. Newton Method

14

Differentiate and set to 0 gives:

g H

1 −

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

Update:

δ + ←

k k

X X

Bundle Adjustment

• 3. Gauss-Newton

15

∑∑

∂ ∂ ∆ + =

i j ij ij ij T

X W Z WJ J H

2 2π

WJ J H

T

≈

Normal equation:

Z W J WJ J

T T

∆ − = δ

Update:

δ + ←

k k

X X

Might get stuck and slow convergence at saddle point!

Bundle Adjustment

16

• 4. Levenberg-Marquardt

Regularized Gauss-Newton with damping factor .

λ

( )

Z W J I WJ J

T T

∆ − = + δ λ : → λ

Gauss-Newton (when convergence is rapid)

: ∞ → λ

Gradient descent (when convergence is slow)

LM

H

Adapt λ during optimization:

• Decrease λ when function value decreases
• Increase λ otherwise

Structure of the Jacobian and Hessian Matrices

• Sparse matrices since 3D structures are locally
• bserved.

17

Efficiently Solving the Normal Equation

• Schur Complement: Exploit structure of H

18

Z W J H

T LM

∆ − = δ =

LM

H

3D Structures Camera Parameters

Efficiently Solving the Normal Equation

• Schur Complement: Exploit structure of H

19

Z W J H

T LM

∆ − = δ

3D Structures Camera Parameters

s

H

=

LM

H

SC

H

T SC

H

C

H

Efficiently Solving the Normal Equation

• Schur Complement: Obtain reduced system

20

Z W J H

T LM

∆ − = δ       =            

C S C S C T SC SC S

H H H H ε ε δ δ

3D Structures Camera Parameters

Multiply both sides by:

      −

−

I H H I

S T SC 1

      − =             −

− − 1 1 S T SC S C S C S SC S T SC C SC S

H H H H H H H H ε ε ε δ δ

Efficiently Solving the Normal Equation

• Schur Complement: Obtain reduced system

21

      − =             −

− − 1 1 S T SC S C S C S SC S T SC C SC S

H H H H H H H H ε ε ε δ δ

1 1

) (

− −

− = −

S T SC S C C SC S T SC C

H H H H H H ε ε δ

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

1 1

) (

− −

− = −

S T SC S C C SC S T SC C

H H H H H H ε ε δ b Ax = ≡

Don’t solve as x=A-1b: A is sparse, but A-1 is not!

QR A =

LU A =

T

LL A =

Solve for x by forward- backward substitutions

Efficiently Solving the Normal Equation

• Example for forward-backward substitutions

23

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

( )( )

b P x P AP P

T T T

=

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 ij ij T ij X

z W z argmin ) | ( ln argmin

• X

Z p

X

( )

      ∆ ∆ − =

∏

ij ij ij T ij ij X X

z W z c Z X p exp ln argmin

• )

| ( ln argmin

• ∑

∆ ∆ =

ij ij ij T ij X

z W z argmin

Robust Cost Function

• Gaussian distribution assumption is not true in

the presence of outliers!

• Causes wrong convergences.

28

29

( )

∑ ∑

∆ ∆ ≡ ∆

ij ij ij T ij X ij ij ij X

z S z z argmin argmin ρ

Robust Cost Function scaled with

ij

W

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 u3 Control actions

Full SLAM: Problem Definition

36

( )

∏ ∏

= = −

=

M i K k jk ik k i i i X,L X,L

l x z p u x x p X p U Z L X p

1 1 1

) , | ( ) , | ( ) ( argmax , | , argmax

• Maximum a Posteriori (MAP) solution:

37

( )

∏ ∏

= = −

=

M i K k jk ik k i i i X,L X,L

l x z p u x x p X p U Z L X p

1 1 1

) , | ( ) , | ( ) ( argmax , | , argmax       + =

∑ ∑

= = − K k jk ik k M i i i i X,L

l x z p u x x p

1 1 1

) , | ( ln ) , | ( ln argmin

• Negative log-

likelihood

Likelihoods:

} ) , ( exp{ ) , | (

2 1 1

i

i i i i i

x u x f u x x p

Λ − −

− − ∝

Process model

} ) , ( exp{ ) , | (

2

k

k jk ik jk ik k

z l x h l x z p

Σ

− − ∝

Measurement model

Full SLAM

Full SLAM

38

      + =

∑ ∑

= = − K k jk ik k M i i i i X,L X,L

l x z p u x x p U Z L X p

1 1 1

) , | ( ln ) , | ( ln argmin

• )

, | , ( argmax

Putting the likelihoods into the equation:

      − + − =

∑ ∑

= Σ = Λ − K k k jk ik M i i i i X,L X,L

k i

z l x h x u x f U Z L X p

1 2 1 2 1

) , ( ) , ( argmin ) , | , ( argmax

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

39

( )

Z W J I WJ J

T T

∆ − = + δ λ

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

1 2 1

... ) , | , ( ) , | , (

−

∫∫ ∫

=

t t

dx dx dx U Z L X p U Z L x p K

t

x 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

t

x L

) , Ν( ~ ) , | , ( Σ µ U Z L x p

t

Mean Error covariance

42

) , (

1 −

=

t t t

u f µ µ

t T t t t t

R F F + Σ = Σ

−1

Prediction:

Process model Error propagation with process noise Kalman gain Update mean

1

) (

−

+ Σ Σ =

t T t t t T t t t

Q H H H K

t t t t

y K + = µ µ

t t t t

H K I Σ − = Σ ) (

Correction:

) (

t t t

h z y µ − =

Measurement residual (innovation) Update covariance

t t t

x h H ∂ ∂ = ) (µ

Measurement Jacobian

1 1)

, (

− −

∂ ∂ =

t t t t

x u f F µ

Process Jacobian

EKF SLAM

Structure of Mean and Covariance

43

                        = Σ                       =

2 2 2 2 2 2 2 1

2 1 2 2 2 1 2 2 2 1 2 1 1 1 1 1 2 1 2 1 2 1

,

N N N N N N N N N N N

l l l l l l yl xl l l l l l l yl xl l l l l l l yl xl l l l y x yl yl yl y y xy xl xl xl x xy x t N t

l l l y x σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ σ θ µ

θ θ θ θ θ θ θ θ θ θ θ

L M O M M M M M L L L L L M

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

) , | , ( U Z L x p

t

) , | , ( U Z L x p

t

FastSLAM

45

} , ... , , , , {

, , , 2 , 2 , 1 , 1

> Σ < > Σ < > Σ < =

m t N m t N m t m t m t m t m t m t

x p µ µ µ

Each particle represents:

Robot state Landmark state (mean and covariance)

) , | ( ~

1 t t t m t

u x x p x

−

Sample the robot state from the process model

) , | (

, t m t m t n

z x L p

N Kalman filter Landmark updates

) , | (

m t m t t m t

x L z p w ∝

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 j i ij X

ij

v v h z

2

) , ( 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 20 Introduction Feb 27 Geometry, Camera Model, Calibration Mar 6 Features, Tracking / Matching Mar 13

Project Proposals by Students

Mar 20 Structure from Motion (SfM) + papers Mar 27 Dense Correspondence (stereo / optical flow) + papers Apr 3 Bundle Adjustment & SLAM + papers Apr 10

Student Midterm Presentations

Apr 17 Easter Break Apr 24 Multi-View Stereo & Volumetric Modeling + papers May 1 Labor Day May 8 3D Modeling with Depth Sensors + papers May 15 3D Scene Understanding + papers May 22 4D Video & Dynamic Scenes + papers May 29

Student Project Demo Day = Final Presentations

Schedule

Next week: Midterm Presentations

Reminder: Prepare short presentation for Monday!