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