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