Augmented Reality VU numerical optimization algorithms. Prof. Vincent Lepetit

Transcription

1 Augmented Reality VU numerical optimization algorithms Prof. Vincent Lepetit

2 P3P: can exploit only 3 correspondences; DLT: difficult to exploit the knowledge of the internal parameters correctly, and the constraint that R should be a rotation matrix. M 3 M 1 M2 M 4 m 1 m 3 m 4 m 2 C

3 Minimization of the Reprojection Error min R,T i Proj A,R,T (M i ) m i 2 Minimization of a physical, meaningful error (reprojection error, in pixels); No restriction on the number of correspondences; Can be very accurate. M 3 M 1 M 2 m 1 m 3 M 4 m 4 Proj A,R,T (M 4 ) A[R T]M 4 m 2 C

4 Minimization of the Reprojection Error min R,T i Proj A,R,T (M i ) m i 2 Non-linear least-squares minimization; Requires an iterative numerical optimization è Requires an initialization. M 3 M 1 M 2 m 1 m 3 M 4 m 4 Proj A,R,T (M 4 ) A[R T]M 4 m 2 C

5 Toy Problem True camera position at (0, 0) 1D camera under 2D translation reprojection error 100 "3D points" taken at randomly in [400;1000]x[-500;+500]

6 Gaussian Noise on the Projections White cross: true camera position; Black cross: global minimum of the objective function. In that case, the global minimum of the objective function is close to the true camera pose.

7 Numerical Optimization p 2 p 1 p 0 Start from an initial guess p 0 : p 0 can be taken randomly but should be as close as possible to the global minimum: - pose computed at time t-1; - pose predicted from pose computed at time t-1 and a motion model; -...

8 Numerical Optimization General methods: Gradient descent / Steepest Descent; Conjugate Gradient;... Non-linear Least-squares optimization: Gauss-Newton; Levenberg-Marquardt;...

9 Numerical Optimization We want to find p that minimizes: i E(p) = Proj A,R(p ),T(p ) (M i ) m i = f (p) b 2 2 where p is a vector of parameters that define the camera pose (translation vector + parameters of the rotation matrix); b is a vector made of the measurements (here the m i ); f is the function that relates the camera pose to these measurements. " u Proj A,R(p ),T(p ) (M 1 ) $ f (p) = $ v Proj A,R(p ),T(p ) (M 1 ) $ # ( ) ( ) % ' ' ' & " u(m 1 )% $ ' b = $ v(m 1 ) ' # $ &'

10 Gradient descent / Steepest Descent p i+1 = p i λ E(p i ) ( ) T ( f (p i ) b) ( ) with J the Jacobian matrix of f, computed at p i E(p i ) = f (p i ) b 2 = f (p i ) b E(p i ) = 2J f (p i ) b Weaknesses: - How to choose λ? - Needs a lot of iterations in long and narrow valleys:

11 The Gauss-Newton and the Levenberg-Marquardt algorithms But first, the Linear Least-Squares Case: E(p) = f (p) b 2 If the function f is linear ie f(p) = Ap, p can be estimated as: p=a + b where A + is the pseudo-inverse of A: A + =(A T A) -1 A T

12 Non-Linear Least-Squares: The Gauss-Newton algorithm Iteration steps: p i+1 =p i + i i is chosen to minimize the residual f(p i+1 ) b 2. It is computed by approximating f to the first order: Δ i = argmin Δ = argmin Δ = argmin Δ f (p i + Δ) b 2 f (p i ) + JΔ b 2 ε i + JΔ 2 First order approximation: f (p i + Δ) f (p i ) + JΔ ε i = f (p i ) b denotes the residual at iteration i Δ i is the solution of the system JΔ = ε i in the least squares sense : Δ i = J + ε i where J + is the pseudo - inverse of J

13 Non-Linear Least-Squares: The Levenberg-Marquardt Algorithm In the Gauss-Newton algorithm: In the Levenberg-Marquardt algorithm: Levenberg-Marquardt Algorithm: 0. Initialize λ with a small value: λ = Compute i and E(p i + i ) Δ i = ( J T J) 1 J T ε i Δ i = ( J T J + λi) 1 J T ε i 2. If E(p i + i ) > E(p i ): λ 10 λ and go back to 1 [happens when the linear approximation of f is too rough] 3. If E(p i + i ) < E(p i ): λ λ / 10, p i+1 p i + i and go back to 1. Once converged, set λ 0 and continue up to convergence.

14 Non-Linear Least-Squares: the Levenberg-Marquardt Algorithm Δ i = ( J T J + λi) 1 J T ε i When λ is small, LM behaves similarly to the Gauss-Newton algorithm. When λ becomes large, LM behaves similarly to a steepest descent to guarantee convergence.

15 Error Estimation Let first assume that p can be computed by: p = g(m) where m is the vector made of the coordinates m i If we want to represent the density of p by a normal distribution, the covariance matrix of this normal distribution is with A the Jacobian matrix of g. Σ p = A Σ m A T We have in fact: m = f(p) Therefore the covariance matrix is: Σ p = J -1 Σ m J T

16 Error Estimation p Σ p The true pose is inside the ellipsoid p p ( ) T Σ 1 p ( p p ) = F 1 n (α) where p is the minimum found by the optimization with a probability α

17 Examples

18 Examples [Here the pose is computed using both 3D/2D correspondences and apparent motion, that is why the uncertainty in depth is less important than in the previous example. Note however that the uncertainties grow when the camera goes far from the 3D model]

19

20 Parameterizing the Rotation Matrix

21 Possible Parameterizations of the Rotation Matrix Rotation in 3D space has only 3 degrees of freedom. It would be awkward to use the nine elements as its parameters. Possible parameterizations: Euler Angles; Quaternions; Exponential Map. All have singularities, can be avoided by locally reparameterizing the rotation. Exponential map has the best properties. [From Grassia JGT98]

22 Euler Angles Rotation defined by angles of rotation around the X-, Y-, and Z- axes. Different conventions. For example: $ cosα sinα 0' $ cosβ 0 sinβ' $ ' & )& )& ) R = & sinα cosα 0 )& )& 0 cosγ sinγ ) %& 0 0 1( )%& sinβ 0 cosβ( )%& 0 sinγ cosγ () Z α β Y X γ

23 Gimbal Lock and Optimization When β = π/2, % 0 sin(γ α) cos(γ α) ( ' * R = ' 0 cos(γ α) sin(γ α) * &' )* the rotation by γ can be cancelled by taking α=γ. That means that - for each possible angle θ, all (α, β = π/2, γ= α + θ) correspond to the same rotation matrix => for each possible angle θ, there is a flat valley of axis (α, β = π/2, γ= α + θ) in the energy to be minimized L

24 Gimbal Lock When β = π/2, $ cosα sinα 0' $ cosβ 0 sinβ' $ ' R = & )& )& ) & sinα cosα 0 )& )& 0 cosγ sinγ ) %& 0 0 1( )%& sinβ 0 cosβ( )%& 0 sinγ cosγ () $ cosα sinα 0' $ 0 0 1' $ ' & )& )& ) = & sinα cosα 0 )& )& 0 cosγ sinγ ) %& 0 0 1( )%& 1 0 0( )%& 0 sinγ cosγ () $ cosα sinα 0' $ 0 sinγ cosγ ' & )& ) = & sinα cosα 0 )& 0 cosγ sinγ ) %& 0 0 1( )%& () $ 0 cosα sinγ sinα cosγ cosα cosγ + sinα sinγ' $ 0 sin(γ α) cos(γ α) ' & ) & ) = & 0 sinα sinγ + cosα cosγ sinα cosγ cosα sinγ ) = & 0 cos(γ α) sin(γ α) ) %& () %& ()

25 A Unit Quaternion Quaternions are hyper-complex numbers that can be written as the linear combination a+bi+cj+dk, with i 2 = j 2 = k 2 = ijk = -1. Can also be interpreted as a scalar plus a 3- vector: (a, v). A rotation about the unit vector w by an angle θ can be represented by the unit quaternion: # q = cos θ 2,wsinθ & % ( $ 2' Z θ w Y X

26 A Unit Quaternion Quaternions are hyper-complex numbers that can be written as the linear combination a+bi+cj+dk, with i 2 = j 2 = k 2 = ijk = -1. Can also be interpreted as a scalar plus a 3- vector: (a, v). A rotation about the unit vector w by an angle θ can be represented by the unit quaternion: To rotate a 3D point M: write it as a quaternion p = (0, M), and take the rotated point p' to be No gimbal lock. # q = cos θ 2,wsinθ & % ( $ 2' $ p'= q.p.q with q = cos θ 2, wsinθ ' & ) % 2( Parameterization of the rotation using the 4 coordinates of a quaternion q: 1. No constraint and rotation performed using singularity: kq yields the same rotation whatever the value of k > 0; q q 2. Additional constraint: norm of q must be constrained to be equal to 1, for example by adding the quadratic term K(1 - q 2 ).

27 Exponential Maps No gimbal lock; No additional constraints; Singularities occur in a region that can easily be avoided. J Parameterization by a 3D vector w=[w 1,w 2,w 3 ] T : Rotation around the axis of direction w of an amount of w Z w w Y X

28 The rotation matrix is given by: Rodrigues' Formula $ 0 w z w y ' & ) Ω = & w z 0 w x ) % & w y w x 0 ( ) R( Ω) = exp(ω) = I + Ω + 1 2! Ω ! Ω3 + = I + sinθ θ ( ) Ω + 1 cosθ θ 2 Ω 2 (Rodrigues' formula) Not singular for small values of θ even if we divide by θ (see Taylor expansions).

29 The Singularities of Exponential Maps Rotation around the axis of direction w of an amount of w à Singularities for w such that w = 2nπ : No rotation, whatever the direction of w. Z w X 4π 2π w Y Avoided during optimization as follows: when w becomes close to 2nπ, say higher than π, w can be replaced by [From Grassia JGT98] $ & 1 2π % w ' ) w (

30 Parameterization of the Rotation Matrix Conclusion: Use exponential maps. More details in [Grassia JGT98]

31 Computing J We need to compute J, the Jacobian of f: " u Proj A,R(p ),T(p ) (M 1 ) $ f (p) = $ v Proj A,R(p ),T(p ) (M 1 ) $ # ( ) ( ) Solution 1: Use Maple or Matlab to produce the analytical form, AND the code. % ' ' ' & Solution 2: Do it the old way...

32 Solution 2 First, decompose f: f M1 ( p) = m m M cam M1 ( p) where M cam M1 p m M M1 ( ( )) " u Proj A,R(p ),T(p ) (M 1 ) $ f (p) = $ v Proj A,R(p ),T(p ) (M 1 ) $ # ( ) ( ) % ' " ' = f M 1 ( p) % $ ' ' # & & ( ) returns M 1 in the camera coordinates system defined by p; ( cam ) returns the projection of M M1 m m ( ) returns the 2D vector corresponding to cam in homogeneous coordinates; m.

33 Chain Rule " u Proj A,R(p ),T(p ) (M 1 ) $ f (p) = $ v Proj A,R(p ),T(p ) (M 1 ) $ # J = f # # & % ( = % $ p' % $ ( ) ( ) f M1 p & ( ( with # f M 1 & % ( $ p ' ' % ' " ' = f M 1 ( p) % $ ' with f M1 ' # & & 2 6 # = m & $ % m ' ( 2 3 # % $ % m M M1 cam ( p) = m m M M1 & # M M1 ( % '( p 3 3 $ ( ( cam ( p) )) cam & ( ' 3 6

34 ( ) = $ u v m m " # " U % % $ ' = W ' $ & V ' $ ' # W & with m = " U % $ ' $ V ' # $ W &' m m = # u % U % v % $ U u V v V u & # W ( 1/W 0 U & % v ( = W 2 ( % ( 0 1/W V ( % ( W ' $ W 2 '

35 ( cam ) cam = AM M1 m M M1 m = A cam M M1

36 M cam M1 ( p) = R( p)m 1 + T [ ] T $ T = [ t x, t y, t ] T z With p = w x, w y, w z, t x, t y, t z cam M M1 p " # % ' : & ) = M cam cam cam cam cam M 1 M M1 M M1 M M1 M M1 + * w x w y w z t x t y ) ) R, ) R, 1 0 0, + ) R,. = + +. M * w 1 +. M x - * w 1 +. M y - * w z - * # R & % (... $ w x ' cam M M1 The matrices can be computed from the expansion of R. 3 3 In C, one can use the cvrodrigues function from the OpenCV library. t z,. -

37 Robust Estimation

38 What if there are Outliers? M 3 M 1 M2 M 4 m 1 m 3 m 4 m 2 C incorrect measure

39 Gaussian Noise on the Projections + 20% outliers White cross: true camera position; Black cross: global minimum of the objective function. The global minimum is now far from the true camera pose.

40 Bayesian interpretation: What Happened? argmin p = argmax p i i Proj A,R(p ),T(p ) (M i ) m i N( Proj A,R(p ),T(p ) (M i ); m i,σi) 2 The error on the 2D point locations m i is assumed to have a Gaussian (Normal) distribution with identical covariance matrices σi, and independent; M 3 M 1 M 2 This assumption is violated when m i is an outlier. m 1 m 3 m 4 M 4 m 2 C

41 min p = min p = min p = max p i i i exp Proj A,R(p ),T(p ) (M i ) m i 2 ( Proj A,R(p ),T(p ) (M i ) m ) T ( i Proj A,R(p ),T(p ) (M i ) m ) i ( Proj A,R(p ),T(p ) (M i ) m ) T % 1/σ 0 ( i ' * Proj A,R(p ),T(p ) (M i ) m i & 0 1/σ) i ( ) ( Proj A,R(p ),T(p ) (M i ) m ) T i Σ ( m Proj A,R(p ),T(p ) (M i ) m ) i % with Σ m = 1/σ 0 ( ' * & 0 1/σ) = max p = max p i i (the 2 equivalent formulations) 1 ( ( ) T Σ ( m Proj A,R(p ),T(p ) (M i ) m )) i exp Proj ( 2π) 2 A,R(p ),T(p ) (M i ) m i Σ m N( Proj A,R(p ),T(p ) (M i ); m i,σ ) m

42 Robust estimation Idea: Replace the Normal distribution by a more suitable distribution, or equivalently replace the least-squares estimator by a "robust estimator": min p min p i i Proj A,R(p ),T(p ) (M i ) m i ρ( Proj A,R(p ),T(p ) (M i ) m ) i 2

43 Example of an M-estimator: The Tukey Estimator x 2 ρ(x) 1 3 if x c if x > c 4 ρ( x) = c 2 % ' 1 % ' x ( - * ', & c ) & ρ( x) = c / 3 ( * * ) The Tukey estimator assumes the measures follow a distribution that is a mixture of: a Normal distribution, for the inliers, a uniform distribution, for the outliers.

44 Normal distribution (inliers) Uniform distribution (outliers) Mixture + = -log(.) -log(.) Least-squares Tukey estimator

45 The Tukey Estimator in Levenberg- Marquardt Optimization Use the following approximation: % ' if x c & ' if x > c ( ρ( x) = 1 2 x 2 (least squares) ρ( x) = 1 c 2 2 (constante)

46 Other M-Estimators

47 Drawbacks of the Tukey Estimator - Non-convex à creates local minimums; - Function becomes flat when too far from the global minimum.

48 Gaussian Noise on the Projections + 20% outliers + Tukey estimator White cross: true camera position; Black cross: global minimum of the object function. The global minimum is very close to the true camera pose. BUT: - local minimums; - the objective function is flat where all the correspondences are considered outliers.

49 Gaussian Noise on the Projections + 50% outliers + Tukey estimator Even more local minimums. Numerical optimization can get trapped into a local minimum.

50 RANSAC

51 How to Optimize? Idea: sampling the space of solutions (the camera pose space here):

52 How to Optimize? Idea: sampling the space of solutions: + Numerical Optimization from the best sampled pose. Problem: Exhaustive regular sampling is too expensive in 6 dimensions. Can we do a smarter sampling?

53 RANSAC RANSAC: RANdom SAmple Consensus Line fitting: the "Throwing Out the worst residual" heuristics can fail (Example for the original paper [Fischler81]): outlier final least-squares solution Ideal line

54 RANSAC As before, we could do a regular sampling, but would not be optimal: Ideal line

55 Idea: Generate hypotheses from subsets of the measurements. If a subset contains no gross errors, the estimated parameters (the hypothesis) are closed to the true ones. Take several subsets at random, retain the best one. Ideal line

56 The quality of a hypothesis is evaluated by the number of measures that lie "close enough" to the predicted line. We need to choose a threshold (T) to decide if the measure is "close enough". RANSAC returns the best hypothesis, ie the hypothesis with the largest number of inliers. T i # 1 if dist(m i,line( p)) T $ % 0 if dist(m i,line( p)) > T

57 Required Number N of Trials To ensure with a probability p that at least one of the sample is free from outliers: Take: N > log(1-p) / log(1-(1-w) n ), where: w is the proportion of outliers, and n the number of measures in a subset. Unfortunately, N increases exponentially with w, the proportion of outliers...

58 Pose Estimation To apply RANSAC to pose estimation, we need a way to compute a camera pose from a subset of measurements, for example a P3P algorithm. Since RANSAC only provides a solution estimated with a limited number of data, it must be followed by a robust minimization to refine the solution.

59 RANSAC What happens when too many measurements are outlier? RANSAC needs a large number of trials to find a subset without any outlier.

60 PROSAC [Chum & Matas05] In some applications, we have a measure of confidence for each measurement, for example a correlation measure.

61 PROSAC [Chum & Matas05] Algorithm: 1. sort the measurements according to their confidence. 2. First trial: subset taken to be the n first measurements; 3. Second trial: subset taken at random in the n+1 first measurements

62 PROSAC Starts by testing the most promising hypotheses; converges to uniform sampling as in RANSAC. à PROSAC tests the same hypotheses than RANSAC but in a different order.

63 Other Variants of RANSAC MLESAC [Torr & Zissermann CVIU'00]: the quality of the hypothesis is measured using a likelihood function, not the number of inliers. The T d,d Test [Matas & Chum, ICV'04]: First, evaluate a hypothesis only on a random small subset of measures (1 measure is actually sufficient). If all the measures in the subset are inliers, evaluate the hypothesis on all the measures. Preemptive RANSAC [Nister, ICCV'03]: All the hypotheses are generated first, and then evaluated in parallel on a subset of measures. Only the best hypotheses are kept and evaluated on a next subset. The evaluation can be interrupted at any time. good review in "A Comparative Analysis of RANSAC Techniques Leading to Adaptive Real-Time Random Sample Consensus", Rahul Raguram, Jan- Michael Frahm, and Marc Pollefeys. ECCV'08.