1  %=======================================================================


2  % Copyright 20082015, LEGI UMR 5519 / CNRS UJF GINP, Grenoble, France


3  % http://www.legi.grenobleinp.fr


4  % Joel.Sommeria  Joel.Sommeria (A) legi.cnrs.fr


5  %


6  % This file is part of the toolbox UVMAT.


7  %


8  % UVMAT is free software; you can redistribute it and/or modify


9  % it under the terms of the GNU General Public License as published


10  % by the Free Software Foundation; either version 2 of the license,


11  % or (at your option) any later version.


12  %


13  % UVMAT is distributed in the hope that it will be useful,


14  % but WITHOUT ANY WARRANTY; without even the implied warranty of


15  % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the


16  % GNU General Public License (see LICENSE.txt) for more details.


17  %=======================================================================


18 


19  %init_intrinsic_param


20  %


21  %Initialization of the intrinsic parameters.


22  %Runs as a script.


23  %


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


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


26  %


27  %OUTPUT: fc: Camera focal length


28  % cc: Principal point coordinates


29  % kc: Distortion coefficients


30  % alpha_c: skew coefficient


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


32  %


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


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


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


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


37  %


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


39  % corresponding entry in the active_images vector to zero.


40  %


41  %


42  %Important function called within that program:


43  %


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


45  %


46  %


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


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


49 


50 


51  if ~exist('two_focals_init'),


52  two_focals_init = 0;


53  end;


54 


55  if ~exist('est_aspect_ratio'),


56  est_aspect_ratio = 1;


57  end;


58 


59  check_active_images;


60 


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


62  click_calib;


63  end;


64 


65 


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


67 


68 


69  % Initialize the homographies:


70 


71  for kk = 1:n_ima,


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


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


74  if (isnan(x_kk(1,1))),


75  if active_images(kk),


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


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


78  end;


79  active_images(kk) = 0;


80  end;


81  if active_images(kk),


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


83  else


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


85  end;


86  end;


87 


88  check_active_images;


89 


90  % initial guess for principal point and distortion:


91 


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


93 


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


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


96 


97 


98 


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


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


101 


102  A = [];


103  b = [];


104 


105  % matrix that subtract the principal point:


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


107 


108  for kk=1:n_ima,


109 


110  if active_images(kk),


111 


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


113 


114  Hkk = Sub_cc * Hkk;


115 


116  % Extract vanishing points (direct and diagonals):


117 


118  V_hori_pix = Hkk(:,1);


119  V_vert_pix = Hkk(:,2);


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


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


122 


123  V_hori_pix = V_hori_pix/norm(V_hori_pix);


124  V_vert_pix = V_vert_pix/norm(V_vert_pix);


125  V_diag1_pix = V_diag1_pix/norm(V_diag1_pix);


126  V_diag2_pix = V_diag2_pix/norm(V_diag2_pix);


127 


128  a1 = V_hori_pix(1);


129  b1 = V_hori_pix(2);


130  c1 = V_hori_pix(3);


131 


132  a2 = V_vert_pix(1);


133  b2 = V_vert_pix(2);


134  c2 = V_vert_pix(3);


135 


136  a3 = V_diag1_pix(1);


137  b3 = V_diag1_pix(2);


138  c3 = V_diag1_pix(3);


139 


140  a4 = V_diag2_pix(1);


141  b4 = V_diag2_pix(2);


142  c4 = V_diag2_pix(3);


143 


144  A_kk = [a1*a2 b1*b2;


145  a3*a4 b3*b4];


146 


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


148 


149 


150  A = [A;A_kk];


151  b = [b;b_kk];


152 


153  end;


154 


155  end;


156 


157 


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


159 


160 


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


162  if ~two_focals_init


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


164  two_focals_init = 1;


165  end;


166  end;


167 


168 


169 


170  if two_focals_init


171  % Use a two focals estimate:


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


173  else


174  % Use a single focal estimate:


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


176  end;


177 


178 


179  if ~est_aspect_ratio,


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


181  f_init(2) = f_init(1);


182  end;


183 


184  alpha_init = 0;


185 


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


187 


188 


189  % Global calibration matrix (initial guess):


190 


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


192  inv_KK = inv(KK);


193 


194 


195  cc = c_init;


196  fc = f_init;


197  kc = k_init;


198  alpha_c = alpha_init;


199 


200 


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


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


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


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


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

