% Suppose that we have a following picture hanging on a blue wall
im = imread('footbalarge.jpg');
%clf;
%image(im);
axis('image');
x_margin = 10;
y_margin = 10;
im = imresize(im,0.20,'bilinear');
wall_picture = zeros([2*y_margin+size(im,1) 2*x_margin+size(im,2) 3]);
wall_picture(:,:,3) = 255;
wall_picture(y_margin+(1:size(im,1)), x_margin+(1:size(im,2)),:)=im;
figure(2);
clf;
image(wall_picture/255);
axis('image');
axis('off');
set(gcf,'Color',[0 0 1]);
axis('off');
set(gca,'Color',[0 0 1]);
wall_scale = 1;
% The original is at 0, 0, 0
% We need to specify the virtual camera

zz1=[size(wall_picture,2)/2 0 -100];
zz1=[size(wall_picture,2)*1.5 0 -20];
zz1 = [size(wall_picture,2)/2 size(wall_picture,1)/2 -100];
ccenter=[size(wall_picture,2)/2 size(wall_picture,1)/2 0];

z1 = ccenter - zz1;
z1= z1/norm(z1);
x1=[1 0 0];
x1=x1-(z1*x1')*z1;
x1=x1/norm(x1);
%fprintf(1,'%15.14f ', x1);
%fprintf(1,'\n');
y1=cross(z1', x1');
y1=y1/norm(y1);
y1=y1';

f1=0.5;
ppoint=ccenter+f1*z1;
t1=zz1';
R1=[x1;y1;z1]';

K1=[f1 0 0;0 f1 0; 0 0 1];
P1=K1*[R1' (-R1'*t1)];

lower_left = [0 0 0 1]';
lower_left = P1 * lower_left;
lower_left = lower_left/lower_left(3);
upper_right = [size(wall_picture,2) size(wall_picture,1) 0 1]';
upper_right = P1 * upper_right;
upper_right = upper_right/upper_right(3);

figure(1);
clf;
%set(gca,'XLim',[max(lower_left(1),upper_right(1)) max(lower_left(1),upper_right(1))]);
%set(gca,'YLim',[max(lower_left(2),upper_right(2)) max(lower_left(2),upper_right(2))]);
%set(gca,'XLim',[-0.5 0.5]);
%set(gca,'YLim',[-0.5 0.5]);
set(gcf,'Color',[1 1 1]);
axis('off');
set(gca,'Color',[1 1 1]);
%axis equal;
%hold on;
% If a plane in the world coordinate is given by A X + B Y + C Z + D = 0;
% then it will be given by [A B C D]*[R t; 0 0 0 1] in the camera coordinate
% In this particular case, we have Z = 0, which is
wall_coeff_world = [0 0 1 0];
wall_coeff_cam = wall_coeff_world * [R1 t1; 0 0 0 1];
keyboard;
cam_image = zeros([768 1024 3]);
intrinsic_cam =[-1024 0 512; 0 -768 384; 0 0 1];
inverse_intrinsic = inv(intrinsic_cam);
points = 0;
for pix_point_col = 1:1024,
   for pix_point_row = 1:768,
      cam_point = inverse_intrinsic * [pix_point_col pix_point_row 1]';
      ray_coeff =[cam_point(1)/f1 cam_point(2)/f1 1]';
      Z = -wall_coeff_cam(4)/(wall_coeff_cam(1:3)*ray_coeff);
      X = cam_point(1)*Z/f1;
      Y = cam_point(2)*Z/f1;
      point_world = R1*[X Y Z]'+t1;
      X = point_world(1);
      Y = point_world(2);
      pX = floor(X); if pX < 1, pX = 1; end 
      if pX > size(wall_picture,2), pX = size(wall_picture,2); end;
      pY = floor(Y); if pY < 1, pY = 1; end
      if pY > size(wall_picture,1), pY = size(wall_picture,1); end;
      pY = size(wall_picture,1)-pY+1;
      cam_image(pix_point_row,pix_point_col,:) = wall_picture(pY,pX,:)/255;
   end 
   %if mod(pix_point_col,100)==0,
   %   fprintf(1,'%d ', pix_point_col);
   %   pause;
   %end
end
fprintf(1,'Done.\n');
image(cam_image);
axis image;
