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

Last change on this file since 967 was 926, checked in by sommeria, 8 years ago

geometry cqlib updated

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