%Computer vision, assignment 5 on surfaces and silhouettes
% Fredrik Kahl, Centre for Mathematical Sciences, 2007

%load: motion in "P" and structure in "X" and masked images in "masks"
load inl5_data;

resolution = 10; %resolution, in voxels per side: 10 is low and 100 is decent

%Create a block that covers all reconstructed 3D points
mx=min(X')';
Mx=max(X')';
dx0=Mx-mx;
xstart=mx-0.05*dx0; %one corner point of the block
xend=Mx+0.05*dx0; %opposite corner point of the block
dx=xend-xstart;
dstep=min(dx)/(resolution-1); %steps in voxel resolution

%create a mesh of points
[x,y,z]=meshgrid(xstart(1):dstep:xend(1),xstart(2):dstep:xend(2),xstart(3):dstep:xend(3));
[sx,sy,sz]=size(x); %size in terms of number of voxels of block
vol=ones(sx,sy,sz); %all voxels in volume are first set to one

for imnr=1:36;
    mask=masks{imnr}; %the mask for image "imnr"
    [m,n]=size(mask);
    Pcurrent=P{imnr}; %the camera matrix for image "imnr"
    
    index=find(vol); %compute indices to those points that are set to one in the volume
    
    %project 3D points to image with camera equation
    tmp=Pcurrent*[x(index),y(index),z(index),ones(length(index),1)]';
    proj=round(tmp./(ones(3,1)*tmp(3,:))); %divide by 3rd coordinate
    
    %find points outside image
    index_remove = find( proj(1,:)<1 | proj(1,:)>n | proj(2,:)<1 | proj(2,:)>m);
    
    %and then remove them in: volume,projected 2d points and index set
    vol(index(index_remove))=0;
    proj(:,index_remove)=[];
    index(index_remove)=[];
    
    %now find the points that are not projected onto the mask
    %and then remove them in: volume,projected 2d points and index set
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%THIS PART NEEDS TO BE WRITTEN...

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if 1, %1:plot 2d image, 0: don't plot 2d image
        figure(1);clf;imagesc(mask);colormap(gray(256));hold on;
        plot(proj(1,:),proj(2,:),'g.');zoom on;
        pause;
    end
end

%plot the surface by finding the isosurface between voxels
% that are set to one and zero
figure(2);clf;
isosurface(x,y,z,vol,0.5);
daspect([1 1 1]); axis tight;
campos([ -23.7637  987.6234  369.9612]);
camlight headlight;
rotate3d on;