Foot_v2.m

%ShapeStart Measurement Systems Project Day 12/17/10
%Author: Karol Hatzilias
%Description - load two images, load calibration(int & ext) for images,
%find corresponding pixels, send them to stereo_match

%HACK <- need to make it input the image numbers
left=13;
right=14;
Rc_l=Rc_15;
Tc_l=Tc_15;
Rc_r=Rc_16;
Tc_r=Tc_16;
% pause;
% eval(['Rc_l=Rc_' left ';'])



%find the corresponding pixels
%-----Need to write this still----------
% l_x=581;
% l_y=388; %manually clicked input for now
%
% r_x=501;
% r_y=382;


l_x=[726 729 727 725 769 770 770 769 812 813 814 812];
l_y=[188 149 108 70 189 152 110 71 188 153 111 72];
r_x=[575 574 574 576 620 618 618 618 663 660 661 663];
r_y=[151 118 80 41 154 119 79 41 155 119 81 41];



for count=1:length(l_x)
[ray1_pt1,ray1_pt2,ray2_pt1,ray2_pt2,holdme]=stereo_match(Rc_l,Tc_l,Rc_r,Tc_r,l_x(count),l_y(count),r_x(count),r_y(count),KK);
%plot the rays
for t=0:1500
    plot_ray1(t+1,:)=(1-t)*ray1_pt1+t*ray1_pt2;
    plot_ray2(t+1,:)=(1-t)*ray2_pt1+t*ray2_pt2;
end

plot3(plot_ray1(:,1),plot_ray1(:,2),plot_ray1(:,3));
hold on;
plot3(plot_ray2(:,1),plot_ray2(:,2),plot_ray2(:,3));

plot3(holdme(1),holdme(2),holdme(3),'rx');
axis equal;

storexyz(count,:)=holdme;
end