
figure(2);
for i=1:size(cameras,1),
   cameras{i}.image=Project_Record_Frame(projectors{1},cameras{i},objects);
   subplot(3,3,i);
   image(cameras{i}.image/255);
   axis('image');
end

for i=1:size(cameras,1),
   imwrite(cameras{i}.image,[sprintf('p1_pat4_cam_%d.jpg',i)],'jpg');
end

im1=[cameras{1}.image(120:900,120:900,:),cameras{2}.image(120:900,120:900,:),cameras{3}.image(120:900,120:900,:);
   cameras{4}.image(120:900,120:900,:),cameras{5}.image(120:900,120:900,:),cameras{6}.image(120:900,120:900,:)];
image(im1/255);
imwrite(im1/255,[sprintf('p1_pat4_cam_all.jpg')],'jpg');

addpath '\cygwin\home\Xiuwen Liu\datasets\corpus-callosum\tools'
%The plane z=0 of the first projector
for z=-1:0.1:-0.1,
pl=[1 0 0]*[1 0 0 0;0 1 0 0;0 0 1 0]*inv([projectors{1}.Rw,projectors{1}.Tw;0 0 0 1])
slice_z=slice_surfaces(v',f',pl);
fprintf('z=%8.6f and curves=%d %d\n',z,size(slice_z{1}));
end

locations=slice_z{1}';
locations=locations(:,[23 1:10]);
figure(3);
clf;
vf0=4;
%We plot the rays and see what happen
im1=cameras{1}.image;
Pcam=[cameras{1}.Rw,cameras{1}.Tw];
cambounds1=[1 size(im1,2)/2 size(im1,2)  1 size(im1,2)/2 size(im1,2)
   1 1             1            size(im1,1) size(im1,1) size(im1,1)
   1 1             1           1            1            1];
cambounds1_n=inv(cameras{1}.K)*cambounds1;
cambounds1_n=cambounds1_n*vf0;

cambounds1_n=[Pcam; 0 0 0 1]*[cambounds1_n;ones([1 size(cambounds1_n,2)])];
for i=1:size(cambounds1_n,2)
   cambounds1_n(:,i)=cambounds1_n(:,i)/cambounds1_n(end,i);
end
hold on;
im_frame=cambounds1_n(:,[1 3 6 4 1]);
plot3(im_frame(1,:),im_frame(2,:),im_frame(3,:),'k-');

cambounds1_n=cambounds1_n(1:3,:);
im2_x=reshape(cambounds1_n(1,:),[3 2])';
im2_y=reshape(cambounds1_n(2,:),[3 2])';
im2_z=reshape(cambounds1_n(3,:),[3 2])';
ray_len=5;
hh2=warp(im2_x,im2_y,im2_z,im1);
hold on;
for i=1:size(locations,2),
   %xx_dir=locations(:,i)-Pcam(:,4);
   %xx_dir=xx_dir/norm(xx_dir);
   xx_p2=[Pcam(:,4)'
      locations(:,i)'];
   plot3(xx_p2(:,1),xx_p2(:,2),xx_p2(:,3),'k-');
end

locations_in1=inv([Pcam;0 0 0 1])*[locations(:,:);ones([1 size(locations,2)])];
for i=1:size(locations_in1,2),
   locations_in1(:,i)=locations_in1(:,i)/locations_in1(end,i);
end
for i=1:size(locations_in1,2),
  locations_in1(:,i)=locations_in1(:,i)*vf0/locations_in1(3,i);
end

locations_in1(4,:)=1;
locations_in1=[Pcam;0 0 0 1]*locations_in1;
for i=1:size(locations_in1,2),
   locations_in1(:,i)=locations_in1(:,i)/locations_in1(end,i);
end
plot3(locations_in1(1,:),locations_in1(2,:),locations_in1(3,:),'g+');
plot3(locations_in1(1,:),locations_in1(2,:),locations_in1(3,:),'go');

hh=plot3(Pcam(1,4),Pcam(2,4),Pcam(3,4),'k+');
set(hh,'MarkerSize',[8]);
hh=plot3(Pcam(1,4),Pcam(2,4),Pcam(3,4),'ko');
set(hh,'MarkerSize',[8]);


vf1=4;
%We plot the rays and see what happen
im1=projectors{1}.image;
Pcam=[projectors{1}.Rw,projectors{1}.Tw];
cambounds1=[1 size(im1,2)/2 size(im1,2)  1 size(im1,2)/2 size(im1,2)
   1 1             1            size(im1,1) size(im1,1) size(im1,1)
   1 1             1           1            1            1];
cambounds1_n=inv(projectors{1}.K)*cambounds1;
cambounds1_n=cambounds1_n*vf0;

cambounds1_n=[Pcam; 0 0 0 1]*[cambounds1_n;ones([1 size(cambounds1_n,2)])];
for i=1:size(cambounds1_n,2)
   cambounds1_n(:,i)=cambounds1_n(:,i)/cambounds1_n(end,i);
end
cambounds1_n=cambounds1_n(1:3,:);
im_frame=cambounds1_n(:,[1 3 6 4 1]);
plot3(im_frame(1,:),im_frame(2,:),im_frame(3,:),'b-');

im2_x=reshape(cambounds1_n(1,:),[3 2])';
im2_y=reshape(cambounds1_n(2,:),[3 2])';
im2_z=reshape(cambounds1_n(3,:),[3 2])';
ray_len=5;
hh2=warp(im2_x,im2_y,im2_z,im1);
hold on;
for i=1:size(locations,2),
   %xx_dir=locations(:,i)-Pcam(:,4);
   %xx_dir=xx_dir/norm(xx_dir);
   xx_p2=[Pcam(:,4)'
      locations(:,i)'];
   plot3(xx_p2(:,1),xx_p2(:,2),xx_p2(:,3),'b-');
end

locations_in1=inv([Pcam;0 0 0 1])*[locations(:,:);ones([1 size(locations,2)])];
for i=1:size(locations_in1,2),
   locations_in1(:,i)=locations_in1(:,i)/locations_in1(end,i);
end
for i=1:size(locations_in1,2),
  locations_in1(:,i)=locations_in1(:,i)*vf0/locations_in1(3,i);
end

locations_in1(4,:)=1;
locations_in1=[Pcam;0 0 0 1]*locations_in1;
for i=1:size(locations_in1,2),
   locations_in1(:,i)=locations_in1(:,i)/locations_in1(end,i);
end
plot3(locations_in1(1,:),locations_in1(2,:),locations_in1(3,:),'b+');
plot3(locations_in1(1,:),locations_in1(2,:),locations_in1(3,:),'bo');

hh=plot3(Pcam(1,4),Pcam(2,4),Pcam(3,4),'b+');
set(hh,'MarkerSize',[8]);
hh=plot3(Pcam(1,4),Pcam(2,4),Pcam(3,4),'bo');
set(hh,'MarkerSize',[8]);
plot_mesh(v,f);

hh=plot3(locations(1,:),locations(2,:),locations(3,:),'y+');set(hh,'MarkerSize',[8]);
