extrinsic_computation
[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c);
[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;
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 ]
if ~exist('kk','var')
return;
end
ns = num2str(kk);
eval(['omc_' ns ' = omc_ext;']);
eval(['Tc_' ns ' = Tc_ext;']);
eval(['y_' ns ' = x_reproj;']);
eval(['X_' ns ' = X_ext;']);
eval(['x_' ns ' = x_ext;']);
eval(['ex_' ns '=x_ext - x_reproj;']);
eval(['n_sq_x_' ns ' = n_sq_x;']);
eval(['n_sq_y_' ns ' = n_sq_y;']);