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.6a

Pose estimation using 4 points v.6a 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.6a

Pose estimation using 4 points v.6a Problem definition At time t, we have the model of the object A1,t, A2,t, A3,t, A4,t, f (focal length) And corresponding image points a1,t, a2,t, a3,t, a4,t We want to find the pose l1,t, l2,t, l3,t, l4,t From l1,t, l2,t, l3,t, l4,t we can find the rotation and translation of the object with respect to the camera coordinates Pose estimation using 4 points v.6a

Pose estimation using 4 points v.6a 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.6a

Uising the Law of cosine Assume 0,A1,A2 is on the same plane. From law of cosine Ref: http://mathworld.wolfram.com/LawofCosines.html |D|2=|A1|2+|A2|2-2*|A1|*|A2|*cos If the length of A1 is L1, and Length of A2 is L2. And If u1=unit vector of A1, u2=unit vector of A2, cos=dot(u1,u2), hence D2= L12+ L22-2*L1*L2*dot(u1,u2) A1 |L1| D  A2 |L2| Pose estimation using 4 points v.6a

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

Exercise 1: Newton’s method (An itervative method ) First guess, x0=2. f(x)=f(x0)+ f’(x0) *(x-x0)0 0  f(x0) + f’(x0) *(x-x0) [0-f(x0)]/f’(x0)  (x-x0) [0-(x02-5)]/2*x0 = x (x-x0) [0-(x02-5)]/2*x0 = x Take x0=2, [0-(22-5)]/2*2 = x ¼= x Since x (x-x0), x=new guess, x0=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) An iterative method for finding the solution of a non-linear system Exercise 1.Find sqrt(5), same as find the non-linear function. f(x)=x2-5=0 Taylor series (by definition) f(x)=f(x0)+f’(x0)*(x-x0)=0 f’(x0)=2*x0, so f(x)=f(x0)+2*x0*(x-x0)=0 http://www.ugrad.math.ubc.ca/coursedoc/math100/notes/approx/newton.html Pose estimation using 4 points v.6a

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. sqrt(5)=2.2360679 (by calculator) f(x)=x2-5=0 Taylor series (by definition) f(x)=f(x0)+f’(x0)*(x-x0)=0 f’(x0)=2*x0, so f(x)=f(x0)+2*x0*(x-x0)=0 Guess, x0=2.25 f(x)=f(x0)+2*x0*(x-x0)=0 f(x)=(x02-5)+2*x0*(x-x0)=0 (5.06-5)+2*2.25*(x-2.25)=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.25-2.2366666||2=0.013333 (small enough), continue if needed... Otherwise the solution is sqrt(5)=2.2366666. http://www.ugrad.math.ubc.ca/coursedoc/math100/notes/approx/newton.html Pose estimation using 4 points v.6a

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

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

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

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

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

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

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.6a

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

Pose estimation using 4 points v.6a 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.6a

Pose estimation using 4 points v.6a Discussion The error function g (or e4) corresponded to the square of the length of a vector. But the other errors e1,..,e4 are the squares of 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.6a

Pose estimation using 4 points v.6a 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.6a

Pose estimation using 4 points v.6a 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 4x4 matrix e1=e12+e23 e2=e14+e24 e3=e34+e13 e4=g Pose estimation using 4 points v.6a

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

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

Pose estimation using 4 points v.6a %******************** 27 oct 2016*** jacobian J (a 6x4 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 6x4 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_mlliuTest Jacobian, new test10.16'); %f-focal length,l1,2,3,4=4length,ai,bi,vi is unit vertor of the i image pt %d_i,j= distance between two points in 3D. %assume they are all real syms f l1 l2 l3 l4 .... a1 b1 c1... a2 b2 c2... a3 b3 c3... a4 b4 c4... d12 d23 d34 d41 d13 d24 real dl = [l1,l2,l3,l4]'; %Vectors of all 6 combintions of 3D-points A1,A2,A3,A4 %-------------------------------------------------------- disp('jacob no square, no square, shown to be correct by experiment 161028') e12=(l1^2+l2^2-2*l1*l2*dot([a1 b1 c1]',[a2 b2 c2]')-d12); e23=(l2^2+l3^2-2*l2*l3*dot([a2 b2 c2]',[a3 b3 c3]')-d23); e34=(l3^2+l4^2-2*l3*l4*dot([a3 b3 c3]',[a4 b4 c4]')-d34); e41=(l4^2+l1^2-2*l4*l1*dot([a4 b4 c4]',[a1 b1 c1]')-d41); e13=(l1^2+l3^2-2*l1*l3*dot([a1 b1 c1]',[a3 b3 c3]')-d13); e24=(l2^2+l4^2-2*l2*l4*dot([a2 b2 c2]',[a4 b4 c4]')-d24); E=[e12 e23 e34 e41 e13 e24 ]; J = jacobian(E,dl); disp('J =') disp(J) size(J) Pose estimation using 4 points v.6a

Pose estimation using 4 points v.6a The Jacobian J1=[ 2*l1 - 2*l2*(a1*a2 + b1*b2 + c1*c2), 2*l2 - 2*l1*(a1*a2 + b1*b2 + c1*c2), 0, 0] J2=[ 0, 2*l2 - 2*l3*(a2*a3 + b2*b3 + c2*c3), 2*l3 - 2*l2*(a2*a3 + b2*b3 + c2*c3), 0] J3=[ 0, 0, 2*l3 - 2*l4*(a3*a4 + b3*b4 + c3*c4), 2*l4 - 2*l3*(a3*a4 + b3*b4 + c3*c4)] J4=[ 2*l1 - 2*l4*(a1*a4 + b1*b4 + c1*c4), 0, 0, 2*l4 - 2*l1*(a1*a4 + b1*b4 + c1*c4)] J5=[ 2*l1 - 2*l3*(a1*a3 + b1*b3 + c1*c3), 0, 2*l3 - 2*l1*(a1*a3 + b1*b3 + c1*c3), 0] J6=[ 0, 2*l2 - 2*l4*(a2*a4 + b2*b4 + c2*c4), 0, 2*l4 - 2*l2*(a2*a4 + b2*b4 + c2*c4)] J=[J1;J2;J3;J4;J5;J6]; % Pose estimation using 4 points v.6a

Pose estimation using 4 points v.6a 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 ------------------------------------------------------------------------------ A1x= 0.1/pwm, A1y= -0.1/pwm, A1z=1/pwm A2x= 0.1/pwm, A2y= 0.1/pwm, A2z=2/pwm A3x= -0.1/pwm, A3y= 0.1/pwm, A3z=3/pwm A4x= -0.1/pwm, A4y= -0.1/pwm, A4z=4/pwm v12=([A1x A1y A1z]'-[A2x A2y A2z]') ;%may need to revseerse it?? v23=([A2x A2y A2z]'-[A3x A3y A3z]');v34=([A3x A3y A3z]'-[A4x A4y A4z]');v41=([A4x A4y A4z]'-[A1x A1y A1z]');v13=([A1x A1y A1z]'-[A3x A3y A3z]');v24=([A2x A2y A2z]'-[A4x A4y A4z]'); d12=norm(v12,2);d23=norm(v23,2);d34=norm(v34,2);d41=norm(v41,2);d13=norm(v13,2);d24=norm(v24,2); vv1=[f*A1x/A1z, f*A1y/A1z,f]'; %vv1 is teh 3D vector of teh imaeg point v1= vv1./norm(vv1); a1=v1(1);b1=v1(2);c1=v1(3); vv2=[f*A2x/A2z, f*A2y/A2z,f]'; %vv1 is teh 3D vector of teh imaeg point v2= vv2./norm(vv2);a2=v2(1);b2=v2(2);c2=v2(3); vv3=[f*A3x/A3z, f*A3y/A3z,f]'; %vv1 is teh 3D vector of teh imaeg point v3= vv3./norm(vv3); a3=v3(1);b3=v3(2);c3=v3(3); vv4=[f*A4x/A4z, f*A4y/A4z,f]'; %vv1 is teh 3D vector of teh imaeg point v4= vv4./norm(vv4); a4=v4(1);b4=v4(2);c4=v4(3); %%%now if you only have f, vv1 vv2 vv3 vv4, d12,d23.... %can you find l1, l2, l3, l4?, use %assume from vv1,vv2,vv3,vv4 you can find abc,1,2,3,4 %%%4 pt algo,. real_len=[norm([A1x A1y A1z]'),norm([A2x A2y A2z]'),norm([A3x A3y A3z]'),norm([A4x A4y A4z]')]' figure(1), clf %--------------------------------------------------------------------------------------------------- plot3(0,0,0) hold on  plot3([0 A1x],[0 A1y],[0 A1z],'r-.'); plot3([0 A2x],[0 A2y],[0 A2z],'g-.'); plot3([0 A3x],[0 A3y],[0 A3z],'b-.'); plot3([0 A4x],[0 A4y],[0 A4z],'k-.'); plot3(real_len(1)*a1,real_len(1)*b1,real_len(1)*c1,'ro'); plot3(real_len(2)*a2,real_len(2)*b2,real_len(2)*c2,'go'); plot3(real_len(3)*a3,real_len(3)*b3,real_len(3)*c3,'bo');plot3(real_len(4)*a4,real_len(4)*b4,real_len(4)*c4,'ko'); drawnow Pose estimation using 4 points v.6a

Pose estimation using 4 points v.6a %initi l1,l2,l3,l4 len=[2.1 2.1 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)), l1=len(1); l2=len(2); l3=len(3); l4=len(4); e12=(l1^2+l2^2-2*l1*l2*dot([a1 b1 c1]',[a2 b2 c2]')-d12^2); e23=(l2^2+l3^2-2*l2*l3*dot([a2 b2 c2]',[a3 b3 c3]')-d23^2); e34=(l3^2+l4^2-2*l3*l4*dot([a3 b3 c3]',[a4 b4 c4]')-d34^2); e41=(l4^2+l1^2-2*l4*l1*dot([a4 b4 c4]',[a1 b1 c1]')-d41^2); e13=(l1^2+l3^2-2*l1*l3*dot([a1 b1 c1]',[a3 b3 c3]')-d13^2); e24=(l2^2+l4^2-2*l2*l4*dot([a2 b2 c2]',[a4 b4 c4]')-d24^2); E=[e12 e23 e34 e41 e13 e24]'; %from find_jacob_four_point_16b_ok %jacob no sqaure , correct and fast, no result with negative %sign J1=[ 2*l1 - 2*l2*(a1*a2 + b1*b2 + c1*c2), 2*l2 - 2*l1*(a1*a2 + b1*b2 + c1*c2), 0, 0] J2=[ 0, 2*l2 - 2*l3*(a2*a3 + b2*b3 + c2*c3), 2*l3 - 2*l2*(a2*a3 + b2*b3 + c2*c3), 0] J3=[ 0, 0, 2*l3 - 2*l4*(a3*a4 + b3*b4 + c3*c4), 2*l4 - 2*l3*(a3*a4 + b3*b4 + c3*c4)] J4=[ 2*l1 - 2*l4*(a1*a4 + b1*b4 + c1*c4), 0, 0, 2*l4 - 2*l1*(a1*a4 + b1*b4 + c1*c4)] J5=[ 2*l1 - 2*l3*(a1*a3 + b1*b3 + c1*c3), 0, 2*l3 - 2*l1*(a1*a3 + b1*b3 + c1*c3), 0] J6=[ 0, 2*l2 - 2*l4*(a2*a4 + b2*b4 + c2*c4), 0, 2*l4 - 2*l2*(a2*a4 + b2*b4 + c2*c4)] J=[J1;J2;J3;J4;J5;J6];%J(6x4) h=(pinv(J))*E; %h(4x1)=pinv(J)'(4x6)*E(6x1) h is 4x1, E is 6x4 len=len-h; err=sqrt(sum(h'*h)); if err<threshold quit_tag=1 end len, err, no_iter = no_iter+1 % pause H,len,real_len   Pose estimation using 4 points v.6a