3D Vision Andreas Geiger, Torsten Sattler Spring 2017
Schedule Feb 20 Introduction Feb 27 Geometry, Camera Model, Calibration Mar 6 Features, Tracking / Matching Project Proposals by Students Mar 13 Mar 20 Structure from Motion (SfM) + papers Mar 27 Dense Correspondence (stereo / optical flow) + papers Bundle Adjustment & SLAM + papers Apr 3 Student Midterm Presentations Apr 10 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 Student Project Demo Day = Final Presentations May 29
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 . z P Cost Function: j ( ) ∑∑ 2 − π argmin , x P C ij j i W ( ) ij X i j π X , j C ∆ j z ij x ij ∆ z ij X = [ , ] P C C i 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 . z P Cost Function: j ∑∑ ∆ ∆ T argmin z W z ij ij ij ( ) X i j π X , j C j ( ) f X x ij − 1 : W ∆ Measurement error covariance z ij ij X = [ , ] P C C i 10
Bundle Adjustment ( ) • Minimize the cost function: argmin f X X 1. Gradient Descent 2. Newton Method 3. Gauss-Newton 4. Levenberg-Marquardt 11
Bundle Adjustment 1. Gradient Descent X k = Initialization: X 0 ( ) ∂ f X = = ∆ Compute gradient: T g Z WJ ∂ X = Iterate until X X k convergence ← − η Update: X X g k k ∂ π η = J : Jacobian : Step size ∂ X Slow convergence near minimum point! 12
Bundle Adjustment 2. Newton Method 2 nd order approximation (Quadratic Taylor Expansion): + δ ≈ + δ + δ δ T 1 ( ) ( ) f X f X g H = 2 = X X X X K K ( ) ∂ + δ 2 f X = Hessian matrix : H ∂ δ 2 = X X k δ + ) δ ( Find that minimizes ! f X = X X K 13
Bundle Adjustment 2. Newton Method Differentiate and set to 0 gives: − δ = − 1 H g ← + δ Update: X X k k Computation of H is not trivial and might get stuck at saddle point! 14
Bundle Adjustment 3. Gauss-Newton ∂ 2 π ∑∑ = + ∆ ij T H J WJ Z W ∂ ij ij 2 X i j ≈ T H J WJ Normal equation: δ = − ∆ T T J WJ J W Z ← + δ Update: X X k k Might get stuck and slow convergence at saddle point! 15
Bundle Adjustment 4. Levenberg-Marquardt λ Regularized Gauss-Newton with damping factor . ( ) + λ δ = − ∆ T T J WJ I J W Z H LM λ → 0 : Gauss-Newton (when convergence is rapid) λ → ∞ : Gradient descent (when convergence is slow) Adapt λ during optimization: • Decrease λ when function value decreases • Increase λ otherwise 16
Structure of the Jacobian and Hessian Matrices • Sparse matrices since 3D structures are locally observed. 17
Efficiently Solving the Normal Equation • Schur Complement: Exploit structure of H δ = − ∆ T H J W Z LM = H LM 3D Camera Structures Parameters 18
Efficiently Solving the Normal Equation • Schur Complement: Exploit structure of H δ = − ∆ T H J W Z LM H H SC s = H LM T H H C SC 3D Camera Structures Parameters 19
Efficiently Solving the Normal Equation • Schur Complement: Obtain reduced system δ = − ∆ T H J W Z LM δ ε H H 3D Structures = S SC S S δ ε T Camera Parameters H H SC C C C 0 I Multiply both sides by: − − 1 T H H I SC S δ ε H H = S SC S S − − − δ ε − ε 1 1 T T 0 H H H H H H C SC S SC C C S SC S 20
Efficiently Solving the Normal Equation • Schur Complement: Obtain reduced system δ ε H H = S SC S S − − − δ ε − ε 1 1 T T 0 H H H H H H C SC S SC C C S SC S δ First solve for from: C Easy to invert a block diagonal matrix − − δ = ε − ε − 1 1 T T ( ) H H H H H H C SC S SC C C S SC S Schur Complement (Sparse and Symmetric Positive Definite Matrix) δ Solve for by backward substitution. SC 21
Efficiently Solving the Normal Equation − − − δ = ε − ε ≡ Ax = 1 1 T T ( ) H H H H H H b C SC S SC C C S SC S Don’t solve as x=A -1 b: A is sparse, but A -1 is not! • Use sparse matrix factorization to solve system A = 1. LU Factorization LU Solve for x by forward- backward substitutions A = 2. QR factorization QR A = T 3. Cholesky Factorization LL • Iterative methods 1. Conjugate gradient 2. Gauss-Seidel 22
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. ( )( ) = T T T P AP P x P b Permutation matrix to reorder A • NP-Complete problem. • Approximate solutions: 1. Minimum degree 2. Column approximate minimum degree permutation 3. Reverse Cuthill-Mckee. 4. … 25
Problem of Fill-In 26
Robust Cost Function ∑ ∆ ∆ • Non-linear least squares: T argmin z W z ij ij ij X ij • Maximum log-likelihood solution: - argmin ln ( | ) p Z X X • Assume that: 1. X is a random variable that follows Gaussian distribution. 2. All observations are independent. ( ) ∏ = − ∆ ∆ T - argmin ln ( | ) - argmin ln exp p X Z c z W z ij ij ij ij X X ij ∑ = ∆ ∆ T argmin z W z ij ij ij X ij 27
Robust Cost Function • Gaussian distribution assumption is not true in the presence of outliers! • Causes wrong convergences. 28
Robust Cost Function ( ) ∑ ∑ ρ ∆ ≡ ∆ ∆ T argmin argmin z z S z ij ij ij ij ij X X ij ij ρ " Robust Cost Function W scaled with ij ij • Similar to iteratively re-weighted least-squares. • Weight is iteratively rescaled with the attenuating ρ " factor . ij • Attenuating factor is computed based on current error. 29
Robust Cost Function ρ ρ (.) " (.) Influence from high errors Gaussian Distribution Reduced influence from high errors Cauchy Distribution 30
Robust Cost Function Outliers are taken into account in Cauchy! 31
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 Control actions u 1 u 3 u 2 Robot poses Observations Map landmarks 35
Full SLAM: Problem Definition • Maximum a Posteriori (MAP) solution: M K ( ) ∏ ∏ = argmax , | , argmax ( ) ( | , ) ( | , ) p X L Z U p X p x x u p z x l − 0 i i 1 i k ik jk = = X,L X,L 1 1 i k 36
Recommend
More recommend
