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

2. 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

3. 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

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

5. Uising the Law of cosine
Assume 0,A1,A2 is on the same plane.
From law of cosine
Ref:
|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

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

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

8. 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)=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
*(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.6a

9. Pose estimation using 4 points v.6a
Using Taylor series
Pose estimation using 4 points v.6a

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

11. The main idea of Newton's method
We saw this formula before: f(x)=f(x0)+f’(x0)*(x-x0) (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

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

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

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

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

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

17. 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

18. 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

19. 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

20. 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

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

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

23. 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
%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 ')
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

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

25. 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

26. Pose estimation using 4 points v.6a
%initi l1,l2,l3,l4
len=[ ]'/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), , ]
J2=[ , 2*l2 - 2*l3*(a2*a3 + b2*b3 + c2*c3), 2*l3 - 2*l2*(a2*a3 + b2*b3 + c2*c3), ]
J3=[ , , 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), , , 2*l4 - 2*l1*(a1*a4 + b1*b4 + c1*c4)]
J5=[ 2*l1 - 2*l3*(a1*a3 + b1*b3 + c1*c3), , 2*l3 - 2*l1*(a1*a3 + b1*b3 + c1*c3), ]
J6=[ , 2*l2 - 2*l4*(a2*a4 + b2*b4 + c2*c4), , 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