script5

clear
close all

    pos1 = [1 17 3];
  target1 = [4 0 3];
    pos2 = [5 17 3];
    target2 = [2 0 3];

cam1 = camobj(pos1,target1);
cam2 = camobj(pos2,target2);


%          1      2      3      4      5      6      7      8
vcube = [ 0 0 0; 1 0 0; 1 1 0; 0 1 0; 0 0 1; 1 0 1; 1 1 1; 0 1 1]';
idx = [1 2 3 4 1 5 6 7 8 5 6 2 3 7 8 4];
vert(1,:) = vcube(1,:)*6;  % hallway width 6 ft.
vert(2,:) = vcube(2,:)*90; % length 90 ft.
vert(3,:) = vcube(3,:)*8;  % height 8 ft.


x = vert(1,idx);
y = vert(2,idx);
z = vert(3,idx);
plot3(x,y,z,'k-');
hold on
ndx = [1 2 6 5 1];
x = vert(1,ndx);
y = vert(2,ndx);
z = vert(3,ndx);
plot3(x,y,z,'r-','LineWidth',2);
hold off

campos(pos2);
camtarget(target2);
camup([0 0 1]);
camproj('perspective');
camva(40);
set(gcf,'renderer','zbuffer')
axis equal
axis off
NPIX = 640;
set(gcf,'Position',[100 100 NPIX NPIX]);
set(gca,'Position',[0 0 1 1]);


f = getframe(gcf);
rgb = f.cdata;

pts = [x; y; z];
ipts = h2e(cam2.KK*cam2.T*e2h(pts));
imshow(rgb);
hold on
plot(ipts(1,:),ipts(2,:),'bx','MarkerSize',11,'LineWidth',2);
hold off
axis ij

T1 = [cam1.T; 0 0 0 1]
T2 = [cam2.T; 0 0 0 1]

inv(T1)
inv(T2)

T1 =

   -0.9848   -0.1738         0    3.9391
         0         0   -1.0000    3.0000
    0.1738   -0.9848         0   16.5675
         0         0         0    1.0000


T2 =

   -0.9848    0.1738         0    1.9696
         0         0   -1.0000    3.0000
   -0.1738   -0.9848         0   17.6102
         0         0         0    1.0000


ans =

   -0.9848         0    0.1738    1.0000
   -0.1738         0   -0.9848   17.0000
         0   -1.0000         0    3.0000
         0         0         0    1.0000


ans =

   -0.9848         0   -0.1738    5.0000
    0.1738         0   -0.9848   17.0000
         0   -1.0000         0    3.0000
         0         0         0    1.0000

% imshow(rgb);
%
% % mypts = ginput(4);
% % save script5 mypts
% load script5
% hold on
% plot(mypts(:,1),mypts(:,2),'bx','MarkerSize',11);
% hold off