% STEREO
%
% Usage: pt3D = stereo(im1, im2, C1, C2)
%
% Where:  im1 and im2 are the two stereo images
%         C1 and C2 are the calibration matrices
%         for these two images respectively
%
% The function will prompt you to digitise some points in the first image
% (finishing by clicking the right button).  The function then
% prompts you to digitise the equivalent points (which you must digitise 
% in exactly the same sequence) in the second image.  
% The function then solves the stereo equations
% and returns the 3D coordinates of the points in pt3D.
function pt3D = stereo(im1, im2, C1, C2)

	fprintf(1, 'Digitise some points in figure 1\n');
	figure(1)
	imshow(im1);
	[u1,v1] = digipts;
	fprintf(1, 'Digitise some points in figure 2\n');
	figure(2)
	imshow(im2);
	[u2,v2] = digipts;
	
	% check if same number of points are selected
	if length(u1) ~= length(u2)
        fprintf(1, 'Same number of points not selected\n');
	end

	for i = 1:length(u1)
	    a = [C1(1:2,1:3) - [u1(i)*C1(3,1:3); v1(i)*C1(3,1:3)];
             C2(1:2,1:3) - [u2(i)*C2(3,1:3); v2(i)*C2(3,1:3)]];
	    c = [u1(i) - C1(1,4);
             v1(i) - C1(2,4);
             u2(i) - C2(1,4);
             v2(i) - C2(2,4)];
        b(:, i) = a \ c;
	end
	pt3D = b';