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!