stereo_match.m
%ShapeStart Measurement Systems Project Day 12/17/10
%Author: Karol Hatzilias
%Description - to take a stereo pair of images and find the interesection
%of a given correspondence
%
%Inputs - undistorted image correspondance, camera center
%
%Output - xyz coordinates of the interesection of the stereo correspondence
%
%exampe of how to call: [x y z]=stereo_match(calib,corr1,corr2) where:
%calib is the output of matlab calibration (calibration_results.mat)
%corr1  = [X Y]
%corr2  = [X Y]


function [ray1_pt1,ray1_pt2,ray2_pt1,ray2_pt2,holdme]=stereo_match(Rc_l,Tc_l,Rc_r,Tc_r,l_x,l_y,r_x,r_y,KK);

%construct the first ray
ray1_pt1=Rc_l^-1*([0;0;0]-Tc_l);
ray1_pt2=Rc_l^-1*(KK^-1*[l_x;l_y;1]-Tc_l);

%construct the second ray
ray2_pt1=Rc_r^-1*([0;0;0]-Tc_r);
ray2_pt2=Rc_r^-1*(KK^-1*[r_x;r_y;1]-Tc_r);

minva=1000;
%find intersection
for test=750:1500
   pointonray1=(1-test)*ray1_pt1+test*ray1_pt2;
   for test2=750:1500
       pointonray2=(1-test2)*ray2_pt1+test2*ray2_pt2;
       if (norm(pointonray1-pointonray2)<minva)
           minva=norm(pointonray1-pointonray2);
           holdme=pointonray2;
       end
   end
end