3D Vision Marc Pollefeys, Viktor Larsson Spring 2020 Schedule Feb - - PowerPoint PPT Presentation

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
• 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

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 ik 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 ik 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 

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                                      

          

             

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 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!