fig3

test back projection with multiple points

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; 4 6 2; 2 3 0.1; 1 2 4];
[N M] = size(pt);
[x y z] = sphere;
ps = 2;
figure(1);
cam1 = cam_obj(cam1,scl);
daspect([1 1 1]);
view(135,60);
grid;
hold on
for k=1:N
    surf(x/ps+pt(k,1),y/ps+pt(k,2),z/ps+pt(k,3));
end
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
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
title('camera 2');

% test back projection equations

C = cam2.pos - cam1.pos;

for k=1:N
    k1 = invpt1(k,:) - cam1.pos;
    k2 = invpt2(k,:) - cam2.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 %d P = [ %g %g %g ]\n',k,P);
    fprintf('|P2-P1| = %g\n',norm(P2-P1));
    fprintf('error = %g\n\n',norm(P-pt(k,:)));
end

projected point 1 P = [ 4 8 3 ]
|P2-P1| = 2.03507e-015
error = 1.60119e-015

projected point 2 P = [ 4 6 2 ]
|P2-P1| = 2.66454e-015
error = 1.66163e-015

projected point 3 P = [ 2 3 0.1 ]
|P2-P1| = 4.44089e-015
error = 6.14603e-015

projected point 4 P = [ 1 2 4 ]
|P2-P1| = 3.58036e-015
error = 2.51215e-015

Published with MATLAB® 7.9