%For each camera
%v1_images=v1_images(1:2,:)
%v2_images=v2_images(1:2,:)
figure(3);
clf;

subplot(2,2,1);
axis('ij');
hold on;
for i=1:size(v1_images,2),
    hh=plot(v1_images(1,i),v1_images(2,i),'r+');
    set(hh,'MarkerSize',[12]);
    text(v1_images(1,i),v1_images(2,i),[sprintf('%d',i)],'FontSize',[20]);
end
title('Left Image Points');

subplot(2,2,2);
axis('ij');
hold on;
for i=1:size(v2_images,2),
    hh=plot(v2_images(1,i),v2_images(2,i),'r+');
    set(hh,'MarkerSize',[12]);
    text(v2_images(1,i),v2_images(2,i),[sprintf('%d',i)],'FontSize',[20]);
end
title('Right Image Points');
%8-point algorithm
keyboard;
% First we need to normalize the inputs
mean1 = mean(v1_images');
mean2 = mean(v2_images');
T1_trans=[1 0 -mean1(1);0 1 -mean1(2); 0 0 1];
T2_trans=[1 0 -mean2(1);0 1 -mean2(2); 0 0 1];
v1_centered=T1_trans*v1_images;
v1_rms=sqrt(sum(sum(v1_centered(1:2,:).^2))/size(v1_images,2));
T1_scale=[sqrt(2)/v1_rms 0 0; 0 sqrt(2)/v1_rms 0; 0 0 1];
v1_norm=T1_scale*v1_centered;
T1_norm=T1_scale*T1_trans;

v2_centered=T2_trans*v2_images;
v2_rms=sqrt(sum(sum(v2_centered(1:2,:).^2))/size(v2_images,2));
T2_scale=[sqrt(2)/v2_rms 0 0; 0 sqrt(2)/v2_rms 0; 0 0 1];
v2_norm=T2_scale*v2_centered;
T2_norm=T2_scale*T2_trans;

%We need to construct the A matrix
A=zeros([size(v1_norm,2) 9]);
for i=1:size(v1_norm,2),
    A(i,:)=[v1_norm(:,i)'*v2_norm(1,i) v1_norm(:,i)'*v2_norm(2,i) v1_norm(:,i)'];
end
[UA,SA,VA]=svd(A);
f_norm=VA(:,end);
F_norm=reshape(f_norm,[3 3])';
[UF,SF,VF]=svd(F_norm);
fprintf(1,'Changing %15.14f to zero to enforce the rank of F to be 2.\n', SF(3,3));
SF(3,3)=0.0; %We need to enforce the rank of F to be 2
F_norm=UF*SF*VF';
%We need to denormalize to get fundamental matrix F
F=T2_norm'*F_norm*T1_norm;
%We do the testing by ploting out the epipolar lines
%Epipolar line in the right image
x_rang=[10 1500];
y_rang=[10 1500];
for i=1:size(v1_images,2),
    line_c=F*v1_images(:,i);
    x_rang_y=(-line_c(3)-line_c(1)*x_rang)./line_c(2);
    x_rang_flag=(x_rang_y>=y_rang(1)) & (x_rang_y <=y_rang(2));
    y_rang_x=(-line_c(3)-line_c(2)*y_rang)./line_c(1);
    y_rang_flag=(y_rang_x>=x_rang(1)) & (y_rang_x <= x_rang(2));
    ind=find(x_rang_flag);
    x_points=[x_rang(ind)];
    y_points=[x_rang_y(ind)];
    ind=find(y_rang_flag);
    x_points=[x_points y_rang_x(ind)];
    y_points=[y_points y_rang(ind)];
    subplot(2,2,2);
    hold on;
    hh=plot(x_points,y_points,'g--');
    set(hh,'LineWidth',[2]);
%     hh=plot(x_rang,x_rang_y,'r--');set(hh,'LineWidth',[2]);
%     hh=plot(y_rang_x,y_rang,'r--');set(hh,'LineWidth',[2]);
end


x_rang=[0 1500];
y_rang=[0 1500];
for i=1:size(v2_images,2),
    line_c=F'*v2_images(:,i);
    x_rang_y=(-line_c(3)-line_c(1)*x_rang)./line_c(2);
    x_rang_flag=(x_rang_y>=y_rang(1)) & (x_rang_y <=y_rang(2));
    y_rang_x=(-line_c(3)-line_c(2)*y_rang)./line_c(1);
    y_rang_flag=(y_rang_x>=x_rang(1)) & (y_rang_x <= x_rang(2));
    ind=find(x_rang_flag);
    x_points=[x_rang(ind)];
    y_points=[x_rang_y(ind)];
    ind=find(y_rang_flag);
    x_points=[x_points y_rang_x(ind)];
    y_points=[y_points y_rang(ind)];
    subplot(2,2,1);
    hold on;
    hh=plot(x_points,y_points,'g--');
    set(hh,'LineWidth',[2]);
     %hh=plot(x_rang,x_rang_y,'r--');set(hh,'LineWidth',[2]);
     %hh=plot(y_rang_x,y_rang,'r--');set(hh,'LineWidth',[2]);
end

%We need to get the essential matrix
E=K2_cam'*F*K1_cam;
W=[0 -1 0;1 0 0; 0 0 1];
[UE,SE,VE]=svd(E);
if det(UE) < 0,
    UE(:,3)=-UE(:,3);
end

if det(VE) < 0,
    VE(:,3)=-VE(:,3);
end

RE_1=UE*W*VE';
RE_2=UE*W'*VE';
T_tran1=UE(:,3);
T_tran2=0-UE(:,3);
R_true=R2'*R1;
T_true=R2'*(t1-t2);
T_true_unit=T_true/norm(T_true);

v1_norm=inv(K1_cam)*v1_images;
v2_norm=inv(K2_cam)*v2_images;
z_pos_max=0;
for i=1:2,
    if i==1,
        Rcan=RE_1;
    else
        Rcan=RE_2;
    end
    for j=1:2,
        if j==1,
            Tcan=T_tran1;
        else
            Tcan=T_tran2;
        end
        XYZs_1=v1_images*0;
        XYZs_2=XYZs_1;
        for kk=1:size(v1_images,2),
            AP=[-1 0 v1_norm(1,kk) 0
                0  -1 v1_norm(2,kk) 0
                -Rcan(1,1)+v2_norm(1,kk)*Rcan(3,1) -Rcan(1,2)+v2_norm(1,kk)*Rcan(3,2) -Rcan(1,3)+v2_norm(1,kk)*Rcan(3,3)  -Tcan(1)+v2_norm(1,kk)*Tcan(3)
                -Rcan(2,1)+v2_norm(2,kk)*Rcan(3,1) -Rcan(2,2)+v2_norm(2,kk)*Rcan(3,2) -Rcan(2,3)+v2_norm(2,kk)*Rcan(3,3)  -Tcan(2)+v2_norm(2,kk)*Tcan(3)];
            [UAP, SAP, VAP]=svd(AP);
            XYZ_1=VAP(1:3,end)./VAP(4,end);
            XYZs_1(:,kk)=XYZ_1;
            XYZ_2=Rcan*XYZ_1+Tcan;
            XYZs_2(:,kk)=XYZ_2;
        end
        zpos=sum(XYZs_1(3,:)>0)+sum(XYZs_2(3,:)>0);
        if zpos > z_pos_max;
            RE=Rcan;
            TE=Tcan;
            z_pos_max=zpos;
            XYZs_1E=XYZs_1;
            XYZs_2E=XYZs_2;
        end
        fprintf(1,'For i=%d and j=%d\n', i, j);
        XYZs_1
        XYZs_2
    end
end

fprintf(1,'The estimated R and T are the follow, which gives %d positive depths.\n', z_pos_max);
RE
TE
subplot(2,2,3);
plot_mesh_edges(XYZs_1E,f);
axis('equal');
title('Reconstructed model in the left camera');

subplot(2,2,4);
plot_mesh_edges(XYZs_2E,f);
axis('equal');

title('Reconstructed model in the right camera');
%Assume we know the true scale;
True_scale=norm(T_true);
TE=TE/norm(TE);
XYZs_1=zeros([4 size(v1_images,2)]);
XYZs_2=XYZs_1;
for kk=1:size(v1_images,2),
    AP=[-1 0 v1_norm(1,kk) 0
        0  -1 v1_norm(2,kk) 0
        -RE(1,1)+v2_norm(1,kk)*RE(3,1) -RE(1,2)+v2_norm(1,kk)*RE(3,2) -RE(1,3)+v2_norm(1,kk)*RE(3,3)  -TE(1)+v2_norm(1,kk)*TE(3)
        -RE(2,1)+v2_norm(2,kk)*RE(3,1) -RE(2,2)+v2_norm(2,kk)*RE(3,2) -RE(2,3)+v2_norm(2,kk)*RE(3,3)  -TE(2)+v2_norm(2,kk)*TE(3)];
     [UAP, SAP, VAP]=svd(AP);
     
    XYZ_1=VAP(:,end)/VAP(4,end);
    XYZs_1(:,kk)=XYZ_1;
    XYZ_2=[RE, TE;0 0 0 1]*XYZ_1;
    XYZs_2(:,kk)=XYZ_2;
end

%To recover the points in the original world coordinate 
XYZs_w1=[R1,t1;0 0 0 1]*XYZs_1;
XYZs_w2=[R2,t2;0 0 0 1]*XYZs_2;
x_rect_ic=cell([1 size(f,2)]);
for i=1:size(f,2),
   x_rect_ic{i}=[0 1 1; 0 0 1];
end
plane_names=cell([1 size(f,2)]);
im_names=['IS3045large.jpg'
   'IS3250large.jpg'  
   'IS3430large.jpg'
   'IS3460large.jpg'
   'IS3151large.jpg'
   'IS3350large.jpg'
   'IS3450large.jpg'
   'IS3550large.jpg'
   'MChieflarge.JPG'
   'footbalarge.jpg'
   'rider-large.jpg'
   'wescotlarge.jpg'
   'Reneg-large.JPG'];
for i=1:size(f,2),
   plane_names{i}=im_names(mod(i-1,size(im_names,1))+1,:);
end

make_vrml_model('reconstructed_left.wrl', XYZs_1E,x_rect_ic,f,plane_names);