extrinsic_computation

% Computation of the Extrinsic Parameters attached to the grid:

[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c);


% Reproject the points on the image:

[x_reproj] = project_points2(X_ext,omc_ext,Tc_ext,fc,cc,kc,alpha_c);

err_reproj = x_ext - x_reproj;

err_std2 = std(err_reproj')';


Basis = [X_ext(:,[ind_orig ind_x ind_orig ind_y ind_orig ])];

VX = Basis(:,2) - Basis(:,1);
VY = Basis(:,4) - Basis(:,1);

nX = norm(VX);
nY = norm(VY);

VZ = min(nX,nY) * cross(VX/nX,VY/nY);

Basis = [Basis VZ];

[x_basis] = project_points2(Basis,omc_ext,Tc_ext,fc,cc,kc,alpha_c);

dxpos = (x_basis(:,2) + x_basis(:,1))/2;
dypos = (x_basis(:,4) + x_basis(:,3))/2;
dzpos = (x_basis(:,6) + x_basis(:,5))/2;



% image(I);
% colormap(gray(256));
imshow(I,gray(256));
hold on;
plot(x_ext(1,:)+1,x_ext(2,:)+1,'r+');
plot(x_reproj(1,:)+1,x_reproj(2,:)+1,'yo');
h1 = text(x_ext(1,ind_orig)-25,x_ext(2,ind_orig)-25,'O');
set(h1,'Color','g','FontSize',14);
h2 = text(dxpos(1)+1,dxpos(2)-30,'X');
set(h2,'Color','g','FontSize',14);
h3 = text(dypos(1)-30,dypos(2)+1,'Y');
set(h3,'Color','g','FontSize',14);
h4 = text(dzpos(1)-10,dzpos(2)-20,'Z');
set(h4,'Color','g','FontSize',14);
plot(x_basis(1,:)+1,x_basis(2,:)+1,'g-','linewidth',2);
title('Image points (+) and reprojected grid points (o)');
hold off;
clear h1 h2 h3 h4


fprintf('\n\nExtrinsic parameters:\n\n');
fprintf('Translation vector: Tc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',Tc_ext);
fprintf('Rotation vector:   omc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',omc_ext);
fprintf('Rotation matrix:    Rc_ext = [ %3.6f \t %3.6f \t %3.6f\n',Rc_ext(1,:)');
fprintf('                               %3.6f \t %3.6f \t %3.6f\n',Rc_ext(2,:)');
fprintf('                               %3.6f \t %3.6f \t %3.6f ]\n',Rc_ext(3,:)');
fprintf('Pixel error:           err = [ %3.5f \t %3.5f ]\n\n',err_std2);

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


Extrinsic parameters:

Translation vector: Tc_ext = [ -0.384353 	 5.968201 	 115.958368 ]
Rotation vector:   omc_ext = [ -1.738262 	 1.924212 	 -0.229528 ]
Rotation matrix:    Rc_ext = [ -0.029887 	 -0.872103 	 0.488409
                               -0.962517 	 0.156887 	 0.221238
                               -0.269568 	 -0.463490 	 -0.844103 ]
Pixel error:           err = [ 3.53832 	 2.49570 ]

% Stores the results:

if ~exist('kk','var')
    return;
end

% Stores location of grid wrt camera:

ns = num2str(kk);
eval(['omc_' ns ' = omc_ext;']);
eval(['Tc_' ns ' = Tc_ext;']);

% Stores the projected points:

eval(['y_' ns ' = x_reproj;']); % reprojected corners
eval(['X_' ns ' = X_ext;']); % world corner coordinates
eval(['x_' ns ' = x_ext;']); % actual corner coordinates
eval(['ex_' ns '=x_ext - x_reproj;']); % reprojectioni errors


% Organize the points in a grid:

eval(['n_sq_x_' ns ' = n_sq_x;']);
eval(['n_sq_y_' ns ' = n_sq_y;']);