1  %init_intrinsic_param


2  %


3  %Initialization of the intrinsic parameters.


4  %Runs as a script.


5  %


6  %INPUT: x_1,x_2,x_3,...: Feature locations on the images


7  % X_1,X_2,X_3,...: Corresponding grid coordinates


8  %


9  %OUTPUT: fc: Camera focal length


10  % cc: Principal point coordinates


11  % kc: Distortion coefficients


12  % alpha_c: skew coefficient


13  % KK: The camera matrix (containing fc, cc and alpha_c)


14  %


15  %Method: Computes the planar homographies H_1, H_2, H_3, ... and computes


16  % the focal length fc from orthogonal vanishing points constraint.


17  % The principal point cc is assumed at the center of the image.


18  % Assumes no image distortion (kc = [0;0;0;0])


19  %


20  %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the


21  % corresponding entry in the active_images vector to zero.


22  %


23  %


24  %Important function called within that program:


25  %


26  %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.


27  %


28  %


29  %VERY IMPORTANT: This function works only with 2D rigs.


30  %In the future, a more general function will be there (working with 3D rigs as well).


31 


32 


33  if ~exist('two_focals_init'),


34  two_focals_init = 0;


35  end;


36 


37  if ~exist('est_aspect_ratio'),


38  est_aspect_ratio = 1;


39  end;


40 


41  check_active_images;


42 


43  if ~exist(['x_' num2str(ind_active(1)) ]),


44  click_calib;


45  end;


46 


47 


48  fprintf(1,'\nInitialization of the intrinsic parameters  Number of images: %d\n',length(ind_active));


49 


50 


51  % Initialize the homographies:


52 


53  for kk = 1:n_ima,


54  eval(['x_kk = x_' num2str(kk) ';']);


55  eval(['X_kk = X_' num2str(kk) ';']);


56  if (isnan(x_kk(1,1))),


57  if active_images(kk),


58  fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)


59  fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk)


60  end;


61  active_images(kk) = 0;


62  end;


63  if active_images(kk),


64  eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']);


65  else


66  eval(['H_' num2str(kk) ' = NaN*ones(3,3);']);


67  end;


68  end;


69 


70  check_active_images;


71 


72  % initial guess for principal point and distortion:


73 


74  if ~exist('nx'), [ny,nx] = size(I); end;


75 


76  c_init = [nx;ny]/2  0.5; % initialize at the center of the image


77  k_init = [0;0;0;0;0]; % initialize to zero (no distortion)


78 


79 


80 


81  % Compute explicitely the focal length using all the (mutually orthogonal) vanishing points


82  % note: The vanihing points are hidden in the planar collineations H_kk


83 


84  A = [];


85  b = [];


86 


87  % matrix that subtract the principal point:


88  Sub_cc = [1 0 c_init(1);0 1 c_init(2);0 0 1];


89 


90  for kk=1:n_ima,


91 


92  if active_images(kk),


93 


94  eval(['Hkk = H_' num2str(kk) ';']);


95 


96  Hkk = Sub_cc * Hkk;


97 


98  % Extract vanishing points (direct and diagonals):


99 


100  V_hori_pix = Hkk(:,1);


101  V_vert_pix = Hkk(:,2);


102  V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2;


103  V_diag2_pix = (Hkk(:,1)Hkk(:,2))/2;


104 


105  V_hori_pix = V_hori_pix/norm(V_hori_pix);


106  V_vert_pix = V_vert_pix/norm(V_vert_pix);


107  V_diag1_pix = V_diag1_pix/norm(V_diag1_pix);


108  V_diag2_pix = V_diag2_pix/norm(V_diag2_pix);


109 


110  a1 = V_hori_pix(1);


111  b1 = V_hori_pix(2);


112  c1 = V_hori_pix(3);


113 


114  a2 = V_vert_pix(1);


115  b2 = V_vert_pix(2);


116  c2 = V_vert_pix(3);


117 


118  a3 = V_diag1_pix(1);


119  b3 = V_diag1_pix(2);


120  c3 = V_diag1_pix(3);


121 


122  a4 = V_diag2_pix(1);


123  b4 = V_diag2_pix(2);


124  c4 = V_diag2_pix(3);


125 


126  A_kk = [a1*a2 b1*b2;


127  a3*a4 b3*b4];


128 


129  b_kk = [c1*c2;c3*c4];


130 


131 


132  A = [A;A_kk];


133  b = [b;b_kk];


134 


135  end;


136 


137  end;


138 


139 


140  % use all the vanishing points to estimate focal length:


141 


142 


143  % Select the model for the focal. (solution to Gerd's problem)


144  if ~two_focals_init


145  if b'*(sum(A')') < 0,


146  two_focals_init = 1;


147  end;


148  end;


149 


150 


151 


152  if two_focals_init


153  % Use a two focals estimate:


154  f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a twofocal model for initial guess


155  else


156  % Use a single focal estimate:


157  f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used


158  end;


159 


160 


161  if ~est_aspect_ratio,


162  f_init(1) = (f_init(1)+f_init(2))/2;


163  f_init(2) = f_init(1);


164  end;


165 


166  alpha_init = 0;


167 


168  %f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used


169 


170 


171  % Global calibration matrix (initial guess):


172 


173  KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1];


174  inv_KK = inv(KK);


175 


176 


177  cc = c_init;


178  fc = f_init;


179  kc = k_init;


180  alpha_c = alpha_init;


181 


182 


183  fprintf(1,'\n\nCalibration parameters after initialization:\n\n');


184  fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc);


185  fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc);


186  fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90  atan(alpha_c)*180/pi);


187  fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',kc);

