cam_proj_calib.m

这是一个很好的标定工具箱

%%% This code is an additional code that helps doing projector calibration in 3D scanning setup.
%%% This is not a useful code for anyone else but me.
%%% I included it in the toolbox for illustration only.


fprintf(1,'3D scanner calibration code\n');
fprintf(1,'(c) Jean-Yves Bouguet - August 2000\n');
fprintf(1,'Intel Corporation\n');


if ~exist('camera_results.mat'),
   if exist('Calib_Results.mat'),
      copyfile('Calib_Results.mat','camera_results.mat');
      delete('Calib_Results.mat');
   else
      disp('ERROR: Need to calibrate the camera first, save results, and run cam_proj_calib');
      break;
   end;
end;



if 0, % If I want to run camera calibration again
   load camera_results;
   % Do estimate distortion:
   est_dist = ones(5,1);
   est_alpha = 1;
   center_optim = 1;
   % Run the main calibration routine:
   go_calib_optim;
   saving_calib;
   copyfile('Calib_Results.mat','camera_results.mat');
   delete('Calib_Results.mat');
end;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% START THE MAIN PROCEDURE %%%%%%%%%%%%%%%%%%%%%%%%%%%

load camera_results;
param = solution;

% Save camera parameters:
fc_save  = fc;
cc_save = cc;
kc_save = kc;
alpha_c_save = alpha_c;

omc_1_save = omc_1;
Rc_1_save = Rc_1;
Tc_1_save = Tc_1;

clear fc cc kc alpha_c


param_cam = param([1:10 16:end]);


% Extract projector data?
if ~exist('projector_data.mat'),
	projector_calib; % extract the projector corners (all the data)
else
   load projector_data; % load the projector corners (previously saved)
end;



% Start projector calibration:

X_proj = [];
x_proj = [];
n_ima_proj = [];

for kk = ind_active,
   eval(['xproj = xproj_' num2str(kk) ';']);
   xprojn = normalize(xproj,fc_save,cc_save,kc_save,alpha_c_save);
   eval(['Rc = Rc_' num2str(kk) ';']);
   eval(['Tc = Tc_' num2str(kk) ';']);
   
   Np_proj = size(xproj,2);
	Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)])));
	Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame
   eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 
   eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']);
   eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']);
   n_ima_proj = [n_ima_proj kk*ones(1,Np_proj)];
end;

% Image size: (may or may not be available)
nx = 1024;
ny = 768;

% No calibration image is available (only the corner coordinates)
no_image = 1;

n_ima_save = n_ima;
X_1_save = X_1;
x_1_save = x_1;
dX_save = dX;
dY_save = dY;

n_ima = 1;
X_1 = X_proj;
x_1 = x_proj;

% Set the toolbox not to prompt the user (choose default values)
dont_ask = 1;

% Do estimate distortion:
est_dist = ones(5,1);
est_alpha = 1;
center_optim = 1;


% Run the main calibration routine:
clear fc kc cc alpha_c KK
go_calib_optim;
go_calib_optim;

param = solution;

param_proj = param([1:10 16:end]);

% Shows the extrinsic parameters:
dX = 30;
dY = 30;
ext_calib;

% Reprojection on the original images:
reproject_calib;
%saving_calib;
%copyfile([save_name '.mat'],'projector_results.mat');


saving_calib;

copyfile('Calib_Results.mat','projector_results.mat');
delete('Calib_Results.mat');


n_ima = n_ima_save;
X_1 = X_1_save;
x_1  = x_1_save;
no_image = 0;
dX = dX_save;
dY = dY_save;

%----------------------- Retrieve results:

% Intrinsic:

% Projector:
fp = fc;
cp = cc;
kp = kc;
alpha_p = alpha_c;

% Camera:
fc = fc_save;
cc = cc_save;
kc = kc_save;
alpha_c = alpha_c_save;


% Extrinsic:

% Relative position of projector and camera:
T = Tc_1;
om = omc_1;
R = rodrigues(om);

% Relative prosition of camera wrt world:
omc = omc_1_save;
Rc = Rc_1_save;
Tc = Tc_1_save;

% relative position of projector wrt world:
Rp = R*Rc;
omp = rodrigues(Rp);
Tp = T + R*Tc;

eval(['save calib_cam_proj  R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp']);


global_optim = 2;

fprintf(1,'A final global optimization (minimizing reprojection errors on both camera and projector images)\n');
fprintf(1,'may be desirable for better results. This procedure may take a while to execute. \n');
   
while (global_optim~=0)&(global_optim~=1),

	global_optim = input('Do you want to run a global optimization? (0=[]=no, 1=yes) ');
   
   if isempty(global_optim),
      global_optim = 0;
   end;
   
   if (global_optim~=0)&(global_optim~=1),
      fprintf(1,'Bad entry. Try again.\n');
   end;   
end;


if global_optim,

	%----------------- global optimization: ---------------------

   load projector_data; % load the projector corners (previously saved)
   load projector_results;
   
   param_proj = solution([1:10 16:end]);
   
   load camera_results;
   
   param_cam = solution([1:10 16:end]);
   
   param = [param_cam;param_proj];
   

   string_global = 'global n_ima';
   for kk = 1:n_ima,
      string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)];
   end;
   eval(string_global);   
   
   % test the global error function:
   e_global = error_cam_proj(param);
   
   param_init = param;
   
   options = [1 1e-4 1e-4 1e-6  0 0 0 0 0 0 0 0 0 6000 0 1e-8 0.1 0];
   
   param = leastsq('error_cam_proj',param,options);
   
   % Computation of the errors:
   
   fc = param(1:2);
   cc = param(3:4);
   alpha_c = param(5);
   kc = param(6:10);
   
   e_cam = [];
   
   for kk = 1:n_ima,
      omckk = param(11+(kk-1)*6:11+(kk-1)*6+2);
      Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2);
      
      eval(['Xkk = X_' num2str(kk) ';']);
      eval(['xkk = x_' num2str(kk) ';']);
      
      ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c);
      
      Rckk = rodrigues(omckk);
      eval(['omc_' num2str(kk) '= omckk;']);
      eval(['Tc_' num2str(kk) '= Tckk;']);
      eval(['Rc_' num2str(kk) '= Rckk;']);
      
      e_cam = [e_cam ekk];
      
   end;
   
   X_proj = [];
   x_proj = [];
   
   for kk = 1:n_ima,
      eval(['xproj = xproj_' num2str(kk) ';']);
      xprojn = normalize(xproj,fc,cc,kc,alpha_c);
      eval(['Rc = Rc_' num2str(kk) ';']);
      eval(['Tc = Tc_' num2str(kk) ';']);   
      Np_proj = size(xproj,2);
      Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)])));
      Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame
      eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 
      eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']);
      eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']);
   end;
   
   
   fp = param((1:2)+n_ima * 6 + 10);
   cp = param((3:4)+n_ima * 6 + 10);
   alpha_p = param((5)+n_ima * 6 + 10);
   kp = param((6:10)+n_ima * 6 + 10);
   
   om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2);
   T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2);
   R = rodrigues(om);
   
   
   e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p);
   
   
   e_global = [e_cam e_proj];
   
   
   
   
   % Relative prosition of camera wrt world:
   omc = omc_1;
   Rc = Rc_1;
   Tc = Tc_1;
   
   % relative position of projector wrt world:
   Rp = R*Rc;
   omp = rodrigues(Rp);
   Tp = T + R*Tc;
   
   
   eval(['save calib_cam_proj_optim  R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp param param_init']);
   
   no_image = 0;
  	% Image size: (may or may not be available)
	nx = 640;
	ny = 480;
   
   comp_error_calib;
   
   % Save the optimal camera parameters:
   saving_calib;
   copyfile('Calib_Results.mat','camera_results_optim.mat');
   delete('Calib_Results.mat');
   
   % Save the optimal camera parameters:
	fc = fp;
   cc = cp;
   alpha_c = alpha_p;
   kc = kp;
   
   n_ima = 1;
   X_1 = X_proj;
   x_1 = x_proj;
   omc_1 = om;
   Tc_1 = T;
   Rc_1 = R;
   
	% Image size: (may or may not be available)
	nx = 1024;
	ny = 768;
	
	% No calibration image is available (only the corner coordinates)
	no_image = 1;
   
   comp_error_calib;
   
   saving_calib;
	copyfile('Calib_Results.mat','projector_results_optim.mat');
	delete('Calib_Results.mat');

else
   
   fprintf(1,'No global optimization performed. You may want to do it later.\n');
   
end;