function X = rok_camera_reconstruct_point (xp, cameras)
    % X = ROK_CAMERA_RECONSTRUCT_POINT (xp, cameras)
    %
    % Reconstruct a 3-D point from Projects 3-D point into camera's image 
    % plane, using the camera modelused by Camera Calibration Toolbox for 
    % Matlab.
    %
    % Input:
    %  - xp = Mx2 vector of points' image coordinates (at least two!)
    %  - cameras = array of M camera parameters structure
    %
    % Output:
    %  - X = 3x1 vector of 3-D coordinates
    %
    % Copyright (C) 2011, Rok Mandeljc <rok.mandeljc@fe.uni-lj.si>
    
    % Make vectors from parameters
    cc = [ cameras.c ]';
    fc = [ cameras.f ]';
    alpha = [ cameras.alpha ]';

    R = cat(3, cameras.R);
    T = cat(2, cameras.T);

    zeta = (cc - xp) ./ fc;

    % Construct the matrices
    A = [ 
            % Equation 1
            reshape(R(1,1,:),[],1) + alpha.*reshape(R(2,1,:),[],1) + zeta(:,1).*reshape(R(3,1,:),[],1), ...
            reshape(R(1,2,:),[],1) + alpha.*reshape(R(2,2,:),[],1) + zeta(:,1).*reshape(R(3,2,:),[],1), ...
            reshape(R(1,3,:),[],1) + alpha.*reshape(R(2,3,:),[],1) + zeta(:,1).*reshape(R(3,3,:),[],1);
            % Equation 2
            reshape(R(2,1,:),[],1) + zeta(:,2).*reshape(R(3,1,:),[],1), ...
            reshape(R(2,2,:),[],1) + zeta(:,2).*reshape(R(3,2,:),[],1), ...
            reshape(R(2,3,:),[],1) + zeta(:,2).*reshape(R(3,3,:),[],1)      
        ];

    b = [
            % Equation 1
            -( T(1,:)' + alpha.*T(2,:)' + zeta(:,1).*T(3,:)' );
            % Equation 2
            -( T(2,:)' + zeta(:,2).*T(3,:)' )
        ];

    % We don't allow invalid entries anyway
    %idx = any(isnan(A), 2) & any(isnan(b), 2);
    %A(idx,:) = [];
    %b(idx,:) = [];

    % Least square solution
    X = A\b;
end
