calib_stereo.m

经典的张正友的摄像机的两步标定程序

% calib_stereo
% Script for Calibrating a stereo rig (two cameras, internal and external calibration):
%
% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points
% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras.
% The two calibration result files must have been saved under Calib_Results_left.mat and Calib_Results_right.mat prior to running this script.
% This script is not fully documented, therefore use it at your own risks. However, it should be rather straighforward to run.
%
% INPUT: Calib_Results_left.mat, Calib_Results_right.mat -> Generated by the standard calibration toolbox on the two cameras individually)
% OUTPUT: Calib_Results_stereo.mat -> The saved result after global stereo calibration
% 
% Main result variables stored in Calib_Results_stereo.mat:
% om, R, T: relative rotation and translation of the right camera wrt the left camera
% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera
% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera
% 
% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at:
% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html
%
% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set
% recompute_intrinsic_right and recompute_intrinsic_left to zero. Default: 1
%
% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)).
% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively.
% XL and XR are related to each other through the following rigid motion transformation:
% XR = R * XL + T
% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras.
%
%
% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered, that probably
% means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two
% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration.
% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images.
% The pattwern can (and should) be moved in space only between two sets of (left,right) images.
% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations
% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the
% two separate calibration. In other words, the points need to correspond.

% (c) Jean-Yves Bouguet - Intel Corporation
% October 25, 2001 -- Last updated April 12, 2002


fprintf(1,'\n\nStereo Calibration script (for more info, try help calib_stereo)\n\n');

if (exist('Calib_Results_left.mat')~=2)|(exist('Calib_Results_right.mat')~=2),
    fprintf(1,'Error: Need the left and right calibration files Calib_Results_left.mat and Calib_Results_right.mat to run stereo calibration\n');
    return;
end;

if ~exist('recompute_intrinsic_right'),
    recompute_intrinsic_right = 1;
end;


if ~exist('recompute_intrinsic_left'),
    recompute_intrinsic_left = 1;
end;

fprintf(1,'Loading the left camera calibration result file Calib_Results_left.mat...\n');

clear calib_name

load Calib_Results_left;

fc_left = fc;
cc_left = cc;
kc_left = kc;
alpha_c_left = alpha_c;
KK_left = KK;

if exist('calib_name'),
    calib_name_left = calib_name;
    format_image_left = format_image;
    type_numbering_left = type_numbering;
    image_numbers_left = image_numbers;
    N_slots_left = N_slots;
else
    calib_name_left = '';
    format_image_left = '';
    type_numbering_left = '';
    image_numbers_left = '';
    N_slots_left = '';
end;

    
X_left = [];


om_left_list = [];
T_left_list = [];

for kk = 1:n_ima,
   
   if active_images(kk),
      
      eval(['Xkk = X_' num2str(kk) ';']);
      eval(['omckk = omc_' num2str(kk) ';']);
      eval(['Rckk = Rc_' num2str(kk) ';']);
      eval(['Tckk = Tc_' num2str(kk) ';']);
      
      N = size(Xkk,2);
      
      Xckk = Rckk * Xkk  + Tckk*ones(1,N);
      
      X_left = [X_left Xckk];

      om_left_list = [om_left_list omckk];
      
      T_left_list = [T_left_list Tckk];
      
  end;
end;



fprintf(1,'Loading the right camera calibration result file Calib_Results_right.mat...\n');

clear calib_name

load Calib_Results_right;

fc_right = fc;
cc_right = cc;
kc_right = kc;
alpha_c_right = alpha_c;
KK_right = KK;

if exist('calib_name'),
    calib_name_right = calib_name;
    format_image_right = format_image;
    type_numbering_right = type_numbering;
    image_numbers_right = image_numbers;
    N_slots_right = N_slots;
else
    calib_name_right = '';
    format_image_right = '';
    type_numbering_right = '';
    image_numbers_right = '';
    N_slots_right = '';
end;

X_right = [];

om_right_list = [];
T_right_list = [];

for kk = 1:n_ima,
   
   if active_images(kk),
      
      eval(['Xkk = X_' num2str(kk) ';']);
      eval(['omckk = omc_' num2str(kk) ';']);
      eval(['Rckk = Rc_' num2str(kk) ';']);
      eval(['Tckk = Tc_' num2str(kk) ';']);
      
      N = size(Xkk,2);
      
      Xckk = Rckk * Xkk  + Tckk*ones(1,N);
      
      X_right = [X_right Xckk];
      
      om_right_list = [om_right_list omckk];
      T_right_list = [T_right_list Tckk];
      
   end;
end;




om_ref_list = [];
T_ref_list = [];
for ii = 1:length(om_left_list),
    % Align the structure from the first view:
    R_ref = rodrigues(om_right_list(:,ii)) * rodrigues(om_left_list(:,ii))';
    T_ref = T_right_list(:,ii) - R_ref * T_left_list(:,ii);
    om_ref = rodrigues(R_ref);
    om_ref_list = [om_ref_list om_ref];
    T_ref_list = [T_ref_list T_ref];
end;


% Robust estimate of the initial value for rotation and translation between the two views:
om = median(om_ref_list')';
T = median(T_ref_list')';




if 0,
    figure(10);
    plot3(X_right(1,:),X_right(2,:),X_right(3,:),'bo');
    hold on;
    [Xr2] = rigid_motion(X_left,om,T);
    plot3(Xr2(1,:),Xr2(2,:),Xr2(3,:),'r+');
    hold off;
    drawnow;
end;


R = rodrigues(om);



% Re-optimize now over all the set of extrinsic unknows (global optimization) and intrinsic parameters:

load Calib_Results_left;

for kk = 1:n_ima,
   if active_images(kk),
      eval(['X_left_'  num2str(kk) ' = X_' num2str(kk) ';']);
      eval(['x_left_'  num2str(kk) ' = x_' num2str(kk) ';']);
      eval(['omc_left_' num2str(kk) ' = omc_' num2str(kk) ';']);
      eval(['Rc_left_' num2str(kk) ' = Rc_' num2str(kk) ';']);
      eval(['Tc_left_' num2str(kk) ' = Tc_' num2str(kk) ';']);
   end;
end;

center_optim_left = center_optim;
est_alpha_left = est_alpha;
est_dist_left = est_dist;
est_fc_left = est_fc;
est_aspect_ratio_left = est_aspect_ratio;
active_images_left = active_images;


load Calib_Results_right;

for kk = 1:n_ima,
   if active_images(kk),
      eval(['X_right_'  num2str(kk) ' = X_' num2str(kk) ';']);
      eval(['x_right_'  num2str(kk) ' = x_' num2str(kk) ';']);
   end;
end;

center_optim_right = center_optim;
est_alpha_right = est_alpha;
est_dist_right = est_dist;
est_fc_right = est_fc;
est_aspect_ratio_right = est_aspect_ratio;
active_images_right = active_images;




if ~recompute_intrinsic_left,
    fprintf(1,'No recomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 0)\n');
    center_optim_left = 0;
    est_alpha_left = 0;
    est_dist_left = zeros(5,1);
    est_fc_left = [0;0];
    est_aspect_ratio_left = 1; % just to fix conflicts
end;