% SOLVESTEREOPT - Homogeneous linear solution of a stereo point
%
% Usage:  [pt, xy_reproj] = solvestereopt(xy, P)
%
% Multiview stereo: Solves 3D location of a point given image coordinates of
% that point in two, or more, images.
%
% Arguments:    xy - 2xN matrix of x, y image coordinates, one column for
%                    each camera.
%                C - N element cell array of corresponding image projection
%                    matrices. Or an N element cell array of camera structures
%
% Returns:      pt - 3D location in space returned in normalised
%                    homogeneous coordinates (a 4-vector with last element = 1)
%        xy_reproj - 2xN matrix of reprojected image coordinates.
%
%
% See also: CORRECTIMAGEPTS, CAMSTRUCT2PROJMATRIX

% Copyright (c) 2011-2016 Peter Kovesi
% Centre for Exploration Targeting
% The University of Western Australia
% peter.kovesi at uwa edu au
% 
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
% 
% The above copyright notice and this permission notice shall be included in 
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% May      2011
% February 2016   Provision for either camera projection matrices or camera
%                 structs 

function [pt, xy_reproj] = solvestereopt(xy, C)

    [dim,N] = size(xy);
    assert(N == length(C));
    assert(dim == 2);
    assert(N >= 2);
    
    % Determine if C is a cell array of projection matrices or camera
    % structures
    if isstruct(C{1})  % Assume camera structure
        % Correct image points from each camera so that they correspond to image
        % points from an ideal camera with no lens distortion.  Also generate the
        % corresponding ideal projection matrices for each camera struct
        for n = 1:N
            xy(:,n) = idealimagepts(C{n}, xy(:,n)); 
            P{n} = camstruct2projmatrix(C{n});
        end
        
    elseif isnumeric(C{1}) % Assume projection matrix
        P = C;
    else
        error('Camera argument must be a camera structure or projection matrix');
    end
    
    % Build eqn of the form A*pt = 0
    A = zeros(2*N, 4);
    for n = 1:N
	A(2*n-1,:) = xy(1,n)*P{n}(3,:) - P{n}(1,:);
	A(2*n  ,:) = xy(2,n)*P{n}(3,:) - P{n}(2,:);
    end
	
    [~,~,v] = svd(A);
    pt = hnormalise(v(:,4));
    
    if nargout == 2
        % Project the point back into the source images to determine the residual
        % error
        xy_reproj = zeros(size(xy));
        for n = 1:N
            xy_reproj(:,n) = cameraproject(P{n}, pt(1:3));
        end    
    end