%clear memory
clc;
clear;
close all;

%input the coordinates of world points xyz
xyz=[0 0 0;...
     6 1 0;...
     6 8 0;...
     4 5 0;...
     0 8 -2;...
     0 7 -6;...
     0 4 -2;...
     0 0 -6;...
     4 2 0;...
     0 6 -3];
xyz(:,1)=xyz(:,1)*9/12;
xyz(:,2)=xyz(:,2)*7/8;
xyz(:,3)=xyz(:,3)*9/12;


xy=[545 981;...
    736 841;...
    745 423;...
    681 625;...
    453 455;...
    274 488;...
    451 713;...
    281 915;...
    679 807;...
    406 577];

xy2=[904 892;...
    1135 790;...
    1154 398;...
    1073 580;...
    848 407;...
    721 430;...
    841 641;...
    716 816;...
    1065 750;...
    812 515];


num=size(xy,1);
im1=ReadBinPGM('Capture_00003.pgm');
im2=ReadBinPGM('Capture_00004.pgm');
[row col]=size(im1);

show(im1);
hold on;
plot(xy(:,1),xy(:,2),'+r');
show(im2);
hold on;
plot(xy2(:,1),xy2(:,2),'+r');

%input the coordinates of image xim, yim
ox=col/2;
oy=row/2;
x=(xy(:,1)-ox)/ox;
y=(xy(:,2)-oy)/ox;

%computer Av=0
A=[];
for i=1:num
    A=[A;x(i)*xyz(i,1) x(i)*xyz(i,2) x(i)*xyz(i,3) x(i) -y(i)*xyz(i,1) -y(i)*xyz(i,2) -y(i)*xyz(i,3) -y(i)];
end

[U, S, V]=svd(A);
v=V(:,8);
% A*v;

%compute |r| and a
gamma=sqrt(v(1)^2+v(2)^2+v(3)^2);
alpha=sqrt(v(5)^2+v(6)^2+v(7)^2)/gamma;

R=[v(5)/alpha v(6)/alpha v(7)/alpha; v(1) v(2) v(3); 0 0 0]/gamma;
T=[v(8)/alpha; v(4); 0]/gamma;

%determine the sign
for n=1:num
    if (x(n)*(R(1,1)*xyz(i,1)+R(1,2)*xyz(i,2)+R(1,3)*xyz(i,3)+T(1)))>0
        R=-R;
        T=-T;
        break;
    end
end

R(3,:)=cross(R(1,:),R(2,:));

%computer Tz, and fx
A=[];
b=[];
for i=1:num
	A=[A; x(i) (R(1,1)*xyz(i,1)+R(1,2)*xyz(i,2)+R(1,3)*xyz(i,3)+T(1))];
	b=[b; -x(i)*(R(3,1)*xyz(i,1)+R(3,2)*xyz(i,2)+R(3,3)*xyz(i,3))];
end
a=inv(A'*A)*A'*b;
T(3,1)=a(1,1);
fx=a(2,1);
%print parameters 
disp('camera parameters:');
alpha
fx
fy=fx/alpha
Rc=R
Tc=T

figure;
hold on;
grid on;
axis equal;
xlabel('x');
ylabel('y');
zlabel('z');
plot3(xyz(:,1),xyz(:,2),xyz(:,3),'.');

Oc=-inv(R)*T;
disp('camera position:');
disp(Oc);
plot3(Oc(1),Oc(2),Oc(3),'.r');
ax=eye(3)*5;
for j=1:3
    ax(:,j)=ax(:,j)-T;
end
ax=inv(R)*ax;
plot3([Oc(1) ax(1,1)],[Oc(2) ax(2,1)],[Oc(3) ax(3,1)],'r');
plot3([Oc(1) ax(1,2)],[Oc(2) ax(2,2)],[Oc(3) ax(3,2)],'g');
plot3([Oc(1) ax(1,3)],[Oc(2) ax(2,3)],[Oc(3) ax(3,3)],'b');

view(-30,20);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%pose estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xyz1=xyz';
for i=1:num
    xyz1(:,i)=R*xyz1(:,i)+T;
end

x2=(xy2(:,1)-ox)/ox;
y2=(xy2(:,2)-oy)/ox;

x3=x;
y3=y;
D=zeros(6,1);
for itr=1:1000
    E=[];
    A=[];
    
    Rx=[1 0 0; 0 cos(D(4)) -sin(D(4)); 0 sin(D(4)) cos(D(4))];
    Ry=[cos(D(5)) 0 sin(D(5)); 0 1 0; -sin(D(5)) 0 cos(D(5))];
    Rz=[cos(D(6)) -sin(D(6)) 0; sin(D(6)) cos(D(6)) 0; 0 0 1];
    
    R=Rz*Ry*Rx;
    xyz3=R*xyz1;
    for i=1:num
        x3(i)=-fx*xyz3(1,i)/(xyz3(3,i)+D(3))+D(1);
        y3(i)=-fy*xyz3(2,i)/(xyz3(3,i)+D(3))+D(2);
    end
    
	dx=x2-x3;
	dy=y2-y3;

    for i=1:num
        E=[E; dx(i); dy(i)];
        c=1/(xyz3(3,i)+D(3));
        A=[A; 
            1 ... 
            0 ... 
            fx*c^2*xyz3(1,i) ...
            fx*c^2*xyz3(1,i)*xyz3(2,i) ...
            -fx*c*(xyz3(3,i)+c*xyz3(1,i)^2) ...
            fx*c*xyz3(2,i);...
            0 ...
            1 ...
            fy*c^2*xyz3(2,i) ...
            fy*c*(xyz3(3,i)+c*xyz3(2,i)^2) ...
            -fy*c^2*xyz3(1,i)*xyz3(2,i) ...
            -fy*c*xyz3(1,i)];
    end
    
    dD=inv(A'*A)*A'*E;
    D=D+dD;
    itr=itr+1;
    if(norm(dD)<1.0e-010)
        break;
    end
end
D
dD
disp('iteration number:');
disp(itr);

for i=1:num
    xyz3(3,i)=xyz3(3,i)+D(3);
    xyz3(1,i)=xyz3(1,i)-D(1)*xyz3(3,i)/fx;
    xyz3(2,i)=xyz3(2,i)-D(2)*xyz3(3,i)/fy;
    xyz3(:,i)=xyz3(:,i)-Tc;
end
xyz3=inv(Rc)*xyz3;
plot3(xyz3(1,:),xyz3(2,:),xyz3(3,:),'.g');

disp('old position of the origin');
disp(xyz(1,:));
disp('new position of the origin');
disp(xyz3(:,1)');