source: trunk/src/toolbox_calib/go_calib_optim_iter.m @ 1110

Last change on this file since 1110 was 1110, checked in by sommeria, 2 years ago

calibration with plane angl corrected

File size: 19.2 KB
Line 
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
32if ~exist('desactivated_images','var')
33    desactivated_images = [];
34end
35
36if ~exist('est_aspect_ratio','var')
37    est_aspect_ratio = 1;
38end
39
40if ~exist('est_fc','var')
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!)
42end
43
44if ~exist('recompute_extrinsic','var')
45    recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters
46    % at each iterstion.
47end
48
49if ~exist('MaxIter','var')
50    MaxIter = 30; % Maximum number of iterations in the gradient descent
51end;
52
53if ~exist('check_cond','var'),
54    check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically
55end;
56
57if ~exist('center_optim','var'),
58    center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point
59end;
60
61if exist('est_dist','var'),
62    if length(est_dist) == 4,
63        est_dist = [est_dist ; 0];
64    end;
65end;
66
67if ~exist('est_dist','var'),
68    est_dist = [1;1;1;1;0];
69end;
70
71if ~exist('est_alpha','var'),
72    est_alpha = 0; % by default, do not estimate skew
73end;
74
75
76% Little fix in case of stupid values in the binary variables:
77center_optim = double(~~center_optim);
78est_alpha = double(~~est_alpha);
79est_dist = double(~~est_dist);
80est_fc = double(~~est_fc);
81est_aspect_ratio = double(~~est_aspect_ratio);
82
83
84
85fprintf(1,'\n');
86
87if ~exist('nx','var')&~exist('ny','var'),
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;
91end;
92
93
94check_active_images;
95
96
97quick_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:
101rig3D = 0;
102for kk = ind_active,
103    eval(['X_kk = X_' num2str(kk) ';']);
104    if is3D(X_kk),
105        rig3D = 1;
106    end;
107end;
108
109
110if 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;
114end;
115
116if ~exist('dont_ask','var'),
117    dont_ask = 0;
118end;
119
120if 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;
126end;
127
128
129% A quick fix for solving conflict
130if ~isequal(est_fc,[1;1]),
131    est_aspect_ratio=1;
132end;
133if ~est_aspect_ratio,
134    est_fc=[1;1];
135end;
136
137
138if ~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');
140else
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;
144end;
145
146if ~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;
156end;
157
158
159if ~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). ');
161    if ~exist('cc','var'),
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;
167else
168    fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n');
169end;
170
171
172if ~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; 
175end;
176
177if ~est_alpha,
178    fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n');
179    alpha_c = 0;
180else
181    fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n');
182end;
183
184
185if ~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;
199end;
200
201
202% Check 3D-ness of the calibration rig:
203rig3D = 0;
204for kk = ind_active,
205    eval(['X_kk = X_' num2str(kk) ';']);
206    if is3D(X_kk),
207        rig3D = 1;
208    end;
209end;
210
211% If the rig is 3D, then no choice: the only valid initialization is manual!
212if rig3D,
213    quick_init = 1;
214end;
215
216
217
218alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent
219
220
221% Conditioning threshold for view rejection
222thresh_cond = 1e6;
223
224
225
226% Initialization of the intrinsic parameters (if necessary)
227
228if ~exist('cc','var')
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
232end
233
234
235if exist('kc','var')
236    if length(kc) == 4
237        fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree');
238        kc = [kc;0];
239    end
240end
241
242if ~exist('alpha_c','var')
243    fprintf(1,'Initialization of the image skew to zero.\n');
244    alpha_c = 0;
245    alpha_smooth = 0.1; % slow convergence
246end
247
248if ~exist('fc','var') && quick_init
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
254end
255
256
257if ~exist('fc','var')
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];
263end
264
265
266if ~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
270end;
271
272if ~est_aspect_ratio,
273    fc(1) = (fc(1)+fc(2))/2;
274    fc(2) = fc(1);
275end;
276
277if ~prod(double(est_dist)),
278    % If no distortion estimated, set to zero the variables that are not estimated
279    kc = kc .* est_dist;
280end;
281
282
283if ~prod(double(est_fc)),
284    fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n');
285end;
286
287
288%%% Initialization of the extrinsic parameters for global minimization:
289comp_ext_calib;
290
291
292
293%%% Initialization of the global parameter vector:
294
295init_param = [fc;cc;alpha_c;kc;zeros(5,1)];
296
297for kk = 1:n_ima,
298    eval(['omckk = omc_' num2str(kk) ';']);
299    eval(['Tckk = Tc_' num2str(kk) ';']);
300    init_param = [init_param; omckk ; Tckk];   
301end;
302
303
304
305%-------------------- Main Optimization:
306
307fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));
308
309
310param = init_param;
311change = 1;
312
313iter = 0;
314
315fprintf(1,'Gradient descent iterations: ');
316
317param_list = param;
318
319
320while (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   
487end;
488
489fprintf(1,'done\n');
490
491
492
493%%%--------------------------- Computation of the error of estimation:
494
495fprintf(1,'Estimation of uncertainties...');
496
497
498check_active_images;
499
500solution = param;
501
502
503% Extraction of the paramters for computing the right reprojection error:
504
505fc = solution(1:2);
506cc = solution(3:4);
507alpha_c = solution(5);
508kc = solution(6:10);
509
510for 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   
530end;
531
532
533% Recompute the error (in the vector ex):
534comp_error_calib;
535
536sigma_x = std(ex(:));
537
538% Compute the size of the Jacobian matrix:
539N_points_views_active = N_points_views(ind_active);
540
541JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
542
543for 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   
571end;
572
573JJ3 = JJ3(ind_Jac,ind_Jac);
574
575JJ2_inv = inv(JJ3); % not bad for sparse matrices!!
576
577param_error = zeros(6*n_ima+15,1);
578param_error(ind_Jac) =  3*sqrt(full(diag(JJ2_inv)))*sigma_x;
579
580solution_error = param_error;
581
582if ~est_aspect_ratio && isequal(est_fc,[1;1]),
583    solution_error(2) = solution_error(1);
584end;
585
586
587%%% Extraction of the final intrinsic and extrinsic paramaters:
588
589extract_parameters;
590
591fprintf(1,'done\n');
592
593
594fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n');
595fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[fc;fc_error]);
596fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ] +/- [ %3.5f   %3.5f ]\n',[cc;cc_error]);
597fprintf(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);
598fprintf(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]);   
599fprintf(1,'Pixel error:          err = [ %3.5f   %3.5f ]\n\n',err_std);
600fprintf(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
607alpha_c_min = alpha_c - alpha_c_error/2;
608alpha_c_max = alpha_c + alpha_c_error/2;
609
610if (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');
613end;
614
615kc_min = kc - kc_error/2;
616kc_max = kc + kc_error/2;
617
618prob_kc = (kc_min < 0) & (kc_max > 0);
619
620if ~(prob_kc(3) && prob_kc(4))
621    prob_kc(3:4) = [0;0];
622end;
623
624
625if 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);
628end;
629
630
631return;
Note: See TracBrowser for help on using the repository browser.