% CALIBRATE
%
% Function to perform camera calibration
%
% Usage:   C = calibrate(im, XYZ, uv)
%
%   Where:   im  - is the image of the calibration target.
%            XYZ - is a n x 3 array of  XYZ coordinates
%                  of the calibration target points. 
%            uv  - is a 2 x n array of the image coordinates
%                  of the calibration target points.
%            C   - is the 3 x 4 camera calibration matrix.
%
%  This function plots the uv coordinates onto the image of
%  the calibration target.  It also projects the XYZ coordinates
%  back into image coordinates using the  calibration matrix
%  and plots these points too as a visual check on the accuracy of
%  the calibration process.  
%  Lines from the origin to the vanishing points in the X, Y and 
%  Z directions are overlaid on the image.
%  The mean squared error between the  positions of the uv coodinates 
%  and the projected XYZ coordinates is also reported.
%
%  The function should also report the error in satisfying the 
%  camera calibration matrix constraint - the magnitude of
%   (q1 x q3).(q2 x q3)
%
function C = calibrate(im, XYZ, uv)
    
    % obtain rows so arbitrary number of points can be used
    [rows, cols] = size(XYZ);
    
    XYZ1 = [XYZ, ones(rows, 1)]; % makes it easier to work with
    
    % build B matrix
    for n = 1:rows
        B(2*n-1, :) = [XYZ1(n, :) 0 0 0 0 -uv(1, n)*XYZ(n, :)];
        B(2*n,   :) = [0 0 0 0 XYZ1(n, :) -uv(2, n)*XYZ(n, :)];
    end
    
    c = B \ uv(:);
    c(12) = 1;
    C = reshape(c,4,3)'
    
    XYZ1 = XYZ1';
    for i = 1:rows
        suv(:,i) = C*XYZ1(:,i);
        suv(:,i) = suv(:,i)/suv(3,i);
    end

    % calculate the mean squared error between the positions of the uv coodinates and suv.
    mse = mean(mean((uv - suv(1:2,:)).^2));
    fprintf(1, 'Mean squared error is %d\n', mse);
    
    % calculate the error in satisfying the camera calibration matrix constraint
    q1 = C(1,1:3)';
    q2 = C(2,1:3)';
    q3 = C(3,1:3)';
    error = abs(dot(cross(q1, q3), cross(q2, q3)));
    fprintf(1, 'Error in satisfying the camera calibration matrix is %d\n', error);
    
    % annotate image
    imshow(im);
    hold;
    % plot uv coordinates
    plot(uv(1, :), uv(2, :),'rx')
        
    % plot XYZ coordinates
    plot(suv(1,:), suv(2,:),'b+')
    
    % draw vanishing lines
    for i = 1:3
        line([C(1,4), C(1,i)/C(3,i)],[C(2,4), C(2,i)/C(3,i)])
    end
    hold;
        
        
        
        
        
        
        
        
        
        
        
        