function w = ibtriangulation(PPM, imagePoints, error)

%N_vies Interval-based Triangulation
%
%INPUT
%PPM: camera matrices, in the format 3x4xn_views
%imagePoints: correspondent points, in the format n_pointsx2xn_views
%error: starting data error
%
%OUTPUT
%w: 3D cuboids 
%
%Remember to install INTLAB Matlab toolbox before running this script
%
%Michela Farenzena - June 2006




if size(PPM,1)~=3 || size(PPM,2)~=4
    fprintf('Error: PPM must be in 3x4xn_views format');
end


global numberOfViews;
numberOfViews = size(PPM,3);

int_point = intval(zeros(size(imagePoints,1), 2,numberOfViews));
int_point = midrad(imagePoints,error);


w = intersect_rays(PPM,int_point)';

%Display the result

for j = 1:size(w,1)
    hold on
    cube(inf(w(j,:)), sup(w(j,:)), 'k')
end

xlabel('x axis');
ylabel('y axis');
zlabel('z axis');
grid on
axis equal



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

function W = intersect_rays(PPM, imagePoints)

vec_ni=[];
vec_mu=[];

global numberOfViews;

W = ones(3,size(imagePoints,1))*infsup(-Inf,Inf);

   for i = 1:numberOfViews
      for j = i+1:numberOfViews 

    
        cl = - inv(PPM(:,1:3,i))*PPM(:,4,i);
        P1 = PPM(:,:,i);
        P2 = PPM(:,:,j);
        er = P2 * [cl' 1]';
        m1 = imagePoints(:,:,i);
        m2 = imagePoints(:,:,j);
    
        Wij = [];
        
        for m = 1:size(m1,1)
            
        % Closed-form triangulation from 2 views 
        
            pr = [m1(m,:) 1]';
            pl = [m2(m,:) 1]';
            pp = P2(:,1:3)*inv(P1(:,1:3))* pr;
            d = cross(er,pp);
            mu = (cross(pl,pp)'* d) / norm(d)^2;
            d = cross(pp,er);
            ni = (cross(pl,er)'* d) / norm(d)^2;


            w1 = [cl; 1]+ ni/mu*[inv(P1(:,1:3))*pr; 0];

        
            if isnan(w1(1))
                w1 = infsup([-Inf,-Inf,-Inf],[Inf,Inf,Inf])';
            end
                
            Wij = [Wij w1(1:3)]; 
           
                
        end
  
         % Then, intersect the results
         
         W = intersect(W,Wij);
         
         if(~isempty(find(isnan(W))))
             fprintf('Empty intersection: correspondence %d \n',find(isnan(W(1,:))))
             return
         end
    end
   end



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


function cube(in, su, color)

%Display a 3D cuboid

x1 = in(1);
y1 = in(2);
z1 = in(3);
x2 = su(1);
y2 = su(2);
z2 = su(3);

hold on;


plot3([x1,x1,x1,x1],[y1,y2,y2,y1],[z1,z1,z2,z2],color, 'linewidth', 1.0);
plot3([x2,x2,x2,x2],[y1,y2,y2,y1],[z1,z1,z2,z2],color, 'linewidth', 1.0);
plot3([x1,x1,x2,x2],[y1,y1,y1,y1],[z1,z2,z2,z1],color, 'linewidth', 1.0);
plot3([x1,x1,x2,x2],[y2,y2,y2,y2],[z1,z2,z2,z1],color, 'linewidth', 1.0);
plot3([x1,x2,x2,x1],[y1,y1,y2,y2],[z1,z1,z1,z1],color, 'linewidth', 1.0);
plot3([x1,x2,x2,x1],[y1,y1,y2,y2],[z2,z2,z2,z2],color, 'linewidth', 1.0);