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