extrinsics

close all
clear

load params1

for n=1:2

eval(['load SaddlePoints' int2str(n)]);
im = imread(filename);
figure
show_saddle_points(im,xc,yc);

wpts = [ 0 0; 9 0; 21 0; 21 9; 21 21; 9 21; 0 21; 0 9];
switch n
    case 1
         ndx = [1 4 8 14 46 42 47 33 ];
    case 2
        ndx = [43 40 31 28 39 10 1 45];
end
imgpts = [xc(ndx)' yc(ndx)'];

n0 = 1;
n1 = 3;
n2 = 7;
hold on
plot(imgpts([n0 n1],1),imgpts([n0 n1],2),'y','LineWidth',2);
text(imgpts(n1,1)+100,imgpts(n1,2),'X','FontSize',24,'Background','w');
plot(imgpts([n0 n2],1),imgpts([n0 n2],2),'y','LineWidth',2);
text(imgpts(n2,1),imgpts(n2,2)+100,'Y','FontSize',24,'Background','w');
hold off

[R, T] = extrinsics(imgpts,wpts,cameraParams);

Rs(:,:,n) = R;
Tw(n,1:3) = -T*R'/12;
end

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

ndx = [0 3 6 9 12 15 18 21];
figure(3);
for k= ndx
    plot(k*ones(1,8),ndx,'k');
    hold on
    plot(ndx,k*ones(1,8),'k');
end
hold off


hold on
plot(wpts([n0 n1],1),wpts([n0 n1],2),'y','LineWidth',2);
text(wpts(n1,1),wpts(n1,2),'X','FontSize',24,'Background','w');
plot(wpts([n0 n2],1),wpts([n0 n2],2),'y','LineWidth',2);
text(wpts(n2,1),wpts(n2,2),'Y','FontSize',24,'Background','w');

for m=1:2
    k = Rs(1:3,3,m);
    plot(Tw(m,1)*12+[0 20*k(1)],Tw(m,2)*12+[0 20*k(2)],'b');
end
plot(Tw(:,1)*12,Tw(:,2)*12,'ko','MarkerSize',7,'MarkerFaceColor','w');
hold off

axis([-50 30 -10 70]);
axis equal
grid
axis ij
xlabel('x (inches)');
ylabel('y (inches)');
title('group 1');

diff = Tw(2,:)-Tw(1,:);
fprintf('T between cameras %g %g %g ft\n',diff)
fprintf('distance %g ft\n',norm(diff));

T between cameras 4.03615 2.78236 0.307346 ft
distance 4.91187 ft

Published with MATLAB® R2017a