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; |
---|