[810]  1  %=======================================================================


[924]  2  % Copyright 20082016, LEGI UMR 5519 / CNRS UGA GINP, Grenoble, France


[810]  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 


[725]  19  %go_calib_optim_iter


 20  %


 21  %Main calibration function. Computes the intrinsic andextrinsic 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  % alpha_c: Skew coefficient


 30  % kc: Distortion coefficients


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


 32  % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space


 33  % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space


 34  % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors


 35  %


 36  %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic


 37  % camera parameters, and the extrinsic parameters (3D locations of the grids in space)


 38  %


 39  %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through


 40  % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.


 41  %


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


 43  % corresponding entry in the active_images vector to zero.


 44  %


 45  %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m


 46  %that is so far implemented to work only with 2D rigs.


 47  %In the future, a more general function will be there.


 48  %For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length


 49 


 50  if ~exist('desactivated_images'),


 51  desactivated_images = [];


 52  end;


 53 


 54 


 55 


 56  if ~exist('est_aspect_ratio'),


 57  est_aspect_ratio = 1;


 58  end;


 59 


 60  if ~exist('est_fc');


 61  est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!)


 62  end;


 63 


 64  if ~exist('recompute_extrinsic'),


 65  recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters


 66  % at each iterstion.


 67  end;


 68 


 69  if ~exist('MaxIter'),


 70  MaxIter = 30; % Maximum number of iterations in the gradient descent


 71  end;


 72 


 73  if ~exist('check_cond'),


 74  check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically


 75  end;


 76 


 77  if ~exist('center_optim'),


 78  center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point


 79  end;


 80 


 81  if exist('est_dist'),


 82  if length(est_dist) == 4,


 83  est_dist = [est_dist ; 0];


 84  end;


 85  end;


 86 


 87  if ~exist('est_dist'),


 88  est_dist = [1;1;1;1;0];


 89  end;


 90 


 91  if ~exist('est_alpha'),


 92  est_alpha = 0; % by default, do not estimate skew


 93  end;


 94 


 95 


 96  % Little fix in case of stupid values in the binary variables:


 97  center_optim = double(~~center_optim);


 98  est_alpha = double(~~est_alpha);


 99  est_dist = double(~~est_dist);


 100  est_fc = double(~~est_fc);


 101  est_aspect_ratio = double(~~est_aspect_ratio);


 102 


 103 


 104 


 105  fprintf(1,'\n');


 106 


 107  if ~exist('nx')&~exist('ny'),


 108  fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n');


 109  nx = 640;


 110  ny = 480;


 111  end;


 112 


 113 


 114  check_active_images;


 115 


 116  quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)


 117 


 118 


 119  % Check 3Dness of the calibration rig:


 120  rig3D = 0;


 121  for kk = ind_active,


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


 123  if is3D(X_kk),


 124  rig3D = 1;


 125  end;


 126  end;


 127 


 128 


 129  if center_optim & (length(ind_active) < 2) & ~rig3D,


 130  fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n');


 131  center_optim = 0; %%% when using a single image, please, no principal point estimation!!!


 132  est_alpha = 0;


 133  end;


 134 


 135  if ~exist('dont_ask'),


 136  dont_ask = 0;


 137  end;


 138 


 139  if center_optim & (length(ind_active) < 5) & ~rig3D,


 140  fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n');


 141  %if ~dont_ask,


 142  % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) ');


 143  % center_optim = isempty(quest);


 144  %end;


 145  end;


 146 


 147 


 148  % A quick fix for solving conflict


 149  if ~isequal(est_fc,[1;1]),


 150  est_aspect_ratio=1;


 151  end;


 152  if ~est_aspect_ratio,


 153  est_fc=[1;1];


 154  end;


 155 


 156 


 157  if ~est_aspect_ratio,


 158  fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) > fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n');


 159  else


 160  if isequal(est_fc,[1;1]),


 161  fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) > both components of fc are estimated (DEFAULT).\n');


 162  end;


 163  end;


 164 


 165  if ~isequal(est_fc,[1;1]),


 166  if isequal(est_fc,[1;0]),


 167  fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n');


 168  else


 169  if isequal(est_fc,[0;1]),


 170  fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n');


 171  else


 172  fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n');


 173  end;


 174  end;


 175  end;


 176 


 177 


 178  if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image


 179  fprintf(1,'Principal point not optimized (center_optim=0). ');


 180  if ~exist('cc'),


 181  fprintf(1,'It is kept at the center of the image.\n');


 182  cc = [(nx1)/2;(ny1)/2];


 183  else


 184  fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n');


 185  end;


 186  else


 187  fprintf(1,'Principal point optimized (center_optim=1)  (DEFAULT). To reject principal point, set center_optim=0\n');


 188  end;


 189 


 190 


 191  if ~center_optim & (est_alpha),


 192  fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n');


 193  est_alpha = 0;


 194  end;


 195 


 196  if ~est_alpha,


 197  fprintf(1,'Skew not optimized (est_alpha=0)  (DEFAULT)\n');


 198  alpha_c = 0;


 199  else


 200  fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n');


 201  end;


 202 


 203 


 204  if ~prod(double(est_dist)),


 205  fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n');


 206  if ~est_dist(1),


 207  fprintf(1,' Second order distortion not estimated (est_dist(1)=0).\n');


 208  end;


 209  if ~est_dist(2),


 210  fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0).\n');


 211  end;


 212  if ~est_dist(5),


 213  fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0)  (DEFAULT) .\n');


 214  end;


 215  if ~prod(double(est_dist(3:4))),


 216  fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n');


 217  end;


 218  end;


 219 


 220 


 221  % Check 3Dness of the calibration rig:


 222  rig3D = 0;


 223  for kk = ind_active,


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


 225  if is3D(X_kk),


 226  rig3D = 1;


 227  end;


 228  end;


 229 


 230  % If the rig is 3D, then no choice: the only valid initialization is manual!


 231  if rig3D,


 232  quick_init = 1;


 233  end;


 234 


 235 


 236 


 237  alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent


 238  % alpha_smooth = 0.01; % modified L. Gostiaux


 239 


 240 


 241  % Conditioning threshold for view rejection


 242  thresh_cond = 1e5;


 243 


 244 


 245 


 246  % Initialization of the intrinsic parameters (if necessary)


 247 


 248  if ~exist('cc'),


 249  fprintf(1,'Initialization of the principal point at the center of the image.\n');


 250  cc = [(nx1)/2;(ny1)/2];


 251  alpha_smooth = 0.1; % slow convergence


 252  end;


 253 


 254 


 255  if exist('kc'),


 256  if length(kc) == 4;


 257  fprintf(1,'Adding a new distortion coefficient to kc > radial distortion model up to the 6th degree');


 258  kc = [kc;0];


 259  end;


 260  end;


 261 


 262 


 263 


 264  if ~exist('alpha_c'),


 265  fprintf(1,'Initialization of the image skew to zero.\n');


 266  alpha_c = 0;


 267  alpha_smooth = 0.1; % slow convergence


 268  end;


 269 


 270  if ~exist('fc')& quick_init,


 271  FOV_angle = 35; % Initial camera field of view in degrees


 272  fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']);


 273  fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1);


 274  est_fc = [1;1];


 275  alpha_smooth = 0.1; % slow


 276  end;


 277 


 278 


 279  if ~exist('fc'),


 280  % Initialization of the intrinsic parameters:


 281  fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n')


 282  init_intrinsic_param; % The right way to go (if quick_init is not active)!


 283  alpha_smooth = 0.1; % slow convergence


 284  est_fc = [1;1];


 285  end;


 286 


 287 


 288  if ~exist('kc'),


 289  fprintf(1,'Initialization of the image distortion to zero.\n');


 290  kc = zeros(5,1);


 291  alpha_smooth = 0.1; % slow convergence


 292  end;


 293 


 294  if ~est_aspect_ratio,


 295  fc(1) = (fc(1)+fc(2))/2;


 296  fc(2) = fc(1);


 297  end;


 298 


 299  if ~prod(double(est_dist)),


 300  % If no distortion estimated, set to zero the variables that are not estimated


 301  kc = kc .* est_dist;


 302  end;


 303 


 304 


 305  if ~prod(double(est_fc)),


 306  fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n');


 307  end;


 308 


 309 


 310  %%% Initialization of the extrinsic parameters for global minimization:


 311  comp_ext_calib;


 312 


 313 


 314 


 315  %%% Initialization of the global parameter vector:


 316 


 317  init_param = [fc;cc;alpha_c;kc;zeros(5,1)];


 318 


 319  for kk = 1:n_ima,


 320  eval(['omckk = omc_' num2str(kk) ';']);


 321  eval(['Tckk = Tc_' num2str(kk) ';']);


 322  init_param = [init_param; omckk ; Tckk];


 323  end;


 324 


 325 


 326 


 327  % Main Optimization:


 328 


 329  fprintf(1,'\nMain calibration optimization procedure  Number of images: %d\n',length(ind_active));


 330 


 331 


 332  param = init_param;


 333  change = 1;


 334 


 335  iter = 0;


 336 


 337  fprintf(1,'Gradient descent iterations: ');


 338 


 339  param_list = param;


 340 


 341 


 342  while (change > 1e2)&(iter < MaxIter),


 343 


 344  fprintf(1,'%d...',iter+1);


 345 


 346  % To speed up: preallocate the memory for the Jacobian JJ3.


 347  % For that, need to compute the total number of points.


 348 


 349  %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active


 350  %% images) through a one step steepest gradient descent.


 351 


 352 


 353  f = param(1:2);


 354  c = param(3:4);


 355  alpha = param(5);


 356  k = param(6:10);


 357 


 358 


 359  % Compute the size of the Jacobian matrix:


 360  N_points_views_active = N_points_views(ind_active);


 361 


 362  JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);


 363  ex3 = zeros(15 + 6*n_ima,1);


 364 


 365 


 366  for kk = ind_active, %1:n_ima,


 367  %if active_images(kk),


 368 


 369  omckk = param(15+6*(kk1) + 1:15+6*(kk1) + 3);


 370 


 371  Tckk = param(15+6*(kk1) + 4:15+6*(kk1) + 6);


 372 


 373  if isnan(omckk(1)),


 374  fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);


 375  return;


 376  end;


 377 


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


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


 380 


 381  Np = N_points_views(kk);


 382 


 383  if ~est_aspect_ratio,


 384  [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha);


 385  dxdf = repmat(dxdf,[1 2]);


 386  else


 387  [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha);


 388  end;


 389 


 390  exkk = x_kk  x;


 391 


 392  A = [dxdf dxdc dxdalpha dxdk]';


 393  B = [dxdom dxdT]';


 394 


 395  JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');


 396  JJ3(15+6*(kk1) + 1:15+6*(kk1) + 6,15+6*(kk1) + 1:15+6*(kk1) + 6) = sparse(B*B');


 397 


 398  AB = sparse(A*B');


 399  JJ3(1:10,15+6*(kk1) + 1:15+6*(kk1) + 6) = AB;


 400  JJ3(15+6*(kk1) + 1:15+6*(kk1) + 6,1:10) = (AB)';


 401 


 402  ex3(1:10) = ex3(1:10) + A*exkk(:);


 403  ex3(15+6*(kk1) + 1:15+6*(kk1) + 6) = B*exkk(:);


 404 


 405  % Check if this view is illconditioned:


 406  if check_cond,


 407  JJ_kk = B'; %[dxdom dxdT];


 408  if (cond(JJ_kk)> thresh_cond),


 409  active_images(kk) = 0;


 410  fprintf(1,'\nWarning: View #%d illconditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk)


 411  desactivated_images = [desactivated_images kk];


 412  param(15+6*(kk1) + 1:15+6*(kk1) + 6) = NaN*ones(6,1);


 413  end;


 414  end;


 415 


 416  %end;


 417 


 418  end;


 419 


 420 


 421  % List of active images (necessary if changed):


 422  check_active_images;


 423 


 424 


 425  % The following vector helps to select the variables to update (for only active images):


 426  selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)];


 427  if ~est_aspect_ratio,


 428  if isequal(est_fc,[1;1])  isequal(est_fc,[1;0]),


 429  selected_variables(2) = 0;


 430  end;


 431  end;


 432  ind_Jac = find(selected_variables)';


 433 


 434  JJ3 = JJ3(ind_Jac,ind_Jac);


 435  ex3 = ex3(ind_Jac);


 436 


 437  JJ2_inv = inv(JJ3); % not bad for sparse matrices!!


 438 


 439 


 440  % Smoothing coefficient:


 441 


 442  alpha_smooth2 = 1(1alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!


 443 


 444  param_innov = alpha_smooth2*JJ2_inv*ex3;


 445 


 446 


 447  param_up = param(ind_Jac) + param_innov;


 448  param(ind_Jac) = param_up;


 449 


 450 


 451  % New intrinsic parameters:


 452 


 453  fc_current = param(1:2);


 454  cc_current = param(3:4);


 455 


 456  if center_optim & ((param(3)<0)(param(3)>nx)(param(4)<0)(param(4)>ny)),


 457  fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n');


 458  center_optim = 0;


 459  cc_current = c;


 460  else


 461  cc_current = param(3:4);


 462  end;


 463 


 464  alpha_current = param(5);


 465  kc_current = param(6:10);


 466 


 467  if ~est_aspect_ratio & isequal(est_fc,[1;1]),


 468  fc_current(2) = fc_current(1);


 469  param(2) = param(1);


 470  end;


 471 


 472  % Change on the intrinsic parameters:


 473  change = norm([fc_current;cc_current]  [f;c])/norm([fc_current;cc_current]);


 474 


 475 


 476  %% Second step: (optional)  It makes convergence faster, and the region of convergence LARGER!!!


 477  %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)


 478  %% The complete gradient descent method is useful to precisely update the intrinsic parameters.


 479 


 480 


 481  if recompute_extrinsic,


 482  MaxIter2 = 20;


 483  for kk =ind_active, %1:n_ima,


 484  %if active_images(kk),


 485  omc_current = param(15+6*(kk1) + 1:15+6*(kk1) + 3);


 486  Tc_current = param(15+6*(kk1) + 4:15+6*(kk1) + 6);


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


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


 489  [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);


 490  [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond);


 491  if check_cond,


 492  if (cond(JJ_kk)> thresh_cond),


 493  active_images(kk) = 0;


 494  fprintf(1,'\nWarning: View #%d illconditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk);


 495  desactivated_images = [desactivated_images kk];


 496  omckk = NaN*ones(3,1);


 497  Tckk = NaN*ones(3,1);


 498  end;


 499  end;


 500  param(15+6*(kk1) + 1:15+6*(kk1) + 3) = omckk;


 501  param(15+6*(kk1) + 4:15+6*(kk1) + 6) = Tckk;


 502  %end;


 503  end;


 504  end;


 505 


 506  param_list = [param_list param];


 507  iter = iter + 1;


 508 


 509  end;


 510 


 511  fprintf(1,'done\n');


 512 


 513 


 514 


 515  %%% Computation of the error of estimation:


 516 


 517  fprintf(1,'Estimation of uncertainties...');


 518 


 519 


 520  check_active_images;


 521 


 522  solution = param;


 523 


 524 


 525  % Extraction of the paramters for computing the right reprojection error:


 526 


 527  fc = solution(1:2);


 528  cc = solution(3:4);


 529  alpha_c = solution(5);


 530  kc = solution(6:10);


 531 


 532  for kk = 1:n_ima,


 533 


 534  if active_images(kk),


 535 


 536  omckk = solution(15+6*(kk1) + 1:15+6*(kk1) + 3);%***


 537  Tckk = solution(15+6*(kk1) + 4:15+6*(kk1) + 6);%***


 538  Rckk = rodrigues(omckk);


 539 


 540  else


 541 


 542  omckk = NaN*ones(3,1);


 543  Tckk = NaN*ones(3,1);


 544  Rckk = NaN*ones(3,3);


 545 


 546  end;


 547 


 548  eval(['omc_' num2str(kk) ' = omckk;']);


 549  eval(['Rc_' num2str(kk) ' = Rckk;']);


 550  eval(['Tc_' num2str(kk) ' = Tckk;']);


 551 


 552  end;


 553 


 554 


 555  % Recompute the error (in the vector ex):


 556  comp_error_calib;


 557 


 558  sigma_x = std(ex(:));


 559 


 560  % Compute the size of the Jacobian matrix:


 561  N_points_views_active = N_points_views(ind_active);


 562 


 563  JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);


 564 


 565  for kk = ind_active,


 566 


 567  omckk = param(15+6*(kk1) + 1:15+6*(kk1) + 3);


 568  Tckk = param(15+6*(kk1) + 4:15+6*(kk1) + 6);


 569 


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


 571 


 572  Np = N_points_views(kk);


 573 


 574  %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);


 575 


 576  if ~est_aspect_ratio,


 577  [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c);


 578  dxdf = repmat(dxdf,[1 2]);


 579  else


 580  [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);


 581  end;


 582 


 583  A = [dxdf dxdc dxdalpha dxdk]';


 584  B = [dxdom dxdT]';


 585 


 586  JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');


 587  JJ3(15+6*(kk1) + 1:15+6*(kk1) + 6,15+6*(kk1) + 1:15+6*(kk1) + 6) = sparse(B*B');


 588 


 589  AB = sparse(A*B');


 590  JJ3(1:10,15+6*(kk1) + 1:15+6*(kk1) + 6) = AB;


 591  JJ3(15+6*(kk1) + 1:15+6*(kk1) + 6,1:10) = (AB)';


 592 


 593  end;


 594 


 595  JJ3 = JJ3(ind_Jac,ind_Jac);


 596 


 597  JJ2_inv = inv(JJ3); % not bad for sparse matrices!!


 598 


 599  param_error = zeros(6*n_ima+15,1);


 600  param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x;


 601 


 602  solution_error = param_error;


 603 


 604  if ~est_aspect_ratio & isequal(est_fc,[1;1]),


 605  solution_error(2) = solution_error(1);


 606  end;


 607 


 608 


 609  %%% Extraction of the final intrinsic and extrinsic paramaters:


 610 


 611  extract_parameters;


 612 


 613  fprintf(1,'done\n');


 614 


 615 


 616  fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n');


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


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


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


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


 621  fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std);


 622  fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n')


 623  %fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n')


 624 


 625 


 626 


 627  %%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode.


 628 


 629  alpha_c_min = alpha_c  alpha_c_error/2;


 630  alpha_c_max = alpha_c + alpha_c_error/2;


 631 


 632  if (alpha_c_min < 0) & (alpha_c_max > 0),


 633  fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n');


 634  fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n');


 635  end;


 636 


 637  kc_min = kc  kc_error/2;


 638  kc_max = kc + kc_error/2;


 639 


 640  prob_kc = (kc_min < 0) & (kc_max > 0);


 641 


 642  if ~(prob_kc(3) & prob_kc(4))


 643  prob_kc(3:4) = [0;0];


 644  end;


 645 


 646 


 647  if sum(prob_kc),


 648  fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n');


 649  fprintf(1,' To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc);


 650  end;


 651 


 652 


[810]  653  return;

