[725] | 1 | %go_calib_optim_iter
|
---|
| 2 | %
|
---|
| 3 | %Main calibration function. Computes the intrinsic andextrinsic 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 | % alpha_c: Skew coefficient
|
---|
| 12 | % kc: Distortion coefficients
|
---|
| 13 | % KK: The camera matrix (containing fc and cc)
|
---|
| 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
|
---|
| 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
|
---|
| 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
|
---|
| 17 | %
|
---|
| 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
|
---|
| 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space)
|
---|
| 20 | %
|
---|
| 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
|
---|
| 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
|
---|
| 23 | %
|
---|
| 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
|
---|
| 25 | % corresponding entry in the active_images vector to zero.
|
---|
| 26 | %
|
---|
| 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
|
---|
| 28 | %that is so far implemented to work only with 2D rigs.
|
---|
| 29 | %In the future, a more general function will be there.
|
---|
| 30 | %For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length
|
---|
| 31 |
|
---|
| 32 | if ~exist('desactivated_images'),
|
---|
| 33 | desactivated_images = [];
|
---|
| 34 | end;
|
---|
| 35 |
|
---|
| 36 |
|
---|
| 37 |
|
---|
| 38 | if ~exist('est_aspect_ratio'),
|
---|
| 39 | est_aspect_ratio = 1;
|
---|
| 40 | end;
|
---|
| 41 |
|
---|
| 42 | if ~exist('est_fc');
|
---|
| 43 | 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!)
|
---|
| 44 | end;
|
---|
| 45 |
|
---|
| 46 | if ~exist('recompute_extrinsic'),
|
---|
| 47 | recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters
|
---|
| 48 | % at each iterstion.
|
---|
| 49 | end;
|
---|
| 50 |
|
---|
| 51 | if ~exist('MaxIter'),
|
---|
| 52 | MaxIter = 30; % Maximum number of iterations in the gradient descent
|
---|
| 53 | end;
|
---|
| 54 |
|
---|
| 55 | if ~exist('check_cond'),
|
---|
| 56 | check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically
|
---|
| 57 | end;
|
---|
| 58 |
|
---|
| 59 | if ~exist('center_optim'),
|
---|
| 60 | center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
|
---|
| 61 | end;
|
---|
| 62 |
|
---|
| 63 | if exist('est_dist'),
|
---|
| 64 | if length(est_dist) == 4,
|
---|
| 65 | est_dist = [est_dist ; 0];
|
---|
| 66 | end;
|
---|
| 67 | end;
|
---|
| 68 |
|
---|
| 69 | if ~exist('est_dist'),
|
---|
| 70 | est_dist = [1;1;1;1;0];
|
---|
| 71 | end;
|
---|
| 72 |
|
---|
| 73 | if ~exist('est_alpha'),
|
---|
| 74 | est_alpha = 0; % by default, do not estimate skew
|
---|
| 75 | end;
|
---|
| 76 |
|
---|
| 77 |
|
---|
| 78 | % Little fix in case of stupid values in the binary variables:
|
---|
| 79 | center_optim = double(~~center_optim);
|
---|
| 80 | est_alpha = double(~~est_alpha);
|
---|
| 81 | est_dist = double(~~est_dist);
|
---|
| 82 | est_fc = double(~~est_fc);
|
---|
| 83 | est_aspect_ratio = double(~~est_aspect_ratio);
|
---|
| 84 |
|
---|
| 85 |
|
---|
| 86 |
|
---|
| 87 | fprintf(1,'\n');
|
---|
| 88 |
|
---|
| 89 | if ~exist('nx')&~exist('ny'),
|
---|
| 90 | 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');
|
---|
| 91 | nx = 640;
|
---|
| 92 | ny = 480;
|
---|
| 93 | end;
|
---|
| 94 |
|
---|
| 95 |
|
---|
| 96 | check_active_images;
|
---|
| 97 |
|
---|
| 98 | quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs)
|
---|
| 99 |
|
---|
| 100 |
|
---|
| 101 | % Check 3D-ness of the calibration rig:
|
---|
| 102 | rig3D = 0;
|
---|
| 103 | for kk = ind_active,
|
---|
| 104 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
| 105 | if is3D(X_kk),
|
---|
| 106 | rig3D = 1;
|
---|
| 107 | end;
|
---|
| 108 | end;
|
---|
| 109 |
|
---|
| 110 |
|
---|
| 111 | if center_optim & (length(ind_active) < 2) & ~rig3D,
|
---|
| 112 | fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n');
|
---|
| 113 | center_optim = 0; %%% when using a single image, please, no principal point estimation!!!
|
---|
| 114 | est_alpha = 0;
|
---|
| 115 | end;
|
---|
| 116 |
|
---|
| 117 | if ~exist('dont_ask'),
|
---|
| 118 | dont_ask = 0;
|
---|
| 119 | end;
|
---|
| 120 |
|
---|
| 121 | if center_optim & (length(ind_active) < 5) & ~rig3D,
|
---|
| 122 | fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n');
|
---|
| 123 | %if ~dont_ask,
|
---|
| 124 | % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) ');
|
---|
| 125 | % center_optim = isempty(quest);
|
---|
| 126 | %end;
|
---|
| 127 | end;
|
---|
| 128 |
|
---|
| 129 |
|
---|
| 130 | % A quick fix for solving conflict
|
---|
| 131 | if ~isequal(est_fc,[1;1]),
|
---|
| 132 | est_aspect_ratio=1;
|
---|
| 133 | end;
|
---|
| 134 | if ~est_aspect_ratio,
|
---|
| 135 | est_fc=[1;1];
|
---|
| 136 | end;
|
---|
| 137 |
|
---|
| 138 |
|
---|
| 139 | if ~est_aspect_ratio,
|
---|
| 140 | 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');
|
---|
| 141 | else
|
---|
| 142 | if isequal(est_fc,[1;1]),
|
---|
| 143 | fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n');
|
---|
| 144 | end;
|
---|
| 145 | end;
|
---|
| 146 |
|
---|
| 147 | if ~isequal(est_fc,[1;1]),
|
---|
| 148 | if isequal(est_fc,[1;0]),
|
---|
| 149 | fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n');
|
---|
| 150 | else
|
---|
| 151 | if isequal(est_fc,[0;1]),
|
---|
| 152 | fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n');
|
---|
| 153 | else
|
---|
| 154 | fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n');
|
---|
| 155 | end;
|
---|
| 156 | end;
|
---|
| 157 | end;
|
---|
| 158 |
|
---|
| 159 |
|
---|
| 160 | if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image
|
---|
| 161 | fprintf(1,'Principal point not optimized (center_optim=0). ');
|
---|
| 162 | if ~exist('cc'),
|
---|
| 163 | fprintf(1,'It is kept at the center of the image.\n');
|
---|
| 164 | cc = [(nx-1)/2;(ny-1)/2];
|
---|
| 165 | else
|
---|
| 166 | fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n');
|
---|
| 167 | end;
|
---|
| 168 | else
|
---|
| 169 | fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n');
|
---|
| 170 | end;
|
---|
| 171 |
|
---|
| 172 |
|
---|
| 173 | if ~center_optim & (est_alpha),
|
---|
| 174 | fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n');
|
---|
| 175 | est_alpha = 0;
|
---|
| 176 | end;
|
---|
| 177 |
|
---|
| 178 | if ~est_alpha,
|
---|
| 179 | fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n');
|
---|
| 180 | alpha_c = 0;
|
---|
| 181 | else
|
---|
| 182 | fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n');
|
---|
| 183 | end;
|
---|
| 184 |
|
---|
| 185 |
|
---|
| 186 | if ~prod(double(est_dist)),
|
---|
| 187 | fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n');
|
---|
| 188 | if ~est_dist(1),
|
---|
| 189 | fprintf(1,' Second order distortion not estimated (est_dist(1)=0).\n');
|
---|
| 190 | end;
|
---|
| 191 | if ~est_dist(2),
|
---|
| 192 | fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0).\n');
|
---|
| 193 | end;
|
---|
| 194 | if ~est_dist(5),
|
---|
| 195 | fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n');
|
---|
| 196 | end;
|
---|
| 197 | if ~prod(double(est_dist(3:4))),
|
---|
| 198 | fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n');
|
---|
| 199 | end;
|
---|
| 200 | end;
|
---|
| 201 |
|
---|
| 202 |
|
---|
| 203 | % Check 3D-ness of the calibration rig:
|
---|
| 204 | rig3D = 0;
|
---|
| 205 | for kk = ind_active,
|
---|
| 206 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
| 207 | if is3D(X_kk),
|
---|
| 208 | rig3D = 1;
|
---|
| 209 | end;
|
---|
| 210 | end;
|
---|
| 211 |
|
---|
| 212 | % If the rig is 3D, then no choice: the only valid initialization is manual!
|
---|
| 213 | if rig3D,
|
---|
| 214 | quick_init = 1;
|
---|
| 215 | end;
|
---|
| 216 |
|
---|
| 217 |
|
---|
| 218 |
|
---|
| 219 | alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent
|
---|
| 220 | % alpha_smooth = 0.01; % modified L. Gostiaux
|
---|
| 221 |
|
---|
| 222 |
|
---|
| 223 | % Conditioning threshold for view rejection
|
---|
| 224 | thresh_cond = 1e5;
|
---|
| 225 |
|
---|
| 226 |
|
---|
| 227 |
|
---|
| 228 | % Initialization of the intrinsic parameters (if necessary)
|
---|
| 229 |
|
---|
| 230 | if ~exist('cc'),
|
---|
| 231 | fprintf(1,'Initialization of the principal point at the center of the image.\n');
|
---|
| 232 | cc = [(nx-1)/2;(ny-1)/2];
|
---|
| 233 | alpha_smooth = 0.1; % slow convergence
|
---|
| 234 | end;
|
---|
| 235 |
|
---|
| 236 |
|
---|
| 237 | if exist('kc'),
|
---|
| 238 | if length(kc) == 4;
|
---|
| 239 | fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree');
|
---|
| 240 | kc = [kc;0];
|
---|
| 241 | end;
|
---|
| 242 | end;
|
---|
| 243 |
|
---|
| 244 |
|
---|
| 245 |
|
---|
| 246 | if ~exist('alpha_c'),
|
---|
| 247 | fprintf(1,'Initialization of the image skew to zero.\n');
|
---|
| 248 | alpha_c = 0;
|
---|
| 249 | alpha_smooth = 0.1; % slow convergence
|
---|
| 250 | end;
|
---|
| 251 |
|
---|
| 252 | if ~exist('fc')& quick_init,
|
---|
| 253 | FOV_angle = 35; % Initial camera field of view in degrees
|
---|
| 254 | fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']);
|
---|
| 255 | fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1);
|
---|
| 256 | est_fc = [1;1];
|
---|
| 257 | alpha_smooth = 0.1; % slow
|
---|
| 258 | end;
|
---|
| 259 |
|
---|
| 260 |
|
---|
| 261 | if ~exist('fc'),
|
---|
| 262 | % Initialization of the intrinsic parameters:
|
---|
| 263 | fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n')
|
---|
| 264 | init_intrinsic_param; % The right way to go (if quick_init is not active)!
|
---|
| 265 | alpha_smooth = 0.1; % slow convergence
|
---|
| 266 | est_fc = [1;1];
|
---|
| 267 | end;
|
---|
| 268 |
|
---|
| 269 |
|
---|
| 270 | if ~exist('kc'),
|
---|
| 271 | fprintf(1,'Initialization of the image distortion to zero.\n');
|
---|
| 272 | kc = zeros(5,1);
|
---|
| 273 | alpha_smooth = 0.1; % slow convergence
|
---|
| 274 | end;
|
---|
| 275 |
|
---|
| 276 | if ~est_aspect_ratio,
|
---|
| 277 | fc(1) = (fc(1)+fc(2))/2;
|
---|
| 278 | fc(2) = fc(1);
|
---|
| 279 | end;
|
---|
| 280 |
|
---|
| 281 | if ~prod(double(est_dist)),
|
---|
| 282 | % If no distortion estimated, set to zero the variables that are not estimated
|
---|
| 283 | kc = kc .* est_dist;
|
---|
| 284 | end;
|
---|
| 285 |
|
---|
| 286 |
|
---|
| 287 | if ~prod(double(est_fc)),
|
---|
| 288 | fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n');
|
---|
| 289 | end;
|
---|
| 290 |
|
---|
| 291 |
|
---|
| 292 | %%% Initialization of the extrinsic parameters for global minimization:
|
---|
| 293 | comp_ext_calib;
|
---|
| 294 |
|
---|
| 295 |
|
---|
| 296 |
|
---|
| 297 | %%% Initialization of the global parameter vector:
|
---|
| 298 |
|
---|
| 299 | init_param = [fc;cc;alpha_c;kc;zeros(5,1)];
|
---|
| 300 |
|
---|
| 301 | for kk = 1:n_ima,
|
---|
| 302 | eval(['omckk = omc_' num2str(kk) ';']);
|
---|
| 303 | eval(['Tckk = Tc_' num2str(kk) ';']);
|
---|
| 304 | init_param = [init_param; omckk ; Tckk];
|
---|
| 305 | end;
|
---|
| 306 |
|
---|
| 307 |
|
---|
| 308 |
|
---|
| 309 | %-------------------- Main Optimization:
|
---|
| 310 |
|
---|
| 311 | fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));
|
---|
| 312 |
|
---|
| 313 |
|
---|
| 314 | param = init_param;
|
---|
| 315 | change = 1;
|
---|
| 316 |
|
---|
| 317 | iter = 0;
|
---|
| 318 |
|
---|
| 319 | fprintf(1,'Gradient descent iterations: ');
|
---|
| 320 |
|
---|
| 321 | param_list = param;
|
---|
| 322 |
|
---|
| 323 |
|
---|
| 324 | while (change > 1e-2)&(iter < MaxIter),
|
---|
| 325 |
|
---|
| 326 | fprintf(1,'%d...',iter+1);
|
---|
| 327 |
|
---|
| 328 | % To speed up: pre-allocate the memory for the Jacobian JJ3.
|
---|
| 329 | % For that, need to compute the total number of points.
|
---|
| 330 |
|
---|
| 331 | %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active
|
---|
| 332 | %% images) through a one step steepest gradient descent.
|
---|
| 333 |
|
---|
| 334 |
|
---|
| 335 | f = param(1:2);
|
---|
| 336 | c = param(3:4);
|
---|
| 337 | alpha = param(5);
|
---|
| 338 | k = param(6:10);
|
---|
| 339 |
|
---|
| 340 |
|
---|
| 341 | % Compute the size of the Jacobian matrix:
|
---|
| 342 | N_points_views_active = N_points_views(ind_active);
|
---|
| 343 |
|
---|
| 344 | JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
|
---|
| 345 | ex3 = zeros(15 + 6*n_ima,1);
|
---|
| 346 |
|
---|
| 347 |
|
---|
| 348 | for kk = ind_active, %1:n_ima,
|
---|
| 349 | %if active_images(kk),
|
---|
| 350 |
|
---|
| 351 | omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
|
---|
| 352 |
|
---|
| 353 | Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
|
---|
| 354 |
|
---|
| 355 | if isnan(omckk(1)),
|
---|
| 356 | fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);
|
---|
| 357 | return;
|
---|
| 358 | end;
|
---|
| 359 |
|
---|
| 360 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
| 361 | eval(['x_kk = x_' num2str(kk) ';']);
|
---|
| 362 |
|
---|
| 363 | Np = N_points_views(kk);
|
---|
| 364 |
|
---|
| 365 | if ~est_aspect_ratio,
|
---|
| 366 | [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f(1),c,k,alpha);
|
---|
| 367 | dxdf = repmat(dxdf,[1 2]);
|
---|
| 368 | else
|
---|
| 369 | [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha);
|
---|
| 370 | end;
|
---|
| 371 |
|
---|
| 372 | exkk = x_kk - x;
|
---|
| 373 |
|
---|
| 374 | A = [dxdf dxdc dxdalpha dxdk]';
|
---|
| 375 | B = [dxdom dxdT]';
|
---|
| 376 |
|
---|
| 377 | JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
|
---|
| 378 | JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');
|
---|
| 379 |
|
---|
| 380 | AB = sparse(A*B');
|
---|
| 381 | JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
|
---|
| 382 | JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';
|
---|
| 383 |
|
---|
| 384 | ex3(1:10) = ex3(1:10) + A*exkk(:);
|
---|
| 385 | ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:);
|
---|
| 386 |
|
---|
| 387 | % Check if this view is ill-conditioned:
|
---|
| 388 | if check_cond,
|
---|
| 389 | JJ_kk = B'; %[dxdom dxdT];
|
---|
| 390 | if (cond(JJ_kk)> thresh_cond),
|
---|
| 391 | active_images(kk) = 0;
|
---|
| 392 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk)
|
---|
| 393 | desactivated_images = [desactivated_images kk];
|
---|
| 394 | param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1);
|
---|
| 395 | end;
|
---|
| 396 | end;
|
---|
| 397 |
|
---|
| 398 | %end;
|
---|
| 399 |
|
---|
| 400 | end;
|
---|
| 401 |
|
---|
| 402 |
|
---|
| 403 | % List of active images (necessary if changed):
|
---|
| 404 | check_active_images;
|
---|
| 405 |
|
---|
| 406 |
|
---|
| 407 | % The following vector helps to select the variables to update (for only active images):
|
---|
| 408 | 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)];
|
---|
| 409 | if ~est_aspect_ratio,
|
---|
| 410 | if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]),
|
---|
| 411 | selected_variables(2) = 0;
|
---|
| 412 | end;
|
---|
| 413 | end;
|
---|
| 414 | ind_Jac = find(selected_variables)';
|
---|
| 415 |
|
---|
| 416 | JJ3 = JJ3(ind_Jac,ind_Jac);
|
---|
| 417 | ex3 = ex3(ind_Jac);
|
---|
| 418 |
|
---|
| 419 | JJ2_inv = inv(JJ3); % not bad for sparse matrices!!
|
---|
| 420 |
|
---|
| 421 |
|
---|
| 422 | % Smoothing coefficient:
|
---|
| 423 |
|
---|
| 424 | alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!
|
---|
| 425 |
|
---|
| 426 | param_innov = alpha_smooth2*JJ2_inv*ex3;
|
---|
| 427 |
|
---|
| 428 |
|
---|
| 429 | param_up = param(ind_Jac) + param_innov;
|
---|
| 430 | param(ind_Jac) = param_up;
|
---|
| 431 |
|
---|
| 432 |
|
---|
| 433 | % New intrinsic parameters:
|
---|
| 434 |
|
---|
| 435 | fc_current = param(1:2);
|
---|
| 436 | cc_current = param(3:4);
|
---|
| 437 |
|
---|
| 438 | if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)),
|
---|
| 439 | fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n');
|
---|
| 440 | center_optim = 0;
|
---|
| 441 | cc_current = c;
|
---|
| 442 | else
|
---|
| 443 | cc_current = param(3:4);
|
---|
| 444 | end;
|
---|
| 445 |
|
---|
| 446 | alpha_current = param(5);
|
---|
| 447 | kc_current = param(6:10);
|
---|
| 448 |
|
---|
| 449 | if ~est_aspect_ratio & isequal(est_fc,[1;1]),
|
---|
| 450 | fc_current(2) = fc_current(1);
|
---|
| 451 | param(2) = param(1);
|
---|
| 452 | end;
|
---|
| 453 |
|
---|
| 454 | % Change on the intrinsic parameters:
|
---|
| 455 | change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]);
|
---|
| 456 |
|
---|
| 457 |
|
---|
| 458 | %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!!
|
---|
| 459 | %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)
|
---|
| 460 | %% The complete gradient descent method is useful to precisely update the intrinsic parameters.
|
---|
| 461 |
|
---|
| 462 |
|
---|
| 463 | if recompute_extrinsic,
|
---|
| 464 | MaxIter2 = 20;
|
---|
| 465 | for kk =ind_active, %1:n_ima,
|
---|
| 466 | %if active_images(kk),
|
---|
| 467 | omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
|
---|
| 468 | Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
|
---|
| 469 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
| 470 | eval(['x_kk = x_' num2str(kk) ';']);
|
---|
| 471 | [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);
|
---|
| 472 | [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);
|
---|
| 473 | if check_cond,
|
---|
| 474 | if (cond(JJ_kk)> thresh_cond),
|
---|
| 475 | active_images(kk) = 0;
|
---|
| 476 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk);
|
---|
| 477 | desactivated_images = [desactivated_images kk];
|
---|
| 478 | omckk = NaN*ones(3,1);
|
---|
| 479 | Tckk = NaN*ones(3,1);
|
---|
| 480 | end;
|
---|
| 481 | end;
|
---|
| 482 | param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk;
|
---|
| 483 | param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk;
|
---|
| 484 | %end;
|
---|
| 485 | end;
|
---|
| 486 | end;
|
---|
| 487 |
|
---|
| 488 | param_list = [param_list param];
|
---|
| 489 | iter = iter + 1;
|
---|
| 490 |
|
---|
| 491 | end;
|
---|
| 492 |
|
---|
| 493 | fprintf(1,'done\n');
|
---|
| 494 |
|
---|
| 495 |
|
---|
| 496 |
|
---|
| 497 | %%%--------------------------- Computation of the error of estimation:
|
---|
| 498 |
|
---|
| 499 | fprintf(1,'Estimation of uncertainties...');
|
---|
| 500 |
|
---|
| 501 |
|
---|
| 502 | check_active_images;
|
---|
| 503 |
|
---|
| 504 | solution = param;
|
---|
| 505 |
|
---|
| 506 |
|
---|
| 507 | % Extraction of the paramters for computing the right reprojection error:
|
---|
| 508 |
|
---|
| 509 | fc = solution(1:2);
|
---|
| 510 | cc = solution(3:4);
|
---|
| 511 | alpha_c = solution(5);
|
---|
| 512 | kc = solution(6:10);
|
---|
| 513 |
|
---|
| 514 | for kk = 1:n_ima,
|
---|
| 515 |
|
---|
| 516 | if active_images(kk),
|
---|
| 517 |
|
---|
| 518 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***
|
---|
| 519 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
|
---|
| 520 | Rckk = rodrigues(omckk);
|
---|
| 521 |
|
---|
| 522 | else
|
---|
| 523 |
|
---|
| 524 | omckk = NaN*ones(3,1);
|
---|
| 525 | Tckk = NaN*ones(3,1);
|
---|
| 526 | Rckk = NaN*ones(3,3);
|
---|
| 527 |
|
---|
| 528 | end;
|
---|
| 529 |
|
---|
| 530 | eval(['omc_' num2str(kk) ' = omckk;']);
|
---|
| 531 | eval(['Rc_' num2str(kk) ' = Rckk;']);
|
---|
| 532 | eval(['Tc_' num2str(kk) ' = Tckk;']);
|
---|
| 533 |
|
---|
| 534 | end;
|
---|
| 535 |
|
---|
| 536 |
|
---|
| 537 | % Recompute the error (in the vector ex):
|
---|
| 538 | comp_error_calib;
|
---|
| 539 |
|
---|
| 540 | sigma_x = std(ex(:));
|
---|
| 541 |
|
---|
| 542 | % Compute the size of the Jacobian matrix:
|
---|
| 543 | N_points_views_active = N_points_views(ind_active);
|
---|
| 544 |
|
---|
| 545 | JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
|
---|
| 546 |
|
---|
| 547 | for kk = ind_active,
|
---|
| 548 |
|
---|
| 549 | omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
|
---|
| 550 | Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
|
---|
| 551 |
|
---|
| 552 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
| 553 |
|
---|
| 554 | Np = N_points_views(kk);
|
---|
| 555 |
|
---|
| 556 | %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
|
---|
| 557 |
|
---|
| 558 | if ~est_aspect_ratio,
|
---|
| 559 | [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c);
|
---|
| 560 | dxdf = repmat(dxdf,[1 2]);
|
---|
| 561 | else
|
---|
| 562 | [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
|
---|
| 563 | end;
|
---|
| 564 |
|
---|
| 565 | A = [dxdf dxdc dxdalpha dxdk]';
|
---|
| 566 | B = [dxdom dxdT]';
|
---|
| 567 |
|
---|
| 568 | JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
|
---|
| 569 | JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');
|
---|
| 570 |
|
---|
| 571 | AB = sparse(A*B');
|
---|
| 572 | JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
|
---|
| 573 | JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';
|
---|
| 574 |
|
---|
| 575 | end;
|
---|
| 576 |
|
---|
| 577 | JJ3 = JJ3(ind_Jac,ind_Jac);
|
---|
| 578 |
|
---|
| 579 | JJ2_inv = inv(JJ3); % not bad for sparse matrices!!
|
---|
| 580 |
|
---|
| 581 | param_error = zeros(6*n_ima+15,1);
|
---|
| 582 | param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x;
|
---|
| 583 |
|
---|
| 584 | solution_error = param_error;
|
---|
| 585 |
|
---|
| 586 | if ~est_aspect_ratio & isequal(est_fc,[1;1]),
|
---|
| 587 | solution_error(2) = solution_error(1);
|
---|
| 588 | end;
|
---|
| 589 |
|
---|
| 590 |
|
---|
| 591 | %%% Extraction of the final intrinsic and extrinsic paramaters:
|
---|
| 592 |
|
---|
| 593 | extract_parameters;
|
---|
| 594 |
|
---|
| 595 | fprintf(1,'done\n');
|
---|
| 596 |
|
---|
| 597 |
|
---|
| 598 | fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n');
|
---|
| 599 | fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ? [ %3.5f %3.5f ]\n',[fc;fc_error]);
|
---|
| 600 | fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ? [ %3.5f %3.5f ]\n',[cc;cc_error]);
|
---|
| 601 | 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);
|
---|
| 602 | 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]);
|
---|
| 603 | fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std);
|
---|
| 604 | fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n')
|
---|
| 605 | %fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n')
|
---|
| 606 |
|
---|
| 607 |
|
---|
| 608 |
|
---|
| 609 | %%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode.
|
---|
| 610 |
|
---|
| 611 | alpha_c_min = alpha_c - alpha_c_error/2;
|
---|
| 612 | alpha_c_max = alpha_c + alpha_c_error/2;
|
---|
| 613 |
|
---|
| 614 | if (alpha_c_min < 0) & (alpha_c_max > 0),
|
---|
| 615 | fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n');
|
---|
| 616 | fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n');
|
---|
| 617 | end;
|
---|
| 618 |
|
---|
| 619 | kc_min = kc - kc_error/2;
|
---|
| 620 | kc_max = kc + kc_error/2;
|
---|
| 621 |
|
---|
| 622 | prob_kc = (kc_min < 0) & (kc_max > 0);
|
---|
| 623 |
|
---|
| 624 | if ~(prob_kc(3) & prob_kc(4))
|
---|
| 625 | prob_kc(3:4) = [0;0];
|
---|
| 626 | end;
|
---|
| 627 |
|
---|
| 628 |
|
---|
| 629 | if sum(prob_kc),
|
---|
| 630 | fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n');
|
---|
| 631 | 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);
|
---|
| 632 | end;
|
---|
| 633 |
|
---|
| 634 |
|
---|
| 635 | return; |
---|