2013_0417 photos script2

clear
close all
load filelist

filename = char(files(1));
rgb = imread(filename);
imshow(rgb);

% pts = ginput(4)
% save image1_pts pts

load image1_pts

hold on
plot([pts(:,1); pts(1,1)],[pts(:,2); pts(1,2)],'b','LineWidth',2);
hold off

cc = [ 1056.532269918812300  1113.068166667991800 ];

hold on
plot(cc(1),cc(2),'yx');
text(cc(1)-60,cc(2)-10,'cc');
hold off

Warning: Image is too big to fit on screen; displaying at 33%

basepts = [ -3.5 0; 3.5 0; 3.5 -7; -3.5 -7]*50;
tf = cp2tform(pts,basepts,'Projective');

out = imtransform(rgb,tf,'Xdata',[-1000 1000],'Ydata',[-1000 500]);
imshow(out);

n = 16;
filename = char(files(n));
rgb2 = imread(filename);
subplot(1,1,1);
imshow(rgb2);

load hallpts16
%hallpts = ginput(6);
%save hallpts16 hallpts
hold on
plot(hallpts(1:2,1),hallpts(1:2,2),'y','LineWidth',2);
plot(hallpts(3:4,1),hallpts(3:4,2),'m','LineWidth',2);
plot(hallpts(5:6,1),hallpts(5:6,2),'b','LineWidth',2);
hold off

Warning: Image is too big to fit on screen; displaying at 33%

out2 = imtransform(rgb2,tf,'Xdata',[-1200 1200],'Ydata',[-5000 500]);
[xv yv] = tformfwd(tf,hallpts(:,1),hallpts(:,2));
xw = xv+1200;
yw = yv+5000;

subplot(1,1,1)
imshow(out2);
hold on
plot(xw(1:2),yw(1:2),'y','LineWidth',2);
plot(xw(3:4),yw(3:4),'m','LineWidth',2);
plot(xw(5:6),yw(5:6),'b','LineWidth',2);
hold off

targ_width = abs(perpdraw(xw(3:4),yw(3:4),xw(6),yw(6)));
fprintf('target width %g pixels\n',targ_width);
hall_width = abs(perpdraw(xw(3:4),yw(3:4),xw(1),yw(1)));
fprintf('hall width %g pixels\n',hall_width);
act_targ_width = 2;
fprintf('hall width = %g\n',hall_width/targ_width*act_targ_width);
%hd = imdistline

Warning: Image is too big to fit on screen; displaying at 8% 
target width 378.978 pixels
hall width 1193.51 pixels
hall width = 6.29859

angle = cross2d([xv(1:2) yv(1:2)],[xv(3:4) yv(3:4)]);
fprintf('hall divergence angle %g deg\n',angle);

hall divergence angle -2.93383 deg

f = 2500;
phi = linspace(90,120,31);
angle = zeros(size(phi));

for k=1:length(phi)
    angle(k) = look_down( phi(k), f, size(rgb2), hallpts );
end

plot(phi,angle,'ko-','LineWidth',2);
grid
xlabel('look angle (deg)');
ylabel('divergence angle (deg)');

ndx = find(and(phi>=100,phi<=105));
phiz = interp1(angle(ndx),phi(ndx),0);
[angle2 tf2] = look_down(phiz, f, size(rgb2), hallpts );
fprintf('look down %g deg, divergence angle %g\n',phiz,angle2);

look down 101.59 deg, divergence angle 0.000136069

out3 = imtransform(rgb2,tf2,'Xdata',[-1200 1200],'Ydata',[-5000 0]);
[xa  ya] = tformfwd(tf2,hallpts(:,1),hallpts(:,2));
xw = xa+1200;
yw = ya+5000;

subplot(1,1,1)
imshow(out3);
hold on
plot(xw(1:2),yw(1:2),'y','LineWidth',2);
plot(xw(3:4),yw(3:4),'m','LineWidth',2);
plot(xw(5:6),yw(5:6),'b','LineWidth',2);
hold off

theta  = cross2d([xv(3:4) yv(3:4)],[0 0; 0 1]);
fprintf('camera orientation angle %g deg\n',theta);


targ_width = abs(perpdraw(xw(3:4),yw(3:4),xw(6),yw(6)));
fprintf('target width %g pixels\n',targ_width);
hall_width = abs(perpdraw(xw(3:4),yw(3:4),xw(1),yw(1)));
fprintf('hall width %g pixels\n',hall_width);
act_targ_width = 2;
fprintf('hall width = %g\n',hall_width/targ_width*act_targ_width);

scale = targ_width/act_targ_width;
cam_height = 600/scale;
fprintf('camera height %g\n',cam_height);

Warning: Image is too big to fit on screen; displaying at 13% 
camera orientation angle -2.32437 deg
target width 506.976 pixels
hall width 1491.7 pixels
hall width = 5.88471
camera height 2.36697

out = imtransform(rgb,tf2,'Xdata',[-1000 1000],'Ydata',[-3000 0]);
imshow(out);

[xpts ypts] = tformfwd(tf2,pts(:,1),pts(:,2));
hold on
plot([xpts(:); xpts(1)]+1000,[ypts(:); ypts(1)]+3000,'b','LineWidth',2);
hold off
xpts = xpts/scale;
ypts = ypts/scale;
[xpts ypts]
width = xpts(2)-xpts(1);
height = ypts(1)-ypts(3);
hold on
plot([0 0 xpts(1)]*scale+1000,[0 ypts(1) ypts(1)]*scale+3000,'y','LineWidth',2);
hold off
fprintf('measured target width %g height %g\n',width,height);
fprintf('actual %g\n',21/12);

fprintf('campos (x, y) = %g %g\n',xpts(1),ypts(1));

save script2 tf2 scale

ans =

   -1.1505   -6.5640
    0.5959   -6.6373
    0.6278   -8.3904
   -1.1470   -8.2397

measured target width 1.74636 height 1.82646
actual 1.75
campos (x, y) = -1.15049 -6.56397

sz = size(rgb2);
[x2 y2] = tformfwd(tf2,sz(2)/2,sz(1))
fprintf('distance from camera to bottom of image %g\n',-y2/scale);
hold on
plot(x2+1000,y2+3000,'ro');
hold off

x2 =

     0


y2 =

  -1.0974e+03

distance from camera to bottom of image 4.32926