Pose Estimation Using Four Corresponding Points M L

  • Slides: 26
Download presentation
Pose Estimation Using Four Corresponding Points M. L. Liu and K. H. Wong, "Pose

Pose Estimation Using Four Corresponding Points M. L. Liu and K. H. Wong, "Pose Estimation using Four Corresponding Points", Pattern Recognition Letters, Volume 20, Number 1 January 1999, pp. 69 -74. KH Wong Pose estimation using 4 points v. 6 a 1

Introduction • In this system, correspondences between four points on a rigid solid object

Introduction • In this system, correspondences between four points on a rigid solid object and four points in an image are used to estimate the new position and orientation of that rigid object under full perspective projection. Synthetic and real data have been used to verify our method. Pose estimation using 4 points v. 6 a 2

Problem definition • At time t, we have the model of the object A

Problem definition • At time t, we have the model of the object A 1, t, A 2, t, A 3, t, A 4, t, f (focal length) • And corresponding image points a 1, t, a 2, t, a 3, t, a 4, t • We want to find the pose l 1, t, l 2, t, l 3, t, l 4, t • From l 1, t, l 2, t, l 3, t, l 4, t we can find the rotation and translation of the object with respect to the camera coordinates Pose estimation using 4 points v. 6 a 3

Formulation • P is the center of perceptivity (camera center) • The vector PAn,

Formulation • P is the center of perceptivity (camera center) • The vector PAn, t • passes through (unit vector un, t ) passes through points an, t and An, t • The length is ln, t • Equation (1) and (2) Pose estimation using 4 points v. 6 a 4

Uising the Law of cosine • Assume 0, A 1, A 2 is on

Uising the Law of cosine • Assume 0, A 1, A 2 is on the same plane. • From law of cosine • • • Ref: http: //mathworld. wolfram. com/Lawof. Cosines. html |D|2=|A 1|2+|A 2|2 -2*|A 1|*|A 2|*cos If the length of A 1 is L 1, and Length of A 2 is L 2. And If u 1=unit vector of A 1, u 2=unit vector of A 2, cos =dot(u 1, u 2), hence |L 1| 2 2 2 D = L 1 + L 2 -2*L 1*L 2*dot(u 1, u 2) 0 Pose estimation using 4 points v. 6 a A 1 D |L 2| A 2 5

Error functions • Pose estimation using 4 points v. 6 a 6

Error functions • Pose estimation using 4 points v. 6 a 6

Exercise 1: Newton’s method (An itervative method ) • • • An iterative method

Exercise 1: Newton’s method (An itervative method ) • • • An iterative method for finding the • • solution of a non-linear system Exercise 1. Find sqrt(5), same as find • the non-linear function. • 2 – f(x)=x -5=0 • – Taylor series (by definition) • – f(x)=f(x 0)+f’(x 0)*(x-x 0)=0 • – f’(x 0)=2*x 0, so • – f(x)=f(x 0)+2*x 0*(x-x 0)=0 • • • First guess, x 0=2. f(x)=f(x 0)+ f’(x 0) *(x-x 0) 0 0 f(x 0) + f’(x 0) *(x-x 0) [0 -f(x 0)]/f’(x 0) (x-x 0) [0 -(x 02 -5)]/2*x 0 = x Take x 0=2, [0 -(22 -5)]/2*2 = x ¼= x Since x (x-x 0), x=new guess, x 0=old_guess ¼ x-2, x 2. 25 That means the next guess is x x 2. 25. Exercise: Complete the steps to find the solution. For your reference: sqrt(5)=2. 2360679 (by calculator) lhttp: //www. ugrad. math. ubc. ca/coursedoc/math 100/ notes/approx/newton. html Pose estimation using 4 points v. 6 a 7

Answer 1: Newton’s method An iterative method for finding the solution of a non-linear

Answer 1: Newton’s method An iterative method for finding the solution of a non-linear system • • Exercise 1. Find sqrt(5), same as find the non-linear function. • Guess, x 0=2. 25 sqrt(5)=2. 2360679 (by calculator) – f(x)=x 2 -5=0 • f(x)=f(x 0)+2*x 0*(x-x 0)=0 – Taylor series (by definition) • f(x)=(x 02 -5)+2*x 0*(x-x 0)=0 – f(x)=f(x 0)+f’(x 0)*(x-x 0)=0 • f(x)=(x 02 -5)+2*x 0*(x-x 0)=0 – f’(x 0)=2*x 0, so • (5. 06 -5)+2*2. 25*(x-2. 25)=0 – f(x)=f(x 0)+2*x 0*(x-x 0)=0 • 0. 06+4. 5*(x-2. 25)=0 • X=((4. 5*2. 25)-0. 06)/4. 5 • X=2. 2366666 (temporally solution, but is good enough. • ||Previous solution-current solution||2 =||2. 252. 2366666||2=0. 013333 (small enough), continue if needed. . . • Otherwise the solution is • sqrt(5)=2. 2366666. lhttp: //www. ugrad. math. ubc. ca/coursedoc/math 100/notes/approx/newton. html Pose estimation using 4 points v. 6 a 8

Using Taylor series http: //www. fepress. org/files/math_primer_fe_taylor. pdf Pose estimation using 4 points v.

Using Taylor series http: //www. fepress. org/files/math_primer_fe_taylor. pdf Pose estimation using 4 points v. 6 a 9

Netwon’s method: http: //en. wikipedia. org/wiki/Newton%27 s_method • Pose estimation using 4 points v.

Netwon’s method: http: //en. wikipedia. org/wiki/Newton%27 s_method • Pose estimation using 4 points v. 6 a 10

The main idea of Newton's method • • We saw this formula before: f(x)=f(x

The main idea of Newton's method • • We saw this formula before: f(x)=f(x 0)+f’(x 0)*(x-x 0) 0 -----(i) From f(x)=f(x 0)+f’(x 0)*(x-x 0) 0 0 f(x 0)+f’(x 0)*(x-x 0) 0 - f(x 0)= f’(x 0)*(x-x 0) [0 - f(x 0)]/ f’(x 0)= x=(x-x 0) We can compute x=[0 - f(x 0)]/ f’(x 0), then Since x=(x-x 0), so x=x 0+ x That means: Xnew_guess= x 0(old_guess) + x Pose estimation using 4 points v. 6 a 11

The top 6 error functions • Pose estimation using 4 points v. 6 a

The top 6 error functions • Pose estimation using 4 points v. 6 a 12

The extra error function • Pose estimation using 4 points v. 6 a 13

The extra error function • Pose estimation using 4 points v. 6 a 13

Stack up all error functions • Pose estimation using 4 points v. 6 a

Stack up all error functions • Pose estimation using 4 points v. 6 a 14

http: //en. wikibooks. org/wiki/Calculus/Taylor_series http: //en. wikipedia. org/wiki/Taylor_series Stack up all 7 error functions

http: //en. wikibooks. org/wiki/Calculus/Taylor_series http: //en. wikipedia. org/wiki/Taylor_series Stack up all 7 error functions • Pose estimation using 4 points v. 6 a 15

The iteration algorithm • Pose estimation using 4 points v. 6 a 16

The iteration algorithm • Pose estimation using 4 points v. 6 a 16

Summary • A 4 -point pose estimation algorithm is introduced. • The mathematical formulas

Summary • A 4 -point pose estimation algorithm is introduced. • The mathematical formulas of the algorithm are shown. • It is fast and accurate algorithm for pose estimation because no rotation matrix is used hence make it less complicated, stable and efficient. Pose estimation using 4 points v. 6 a 17

Discussion • The error function g (or e 4) corresponded to the square of

Discussion • The error function g (or e 4) corresponded to the square of the length of a vector. But the other errors e 1, . . , e 4 are the squares of the difference between two vectors. They don’t seem to match. We can try to make g g^2 and see if it works better or not. My guess is it is more or less the same because near convergence they all become very small. Pose estimation using 4 points v. 6 a 18

Further work • Extend to N-points – Randomly select 4 pints form the object

Further work • Extend to N-points – Randomly select 4 pints form the object and calculate the pose, all the poses should agree, use RANSAC to select the best solution. • Add Kalman filter for pose estimation to make it more robust. • Extend it to Structure from motion – Assume all planar structure first and gradually improve the structure results, use a 2 -pass: (i) Pose (ii) Structure, alternating algorithm, . Pose estimation using 4 points v. 6 a 19

Appendix: Modified algorithm Warning: This modified Implementation is not very stable and accurate. •

Appendix: Modified algorithm Warning: This modified Implementation is not very stable and accurate. • We prefer the Jacobean J to be a square matrix, so Inverse can be applied • Combine square-error function to make J a 4 x 4 matrix • e 1=e 12+e 23 • e 2=e 14+e 24 • e 3=e 34+e 13 • e 4=g Pose estimation using 4 points v. 6 a 20

The terms of the Jacobean • Pose estimation using 4 points v. 6 a

The terms of the Jacobean • Pose estimation using 4 points v. 6 a 21

Find g or e 4 (the choice of points is not the same as

Find g or e 4 (the choice of points is not the same as the paper) • Pose estimation using 4 points v. 6 a 22

 • • • • • • • • • %********** 27 oct 2016***

• • • • • • • • • %********** 27 oct 2016*** jacobian J (a 6 x 4 matrix ) %ref %Liu M. L. and Wong K. H. , "Pose Estimation Using Four Corresponding Points", %Pattern Recognition Letters, Volume 20, Number 1 January 1999, pp. 69 -74. %this program generates the 6 x 4 jacobian for the processing function find_jacob_four_point_algo_mlliu_sfm %try to use it for sfm rather than pose clc, clear; disp('test four_point_algo_mlliu. Test Jacobian, new test 10. 16'); %f-focal length, l 1, 2, 3, 4=4 length, ai, bi, vi is unit vertor of the i image pt %d_i, j= distance between two points in 3 D. %assume they are all real syms f l 1 l 2 l 3 l 4. . a 1 b 1 c 1. . . a 2 b 2 c 2. . . a 3 b 3 c 3. . . a 4 b 4 c 4. . . d 12 d 23 d 34 d 41 d 13 d 24 real dl = [l 1, l 2, l 3, l 4]'; %Vectors of all 6 combintions of 3 D-points A 1, A 2, A 3, A 4 %---------------------------- disp('jacob no square, shown to be correct by experiment 161028') e 12=(l 1^2+l 2^2 -2*l 1*l 2*dot([a 1 b 1 c 1]', [a 2 b 2 c 2]')-d 12); e 23=(l 2^2+l 3^2 -2*l 3*dot([a 2 b 2 c 2]', [a 3 b 3 c 3]')-d 23); e 34=(l 3^2+l 4^2 -2*l 3*l 4*dot([a 3 b 3 c 3]', [a 4 b 4 c 4]')-d 34); e 41=(l 4^2+l 1^2 -2*l 4*l 1*dot([a 4 b 4 c 4]', [a 1 b 1 c 1]')-d 41); e 13=(l 1^2+l 3^2 -2*l 1*l 3*dot([a 1 b 1 c 1]', [a 3 b 3 c 3]')-d 13); e 24=(l 2^2+l 4^2 -2*l 4*dot([a 2 b 2 c 2]', [a 4 b 4 c 4]')-d 24); E=[e 12 e 23 e 34 e 41 e 13 e 24 ]; J = jacobian(E, dl); disp('J =') disp(J) Pose estimation using 4 points v. 6 a size(J) 23

The Jacobian • • • J 1=[ 2*l 1 - 2*l 2*(a 1*a 2

The Jacobian • • • J 1=[ 2*l 1 - 2*l 2*(a 1*a 2 + b 1*b 2 + c 1*c 2), 2*l 2 - 2*l 1*(a 1*a 2 + b 1*b 2 + c 1*c 2), 0] J 2=[ 0, 2*l 2 - 2*l 3*(a 2*a 3 + b 2*b 3 + c 2*c 3), 2*l 3 - 2*l 2*(a 2*a 3 + b 2*b 3 + c 2*c 3), 0] J 3=[ 0, 2*l 3 - 2*l 4*(a 3*a 4 + b 3*b 4 + c 3*c 4), 2*l 4 - 2*l 3*(a 3*a 4 + b 3*b 4 + c 3*c 4)] J 4=[ 2*l 1 - 2*l 4*(a 1*a 4 + b 1*b 4 + c 1*c 4), 0, 2*l 4 - 2*l 1*(a 1*a 4 + b 1*b 4 + c 1*c 4)] J 5=[ 2*l 1 - 2*l 3*(a 1*a 3 + b 1*b 3 + c 1*c 3), 0, 2*l 3 - 2*l 1*(a 1*a 3 + b 1*b 3 + c 1*c 3), 0] J 6=[ 0, 2*l 2 - 2*l 4*(a 2*a 4 + b 2*b 4 + c 2*c 4), 0, 2*l 4 - 2*l 2*(a 2*a 4 + b 2*b 4 + c 2*c 4)] J=[J 1; J 2; J 3; J 4; J 5; J 6]; % Pose estimation using 4 points v. 6 a 24

 • • • • • • • • • • function four_pt %testing

• • • • • • • • • • function four_pt %testing code clc, clearvars; clear %assume they are all real ------------------------------------------f=1000 pwm=5*10^-6; %pixel_width_in_meters %assume the four points in pixles are ---------------------------------------A 1 x= 0. 1/pwm, A 1 y= -0. 1/pwm, A 1 z=1/pwm A 2 x= 0. 1/pwm, A 2 y= 0. 1/pwm, A 2 z=2/pwm A 3 x= -0. 1/pwm, A 3 y= 0. 1/pwm, A 3 z=3/pwm A 4 x= -0. 1/pwm, A 4 y= -0. 1/pwm, A 4 z=4/pwm v 12=([A 1 x A 1 y A 1 z]'-[A 2 x A 2 y A 2 z]') ; %may need to revseerse it? ? v 23=([A 2 x A 2 y A 2 z]'-[A 3 x A 3 y A 3 z]'); v 34=([A 3 x A 3 y A 3 z]'-[A 4 x A 4 y A 4 z]'); v 41=([A 4 x A 4 y A 4 z]'-[A 1 x A 1 y A 1 z]'); v 13=([A 1 x A 1 y A 1 z]'-[A 3 x A 3 y A 3 z]'); v 24=([A 2 x A 2 y A 2 z]'-[A 4 x A 4 y A 4 z]'); d 12=norm(v 12, 2); d 23=norm(v 23, 2); d 34=norm(v 34, 2); d 41=norm(v 41, 2 ); d 13=norm(v 13, 2); d 24=norm(v 24, 2); vv 1=[f*A 1 x/A 1 z, f*A 1 y/A 1 z, f]'; %vv 1 is teh 3 D vector of teh imaeg point v 1= vv 1. /norm(vv 1); a 1=v 1(1); b 1=v 1(2); c 1=v 1(3); vv 2=[f*A 2 x/A 2 z, f*A 2 y/A 2 z, f]'; %vv 1 is teh 3 D vector of teh imaeg point v 2= vv 2. /norm(vv 2); a 2=v 2(1); b 2=v 2(2); c 2=v 2(3); vv 3=[f*A 3 x/A 3 z, f*A 3 y/A 3 z, f]'; %vv 1 is teh 3 D vector of teh imaeg point v 3= vv 3. /norm(vv 3); a 3=v 3(1); b 3=v 3(2); c 3=v 3(3); vv 4=[f*A 4 x/A 4 z, f*A 4 y/A 4 z, f]'; %vv 1 is teh 3 D vector of teh imaeg point v 4= vv 4. /norm(vv 4); a 4=v 4(1); b 4=v 4(2); c 4=v 4(3); %%%now if you only have f, vv 1 vv 2 vv 3 vv 4, d 12, d 23. . %can you find l 1, l 2, l 3, l 4? , use %assume from vv 1, vv 2, vv 3, vv 4 you can find abc, 1, 2, 3, 4 %%%4 pt algo, . real_len=[norm([A 1 x A 1 y A 1 z]'), norm([A 2 x A 2 y A 2 z]'), norm([A 3 x A 3 y A 3 z]'), norm([A 4 x A 4 y A 4 z]')]' figure(1), clf %-------------------------------------------------plot 3(0, 0, 0) hold on plot 3([0 A 1 x], [0 A 1 y], [0 A 1 z], 'r-. '); plot 3([0 A 2 x], [0 A 2 y], [0 A 2 z], 'g-. '); plot 3([0 A 3 x], [0 A 3 y], [0 A 3 z], 'b-. '); plot 3([0 A 4 x], [0 A 4 y], [0 A 4 z], 'k-. '); plot 3(real_len(1)*a 1, real_len(1)*b 1, real_len(1)*c 1, 'ro'); plot 3(real_len(2)*a 2, real_len(2)*b 2, real_len(2)*c 2, 'go'); plot 3(real_len(3)*a 3, real_len(3)*b 3, real_len(3)*c 3, 'bo'); plot 3(real_len(4)*a 4, real_len(4)*b 4, real_len(4)*c 4, 'ko'); drawnow Pose estimation using 4 points v. 6 a 25

 • • • • • • • • • • %initi l 1,

• • • • • • • • • • %initi l 1, l 2, l 3, l 4 len=[2. 1 2. 1]'/pwm; max_iter=100000; quit_tag=0; threshold=0. 001 no_iter =0 while ( quit_tag==0 & (no_iter <= max_iter)), l 1=len(1); l 2=len(2); l 3=len(3); l 4=len(4); e 12=(l 1^2+l 2^2 -2*l 1*l 2*dot([a 1 b 1 c 1]', [a 2 b 2 c 2]')-d 12^2); e 23=(l 2^2+l 3^2 -2*l 3*dot([a 2 b 2 c 2]', [a 3 b 3 c 3]')-d 23^2); e 34=(l 3^2+l 4^2 -2*l 3*l 4*dot([a 3 b 3 c 3]', [a 4 b 4 c 4]')-d 34^2); e 41=(l 4^2+l 1^2 -2*l 4*l 1*dot([a 4 b 4 c 4]', [a 1 b 1 c 1]')-d 41^2); e 13=(l 1^2+l 3^2 -2*l 1*l 3*dot([a 1 b 1 c 1]', [a 3 b 3 c 3]')-d 13^2); e 24=(l 2^2+l 4^2 -2*l 4*dot([a 2 b 2 c 2]', [a 4 b 4 c 4]')-d 24^2); E=[e 12 e 23 e 34 e 41 e 13 e 24]'; %from find_jacob_four_point_16 b_ok %jacob no sqaure , correct and fast, no result with negative %sign J 1=[ 2*l 1 - 2*l 2*(a 1*a 2 + b 1*b 2 + c 1*c 2), 2*l 2 - 2*l 1*(a 1*a 2 + b 1*b 2 + c 1*c 2), 0] J 2=[ 0, 2*l 2 - 2*l 3*(a 2*a 3 + b 2*b 3 + c 2*c 3), 2*l 3 - 2*l 2*(a 2*a 3 + b 2*b 3 + c 2*c 3), 0] J 3=[ 0, 2*l 3 - 2*l 4*(a 3*a 4 + b 3*b 4 + c 3*c 4), 2*l 4 - 2*l 3*(a 3*a 4 + b 3*b 4 + c 3*c 4)] J 4=[ 2*l 1 - 2*l 4*(a 1*a 4 + b 1*b 4 + c 1*c 4), 0, 2*l 4 - 2*l 1*(a 1*a 4 + b 1*b 4 + c 1*c 4)] J 5=[ 2*l 1 - 2*l 3*(a 1*a 3 + b 1*b 3 + c 1*c 3), 0, 2*l 3 - 2*l 1*(a 1*a 3 + b 1*b 3 + c 1*c 3), 0] J 6=[ 0, 2*l 2 - 2*l 4*(a 2*a 4 + b 2*b 4 + c 2*c 4), 0, 2*l 4 - 2*l 2*(a 2*a 4 + b 2*b 4 + c 2*c 4)] J=[J 1; J 2; J 3; J 4; J 5; J 6]; %J(6 x 4) h=(pinv(J))*E; %h(4 x 1)=pinv(J)'(4 x 6)*E(6 x 1) h is 4 x 1, E is 6 x 4 len=len-h; err=sqrt(sum(h'*h)); if err<threshold quit_tag=1 end len, err, no_iter = no_iter+1 % pause end Pose estimation using 4 points v. 6 a 26 H, len, real_len