source: trunk/src/toolbox_calib/init_intrinsic_param.m @ 962

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

geometry cqlib updated

File size: 5.0 KB
Line 
1%init_intrinsic_param
2%
3%Initialization of the intrinsic 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%             kc: Distortion coefficients
12%        alpha_c: skew coefficient
13%        KK: The camera matrix (containing fc, cc and alpha_c)
14%
15%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes
16%        the focal length fc from orthogonal vanishing points constraint.
17%        The principal point cc is assumed at the center of the image.
18%        Assumes no image distortion (kc = [0;0;0;0])
19%
20%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
21%      corresponding entry in the active_images vector to zero.
22%
23%
24%Important function called within that program:
25%
26%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
27%
28%
29%VERY IMPORTANT: This function works only with 2D rigs.
30%In the future, a more general function will be there (working with 3D rigs as well).
31
32
33if ~exist('two_focals_init'),
34    two_focals_init = 0;
35end;
36
37if ~exist('est_aspect_ratio'),
38    est_aspect_ratio = 1;
39end;
40
41check_active_images;
42
43if ~exist(['x_' num2str(ind_active(1)) ]),
44    click_calib;
45end;
46
47
48fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active));
49
50
51% Initialize the homographies:
52
53for kk = 1:n_ima,
54    eval(['x_kk = x_' num2str(kk) ';']);
55    eval(['X_kk = X_' num2str(kk) ';']);
56    if (isnan(x_kk(1,1))),
57        if active_images(kk),
58            fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
59            fprintf(1,'         Set active_images(%d)=1; and run Extract grid corners.\n',kk)
60        end;
61        active_images(kk) = 0;
62    end;
63    if active_images(kk),
64        eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']);
65    else
66        eval(['H_' num2str(kk) ' = NaN*ones(3,3);']);
67    end;
68end;
69
70check_active_images;
71
72% initial guess for principal point and distortion:
73
74if ~exist('nx'), [ny,nx] = size(I); end;
75
76c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image
77k_init = [0;0;0;0;0]; % initialize to zero (no distortion)
78
79
80
81% Compute explicitely the focal length using all the (mutually orthogonal) vanishing points
82% note: The vanihing points are hidden in the planar collineations H_kk
83
84A = [];
85b = [];
86
87% matrix that subtract the principal point:
88Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1];
89
90for kk=1:n_ima,
91   
92    if active_images(kk),
93       
94        eval(['Hkk = H_' num2str(kk) ';']);
95       
96        Hkk = Sub_cc * Hkk;   
97       
98        % Extract vanishing points (direct and diagonals):
99       
100        V_hori_pix = Hkk(:,1);
101        V_vert_pix = Hkk(:,2);
102        V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2;
103        V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2;
104       
105        V_hori_pix = V_hori_pix/norm(V_hori_pix);
106        V_vert_pix = V_vert_pix/norm(V_vert_pix);
107        V_diag1_pix = V_diag1_pix/norm(V_diag1_pix);
108        V_diag2_pix = V_diag2_pix/norm(V_diag2_pix);
109       
110        a1 = V_hori_pix(1);
111        b1 = V_hori_pix(2);
112        c1 = V_hori_pix(3);
113       
114        a2 = V_vert_pix(1);
115        b2 = V_vert_pix(2);
116        c2 = V_vert_pix(3);
117       
118        a3 = V_diag1_pix(1);
119        b3 = V_diag1_pix(2);
120        c3 = V_diag1_pix(3);
121       
122        a4 = V_diag2_pix(1);
123        b4 = V_diag2_pix(2);
124        c4 = V_diag2_pix(3);
125       
126        A_kk = [a1*a2  b1*b2;
127            a3*a4  b3*b4];
128       
129        b_kk = -[c1*c2;c3*c4];
130       
131       
132        A = [A;A_kk];
133        b = [b;b_kk];
134       
135    end;
136   
137end;
138
139
140% use all the vanishing points to estimate focal length:
141
142
143% Select the model for the focal. (solution to Gerd's problem)
144if ~two_focals_init
145    if b'*(sum(A')') < 0,
146        two_focals_init = 1;
147    end;
148end;
149
150   
151
152if two_focals_init
153    % Use a two focals estimate:
154    f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess
155else
156    % Use a single focal estimate:
157    f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used
158end;
159
160
161if ~est_aspect_ratio,
162    f_init(1) = (f_init(1)+f_init(2))/2;
163    f_init(2) = f_init(1);
164end;
165
166alpha_init = 0;
167
168%f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used
169
170
171% Global calibration matrix (initial guess):
172
173KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1];
174inv_KK = inv(KK);
175
176
177cc = c_init;
178fc = f_init;
179kc = k_init;
180alpha_c = alpha_init;
181
182
183fprintf(1,'\n\nCalibration parameters after initialization:\n\n');
184fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ]\n',fc);
185fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ]\n',cc);
186fprintf(1,'Skew:             alpha_c = [ %3.5f ]   => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi);
187fprintf(1,'Distortion:            kc = [ %3.5f   %3.5f   %3.5f   %3.5f   %5.5f ]\n',kc);   
Note: See TracBrowser for help on using the repository browser.