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 KH Wong Pose estimation using 4 points v.4g21

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.4g22

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.4g23

Formulation P is the center of perceptivity (camera center) The vector PA n,t passes through (unit vector u n,t ) passes through points a n,t and A n,t The length is l n,t Equation (1) and (2) Pose estimation using 4 points v.4g24

Error functions Pose estimation using 4 points v.4g2 5

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. – f(x)=x 2 -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 )]/2*x 0 =  x  (x-x 0 ) [0-(x )]/2*x 0 =  x Take x 0 =2, [0-(2 2 -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  Exercise: Complete the steps to find the solution. For your reference: sqrt(5)= (by calculator) Pose estimation using 4 points v.4g2 6 otes/approx/newton.html otes/approx/newton.html

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)= (by calculator) – f(x)=x 2 -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 Pose estimation using 4 points v.4g27 Guess, x0=2.25 f(x)=f(x0)+2*x0*(x-x0)=0 f(x)=(x0 2 -5)+2*x0*(x-x0)=0 (5.06-5)+2*2.25*(x-2.25)= *(x-2.25)=0 X=((4.5*2.25)-0.06)/4.5 X= (temporally solution, but is good enough. ||Previous solution-current solution||2 =|| ||2= (small enough), continue if needed... Otherwise the solution is sqrt(5)=

Pose estimation using 4 points v.4g2 8 Using Taylor series

Netwon’s method: Pose estimation using 4 points v.4g29

The main idea of Newton's method We saw this formula before: f(x)=f(x 0 )+f’(x 0 )*(x-x 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: X new_guess = x 0(old_guess) +  x Pose estimation using 4 points v.4g2 10

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

The extra error function Pose estimation using 4 points v.4g212

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

Stack up all 7 error functions Pose estimation using 4 points v.4g

The iteration algorithm Pose estimation using 4 points v.4g215

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.4g216

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.4g217

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.4g218

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.4g219

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

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

%*feb 2014*** jacob (a 4x4 matrix )for %Liu M.L.and Wong K.H., "Pose Estimation %Using Four Corresponding Points", %Pattern Recognition Letters, Volume 20, % Number 1 January 1999, pp function four_point_algo_mlliu_khwong clc,clear; disp('test four_point_algo_mlliuTest Jacobian'); %assume they are all real syms f l1 l2 l3 l4 cos_12 cos_13 cos_14 … cos_23 cos_24 cos_34...\ D12 D13 D14 D23 D24 D34...\ te11 te12 te13 te21 te22 te23 real %Vectors of all 6 combinations of 3D-points A1,A2,A3,A4 %cos_12=cos(angle between unit vectors of PA1 and PA2),and(P=camera center) dl = [l1,l2,l3,l4]'; %---error functions of the 6 combinations, %see equation 3 of liu+wong paper e12=(l1^2+l2^2-2*l1*l2*cos_12-D12)^2; e13=(l1^2+l3^2-2*l1*l3*cos_13-D13)^2; e14=(l1^2+l4^2-2*l1*l4*cos_14-D14)^2; e23=(l2^2+l3^2-2*l2*l3*cos_23-D23)^2; e24=(l2^2+l4^2-2*l2*l4*cos_24-D24)^2; e34=(l3^2+l4^2-2*l3*l4*cos_34-D34)^2; e1=e12+e23 e2=e14+e24 e3=e34+e13 % for g % g=l1*l2*l4*te11-l3*l1*l4*te12- l2*l3*l4*te13...\ -l2*l1*l3*te21-l3*l1*l3*te22- l2*l3*l3*te23; e4=g; % E=[e1 e2 e3 e4 ]; J = jacobian(E,dl); disp('J ='), disp(J), size(J), Pose estimation using 4 points v.4g222