fig2

cam1.pos = [10 2 5];
cam1.target = [0 0 0];
scl = 2;

cam2.pos = [2 10 5];
cam2.target = [0 0 0];

pt = [4 8 3];
[x y z] = sphere;
ps = 4;
figure(1);
surf(x/ps+pt(1),y/ps+pt(2),z/ps+pt(3));
daspect([1 1 1]);
view(135,60);
hold on;
cam1 = cam_obj(cam1,scl);
text(cam1.pos(1),cam1.pos(2),cam1.pos(3),'1','FontSize',16);
[img1 invpt1] = cam_plot(cam1,pt,'g');
cam2 = cam_obj(cam2,scl);
text(cam2.pos(1),cam2.pos(2),cam2.pos(3),'2','FontSize',16);
[img2 invpt2] = cam_plot(cam2,pt,'g');
hold off
axis equal
xlabel('x axis');
ylabel('y axis');
zlabel('z axis');

figure(2);
subplot(1,2,1);
plot(img1(1),img1(2),'ko','MarkerSize',10,'MarkerFaceColor','g');
grid
axis([-scl scl -scl scl]);
axis square
axis ij
title('camera 1');
subplot(1,2,2);
plot(img2(1),img2(2),'ko','MarkerSize',10,'MarkerFaceColor','g');
grid
axis([-scl scl -scl scl]);
axis square
axis ij
title('camera 2');

% test back projection equations
% we are working in world coordinates (using invpt)

k1 = invpt1 - cam1.pos;
k2 = invpt2 - cam2.pos;

C = cam2.pos - cam1.pos;

A = [dot(k1,k1) -dot(k1,k2); -dot(k1,k2) dot(k2,k2)];
b = [dot(k1,C); -dot(k2,C)];

q = A\b;

P1 = cam1.pos + q(1)*k1;
P2 = cam2.pos + q(2)*k2;
P = 0.5*(P1+P2);
fprintf('projected point P = [ %g %g %g ]\n',P);
fprintf('|P2-P1| = %g\n',norm(P2-P1));
fprintf('error = %g\n',norm(P-pt));

projected point P = [ 4 8 3 ]
|P2-P1| = 2.97904e-15
error = 2.03507e-15

Published with MATLAB® R2016a