ext_calib2.m

OPENCV系列的

%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('n_ima')|~exist('fc'),
   fprintf(1,'No calibration data available.\n');
   return;
end;

check_active_images;

if ~exist(['omc_' num2str(ind_active(1))]),
   fprintf(1,'No calibration data available.\n');
   return;
end;

%if ~exist('no_grid'),
   no_grid = 0;
%end;

if ~exist(['n_sq_x_' num2str(ind_active(1))]),
   no_grid = 1;
end;

if ~exist('alpha_c'),
   alpha_c = 0;
end;


if 0,

err_std = std(ex');

fprintf(1,'\n\nCalibration results without principal point estimation:\n\n');
fprintf(1,'Focal Length:     fc = [ %3.5f   %3.5f]\n',fc);
fprintf(1,'Principal point:  cc = [ %3.5f   %3.5f]\n',cc);
fprintf(1,'Distortion:       kc = [ %3.5f   %3.5f   %3.5f   %3.5f]\n',kc);   
fprintf(1,'Pixel error:      err = [ %3.5f   %3.5f]\n\n',err_std); 

end;


% Color code for each image:

colors = 'brgkcm';


%%% Show the extrinsic parameters

if ~exist('dX'),
   eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']);
   dY = dX;
end;

IP = 5*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1];
BASE = 5*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]);
IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15);
POS = [[6*dX;0;0] [0;6*dX;0] [-dX;0;5*dX] [-dX;-dX;-dX] [0;0;-dX]];


if ishandle(4),
	figure(4);
   [a,b] = view;
else
   figure(4);
   a = 50;
   b = 20;
end;

 
figure(4);
clf;
hold on;



for kk = 1:n_ima,
   
   if active_images(kk);
      
      if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]),
	 
	 eval(['XX_kk = X_' num2str(kk) ';']);

	 if ~isnan(XX_kk(1,1))
	    
	    eval(['omc_kk = omc_' num2str(kk) ';']);
	    eval(['Tc_kk = Tc_' num2str(kk) ';']);
	    N_kk = size(XX_kk,2);
	    
	    if ~exist(['n_sq_x_' num2str(kk)]),
	       no_grid = 1;
	    else
	       eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
	       if isnan(n_sq_x(1)),
		  no_grid = 1;
	       end;  
	    end;
	       
	    
	    if ~no_grid,
	       eval(['n_sq_x = n_sq_x_' num2str(kk) ';']);
	       eval(['n_sq_y = n_sq_y_' num2str(kk) ';']);
	       if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))),
		  no_grid = 1;
	       end;
	    end;
	    
       if ~isnan(omc_kk(1,1)),
          
          R_kk = rodrigues(omc_kk);
          
          BASEk = R_kk'*(BASE - Tc_kk * ones(1,6));
          IPk = R_kk'*(IP - Tc_kk * ones(1,15));
          POSk = R_kk'*(POS - Tc_kk * ones(1,5));
          
	       YY_kk = XX_kk;
	       	       
	       if ~no_grid,
		  YYx = zeros(n_sq_x+1,n_sq_y+1);
		  YYy = zeros(n_sq_x+1,n_sq_y+1);
		  YYz = zeros(n_sq_x+1,n_sq_y+1);
		  
		  YYx(:) = YY_kk(1,:);
		  YYy(:) = YY_kk(2,:);
		  YYz(:) = YY_kk(3,:);
		  
		  %keyboard;
        
        figure(4);
        plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1');
        plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1);
        %text(POSk(1,1),POSk(2,1),POSk(3,1),'X_c');
        %text(POSk(1,2),POSk(2,2),POSk(3,2),'Y_c');
        %text(POSk(1,3),POSk(2,3),POSk(3,3),'Z_c');
        %text(POSk(1,4),POSk(2,4),POSk(3,4),'O_c');
		  text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold');

		  hhh= mesh(YYx,YYy,YYz);
		  set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none');
	       else
		  
             figure(4);
             
        plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1');
        plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1);
        %text(POSk(1,1),POSk(2,1),POSk(3,1),'X_c');
        %text(POSk(1,2),POSk(2,2),POSk(3,2),'Y_c');
        %text(POSk(1,3),POSk(2,3),POSk(3,3),'Z_c');
        %text(POSk(1,4),POSk(2,4),POSk(3,4),'O_c');
		  hww = text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold');

		  plot3(YY_kk(1,:),YY_kk(2,:),YY_kk(3,:),['.' colors(rem(kk-1,6)+1)]);
		  
	       end;
            
	    end;
	 
	 end;
	 
      end;
      
   end;
   
end;

figure(4);rotate3d on;
axis('equal');
title('Extrinsic parameters');
%view(60,30);
xlabel('X_{world}')
ylabel('Y_{world}')
zlabel('Z_{world}')
view(a,b);
grid on;
hold off;

set(4,'Name','3D','NumberTitle','off');

%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n');