Pose Tracking II! Gordon Wetzstein! Stanford University! EE 267 Virtual Reality! Lecture 12! stanford.edu/class/ee267/!

Pose Tracking II! Gordon Wetzstein! Stanford University! EE 267 Virtual Reality! Lecture 12! stanford.edu/class/ee267/!!

WARNING! this class will be dense! will learn how to use nonlinear optimization (Levenberg- Marquardt algorithm) for pose estimation! why???!! more accurate than homography method! can dial in lens distortion estimation, and estimation of intrinsic parameters (beyond this lecture, see lecture notes)! LM is very common in 3D computer vision à camera-based tracking!

Pose Estimation - Overview! x i y i 0 goal: estimate pose via nonlinear least squares optimization! minimize p { } 2 b f ( g( p) ) 2 image formation minimize reprojection error! pose p is 6-element vector with 3 Euler angles and translation of VRduino w.r.t. base station! x i n y i n b = x 1 n y 1 n! x 4 n y 4 n p = θ x θ y θ z t x t y t z

Overview! review: gradients, Jacobian matrix, chain rule, iterative optimization! nonlinear optimization: Gauss-Newton, Levenberg-Marquardt! pose estimation using LM! pose estimation with VRduino using nonlinear optimization!

Review!

Review: Gradients! gradient of a function that depends on multiple variables:! x f x ( ) = f, f,, f x 1 x 2 x n f :R n R

Review: The Jacobian Matrix! gradient of a vector-valued function that depends on multiple variables:! x f x ( ) = J f = f 1! f 1 x 1 x n " # " f m! f m x 1 x n f :R n R m, J f R m n

Review: The Chain Rule! here s how you ve probably been using it so far:! x f g x ( ( )) = f g g x this rule applies when! f :R R g :R R

Review: The Chain Rule! here s how it is applied in general:! x f g x ( ( ) ) = J f J g = f 1! f 1 g 1 g o " # " f m! f m g 1 g o g 1! g 1 x 1 x n " # " g o! g o x 1 x n f :R o R m, g :R n R o, J f R m o, J g R o n

Review: Minimizing a Function! goal: find point x * that minimizes a nonlinear function f(x)! f ( x) x * x

Review: What is a Gradient?! gradient of f at some point x 0 is the slope at that point! f ( x) x 0 x

Review: What is a Gradient?! gradient of f at some point x 0 is the slope at that point! f ( x) x 0 x

Review: What is a Gradient?! extremum is where gradient is 0! (sometimes have to check 2 nd derivative to see if it s a minimum and not a maximum or saddle point)! f ( x) x * x

Review: Optimization! extremum is where gradient is 0! (sometimes have to check 2 nd derivative to see if it s a minimum and not a maximum or saddle point)! convex! non-convex! f ( x) f ( x) x x! convex optimization: there is only a single global minimum! non-convex optimization: multiple local minima!

Review: Optimization! how to find where gradient is 0?! f ( x)! x

Review: Optimization! how to find where gradient is 0?! f ( x) x 0 x 1. start with some initial guess x 0, e.g. a random value!

Review: Optimization! how to find where gradient is 0?! f ( x) x 0 x 1. start with some initial guess x 0, e.g. a random value! 2. update guess by linearizing function and minimizing that!

Review: Optimization! how to linearize a function? à using Taylor expansion!! f ( x) f ( x 0 + Δx) f ( x 0 ) + J f Δx + ε x 0 x

Review: Optimization! find minimum of linear function approximation! f ( x) f ( x 0 + Δx) f ( x 0 ) + J f Δx + ε x 0 x minimize Δx 2 b f ( x 0 + Δx) 2 2 b ( f ( x 0 ) + J f Δx) 2

Review: Optimization! find minimum of linear function approximation (gradient=0)! f ( x) f ( x 0 + Δx) f ( x 0 ) + J f Δx + ε x 0 equate gradient to zero:! 0 = J f T J f Δx J f T b f x x minimize Δx ( ( )) Δx = J T f J f 2 b f ( x 0 + Δx) 2 2 b ( f ( x 0 ) + J f Δx) 2 ( ) 1 J f T b f x normal equations! ( ( ))

Review: Optimization! take step and repeat procedure! f ( x) x 0 x 0 + Δx x ( ) 1 J f T b f x Δx = J f T J f ( ( ))

Review: Optimization! take step and repeat procedure, will get there eventually! f ( x) x 0 x 0 + Δx x ( ) 1 J f T b f x Δx = J f T J f ( ( ))

Review: Optimization Gauss-Newton! results in an iterative algorithm! f ( x) x k+1 = x k + Δx x 0 x ( ) 1 J f T b f x Δx = J f T J f ( ( ))

Review: Optimization Gauss-Newton! results in an iterative algorithm! x 0 f ( x) x 1. x = rand() // initialize x0 2. for k=1 to max_iter 3. f = eval_objective(x) 4. J = eval_jacobian(x) 5. x = x + inv(j *J)*J *(b-f) // update x ( ) 1 J f T b f x Δx = J f T J f ( ( ))

Review: Optimization Gauss-Newton! matrix J T J can be ill-conditioned (i.e. not invertible)! f ( x) x 0 x ( ) 1 J f T b f x Δx = J f T J f ( ( ))

Review: Optimization Levenberg! matrix J T J can be ill-conditioned (i.e. not invertible)! add a diagonal matrix to make invertible acts as damping! f ( x) x 0 x ( ) 1 J f T b f x Δx = J f T J f + λi ( ( ))

Review: Optimization Levenberg-Marquardt!! matrix J T J can be ill-conditioned (i.e. not invertible)! better: use J T J instead of I as damping. This is LM!! f ( x) x 0 x ( ) 1 J f T b f x Δx = J T f J f + λ diag( J T f J f ) ( ( ))

Review: Optimization Levenberg-Marquardt!! matrix J T J can be ill-conditioned (i.e. not invertible)! better: use J T J instead of I as damping. This is LM!! x 0 f ( x) x 1. x = rand() // initialize x0 2. for k=1 to max_iter 3. f = eval_objective(x) 4. J = eval_jacobian(x) 5. x = x+inv(j *J+lambda*diag(J *J))*J *(b-f) ( ) 1 J f T b f x Δx = J T f J f + λ diag( J T f J f ) ( ( ))

Pose Estimation via Levenberg-Marquardt!

Pose Estimation - Overview! x i y i 0 goal: estimate pose via nonlinear least squares optimization! minimize p { } 2 b f ( g( p) ) 2 image formation minimize reprojection error! pose p is 6-element vector with 3 Euler angles and translation of VRduino w.r.t. base station! x i n y i n b = x 1 n y 1 n! x 4 n y 4 n p = θ x θ y θ z t x t y t z

Pose Estimation - Objective Function! goal: estimate pose via nonlinear least squares optimization! minimize p { } 2 b f ( g( p) ) 2 x i y i 0 image formation objective function is sum of squares of reprojection error! x i n y i n 2 ( ) = n x1 f 2 1 ( g( p) ) b f g( p) ( ) 2 + ( y n 1 f 2 ( g( p) )) 2 + + ( x n 4 f 7 ( g( p) )) 2 + ( y n 4 f 8 ( g( p) )) 2

Image Formation! 1. transform 3D point into view space:! x i c y i c z i c = = 1 0 0 0 1 0 0 0 1 h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 r 11 r 12 t x r 21 r 22 t y r 31 r 32 t z x i y i 1 x i y i 1 x i n y i n x i y i 0 2. perspective divide:! x i n y i n = x i c z i c y i c z i c

Image Formation: g(p) and f(h)! split up image formation into two functions! ( ) = f g( p) f h ( ) g :R 6 R 9, f :R 9 R 8

Image Formation: f(h)! f(h) uses elements of homography matrix h to compute projected 2D coordinates as! f ( h) = f 1 f 2 f 7 f 8 ( h) ( h)! h ( ) ( h) = n x 1 n y 1! n x 4 n y 4 = h 1 x 1 + h 2 y 1 + h 3 h 7 x 1 + h 8 y 1 + h 9 h 4 x 1 + h 5 y 1 + h 6 h 7 x 1 + h 8 y 1 + h 9! h 1 x 4 + h 2 y 4 + h 3 h 7 x 4 + h 8 y 4 + h 9 h 4 x 4 + h 5 y 4 + h 6 h 7 x 4 + h 8 y 4 + h 9

! f 1 Jacobian Matrix of f(h)! ( h) = h x +h y +h 1 1 2 1 3 h 7 x 1 +h 8 y 1 +h 9 J f = f 1! f 1 h 1 h 9 " # " f 8! f 8 h 1 h 9 first row of Jacobian matrix! f 1 h 1 = f 1 h 2 = f 1 h 3 = f 1 h 4 = 0, x 1 h 7 x 1 + h 8 y 1 + h 9 y 1 h 7 x 1 + h 8 y 1 + h 9 1 h 7 x 1 + h 8 y 1 + h 9 f 1 h 5 = 0, f 1 h 6 = 0 f 1 = h 7 f 1 = h 8 h 1 x 1 + h 2 y 1 + h 3 ( ) 2 h 7 x 1 + h 8 y 1 + h 9 h 1 x 1 + h 2 y 1 + h 3 ( ) 2 h 7 x 1 + h 8 y 1 + h 9 f 1 = h x + h y + h 1 1 2 1 3 h 9 h 7 x 1 + h 8 y 1 + h 9 ( ) 2 x 1 y 1

! Jacobian Matrix of f(h)! the remaining rows of the Jacobian can be derived with a similar pattern! J f = f 1! f 1 h 1 h 9 " # " f 8! f 8 h 1 h 9 see course notes for a detailed deriavtion of the elements of this Jacobian matrix!

Image Formation: g(p)! g(p) uses 6 pose parameters to compute elements of homography matrix h as! g( p) = g 1 g 9 ( p)! p ( ) = h 1! h 9 definition of homography matrix:! h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 = 1 0 0 0 1 0 0 0 1 r 11 r 12 t x r 21 r 22 t y r 31 r 32 t z rotation matrix from Euler angles:! ( ) sin( θ z ) 0 ( ) R = R z ( θ z ) R x ( θ x ) R y θ y r 11 r 12 r 13 cos θ z 1 0 0 r 21 r 22 r 23 = sin( θ z ) cos( θ z ) 0 0 cos θ x r 31 r 32 r 33 0 0 1 0 sin θ x ( ) sin( θ x ) ( ) cos( θ x ) cos( θ y ) 0 sin( θ y ) 0 1 0 sin θ y ( ) 0 cos( θ y )

write as! Image Formation: g(p)! h 1 = g 1 ( p) = cos( θ y )cos θ z h 2 = g 2 ( p) = cos( θ x )sin θ z h 3 = g 3 ( p) = t x h 4 = g 4 ( p) = cos( θ y )sin θ z h 5 = g 5 ( p) = cos( θ x )cos θ z h 6 = g 6 ( p) = t y h 7 = g 7 ( p) = cos( θ x )sin( θ y ) h 8 = g 8 ( p) = sin( θ x ) h 9 = g 9 ( p) = t z ( ) sin θ x ( ) ( ) + sin θ x ( ) ( )sin( θ y )sin θ z ( ) ( )sin( θ y )cos θ z ( )

! Jacobian Matrix of g(p)! ( ) p = ( p 1, p 2, p 3, p 4, p 5, p 6 ) = θ x, θ y, θ z, t x, t y, t z h 1 = g 1 ( p) = cos( θ y )cos θ z h 2 = g 2 ( p) = cos( θ x )sin θ z ( ) sin θ x ( ) ( )sin( θ y )sin θ z ( ) J g = g 1! g 1 p 1 p 6 " # " g 9! g 9 p 1 p 6 h 3 = g 3 ( p) = t x h 4 = g 4 ( p) = cos( θ y )sin θ z h 5 = g 5 ( p) = cos( θ x )cos θ z ( ) + sin θ x ( ) ( )sin( θ y )cos θ z ( ) h 6 = g 6 ( p) = t y h 7 = g 7 ( p) = cos( θ x )sin θ y h 8 = g 8 ( p) = sin( θ x ) ( ) h 9 = g 9 ( p) = t z

! h 1 = g 1 ( p) = cos( θ y )cos θ z Jacobian Matrix of g(p)! p = ( p 1, p 2, p 3, p 4, p 5, p 6 ) = θ x, θ y, θ z, t x, t y, t z ( ) J g = ( ) sin θ x ( )sin( θ y )sin θ z ( ) g 1! g 1 p 1 p 6 " # " g 9! g 9 p 1 p 6 first row of Jacobian matrix! g 1 = cos( θ x )sin( θ y )sin( θ z ) p 1 g 1 = sin( θ y )cos( θ z ) sin( θ x )cos( θ y )sin( θ z ) p 2 g 1 = cos( θ y )sin( θ z ) sin( θ x )sin( θ y )cos( θ z ) p 3 g 1 p 4 = 0, g 1 p 5 = 0, g 1 p 6 = 0

! Jacobian Matrix of g(p)! the remaining rows of the Jacobian can be derived with a similar pattern! J g = g 1! g 1 p 1 p 6 " # " g 9! g 9 p 1 p 6 see course notes for a detailed deriavtion of the elements of this Jacobian matrix!

! Jacobian Matrices of f and g! to get the Jacobian of f(g(p)), compute the two Jacobian matrices and multiply them! J = J f J g = f 1! f 1 h 1 h 9 " # " f 8! f 8 h 1 h 9 g 1! g 1 p 1 p 6 " # " g 9! g 9 p 1 p 6

Pose Tracking with LM! LM then iteratively updates pose as! ( ( )) 1 J T b f p k p ( k+1) = p ( k) + J T J + λ diag J T J ( ( ( ) )) pseudo-code! 1. p =... // initialize p0 2. for k=1 to max_iter 3. f = eval_objective(p) 4. J = get_jacobian(p) 5. p = p + inv(j *J+lambda*diag(J *J))*J *(b-f)

Pose Tracking with LM! 1. value = function eval_objective(p) 2. for i=1:4 3. value(2*(i-1)) =... 4. value(2*(i-1)+1) =... h 1 x i + h 2 y i + h 3 h 7 x i + h 8 y i + h 9 h 4 x i + h 5 y i + h 6 h 7 x i + h 8 y i + h 9

Pose Tracking with LM! 1. J = function get_jacobian(p) 2. Jf = get_jacobian_f(p) 3. Jg = get_jacobian_g(p) 4. J = Jf*Jg J f = J g = f 1! f 1 h 1 h 9 " # " f 8! f 8 h 1 h 9 g 1! g 1 p 1 p 6 " # " g 9! g 9 p 1 p 6

!! Pose Tracking with LM on VRduino! some more hints for implementation:! let Arduino Matrix library compute matrix-matrix multiplications and also matrix inverses for you!! run homography method and use that to initialize p for LM! use something like 5-25 interations of LM per frame for real-time performance! user-defined parameter! λ good luck!

Pose Tracking with LM on VRduino! live demo!

Outlook: Camera Calibration! http://www.vision.caltech.edu/bouguetj/calib_doc/! camera calibration is one of the most fundamental problems in computer vision and imaging! task: estimate intrinsic (lens distortion, focal length, principle point) & extrinsic (translation, rotation) camera parameters given images of planar checkerboards! uses similar procedure as discussed today!

Outlook: Sensor Fusion with Extended Kalman Filter!! also desirable: estimate bias of each of all IMU sensors! also desirable: joint pose estimation from all IMU + photodiode measurements! can do all of that with an Extended Kalman Filter - slightly too advanced for this class, but you can find a lot of literature in the robotic vision community!

Outlook: Sensor Fusion with Extended Kalman Filter! Extended Kalman filter: can be interpreted as a Bayesian framework for sensor fusion! Hidden Markov Model (HMM)! known initial state! x x 0 1 x t 1 x unknown, evolving t states! z 1 z t 1 z t measurements!

Must read: course notes on tracking!!