1 | %=======================================================================
|
---|
2 | % Copyright 2008-2016, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France
|
---|
3 | % http://www.legi.grenoble-inp.fr
|
---|
4 | % Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr
|
---|
5 | %
|
---|
6 | % This file is part of the toolbox UVMAT.
|
---|
7 | %
|
---|
8 | % UVMAT is free software; you can redistribute it and/or modify
|
---|
9 | % it under the terms of the GNU General Public License as published
|
---|
10 | % by the Free Software Foundation; either version 2 of the license,
|
---|
11 | % or (at your option) any later version.
|
---|
12 | %
|
---|
13 | % UVMAT is distributed in the hope that it will be useful,
|
---|
14 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
15 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
---|
16 | % GNU General Public License (see LICENSE.txt) for more details.
|
---|
17 | %=======================================================================
|
---|
18 |
|
---|
19 | %init_intrinsic_param
|
---|
20 | %
|
---|
21 | %Initialization of the intrinsic parameters.
|
---|
22 | %Runs as a script.
|
---|
23 | %
|
---|
24 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images
|
---|
25 | % X_1,X_2,X_3,...: Corresponding grid coordinates
|
---|
26 | %
|
---|
27 | %OUTPUT: fc: Camera focal length
|
---|
28 | % cc: Principal point coordinates
|
---|
29 | % kc: Distortion coefficients
|
---|
30 | % alpha_c: skew coefficient
|
---|
31 | % KK: The camera matrix (containing fc, cc and alpha_c)
|
---|
32 | %
|
---|
33 | %Method: Computes the planar homographies H_1, H_2, H_3, ... and computes
|
---|
34 | % the focal length fc from orthogonal vanishing points constraint.
|
---|
35 | % The principal point cc is assumed at the center of the image.
|
---|
36 | % Assumes no image distortion (kc = [0;0;0;0])
|
---|
37 | %
|
---|
38 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
|
---|
39 | % corresponding entry in the active_images vector to zero.
|
---|
40 | %
|
---|
41 | %
|
---|
42 | %Important function called within that program:
|
---|
43 | %
|
---|
44 | %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
|
---|
45 | %
|
---|
46 | %
|
---|
47 | %VERY IMPORTANT: This function works only with 2D rigs.
|
---|
48 | %In the future, a more general function will be there (working with 3D rigs as well).
|
---|
49 |
|
---|
50 |
|
---|
51 | if ~exist('two_focals_init'),
|
---|
52 | two_focals_init = 0;
|
---|
53 | end;
|
---|
54 |
|
---|
55 | if ~exist('est_aspect_ratio'),
|
---|
56 | est_aspect_ratio = 1;
|
---|
57 | end;
|
---|
58 |
|
---|
59 | check_active_images;
|
---|
60 |
|
---|
61 | if ~exist(['x_' num2str(ind_active(1)) ]),
|
---|
62 | click_calib;
|
---|
63 | end;
|
---|
64 |
|
---|
65 |
|
---|
66 | fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active));
|
---|
67 |
|
---|
68 |
|
---|
69 | % Initialize the homographies:
|
---|
70 |
|
---|
71 | for kk = 1:n_ima,
|
---|
72 | eval(['x_kk = x_' num2str(kk) ';']);
|
---|
73 | eval(['X_kk = X_' num2str(kk) ';']);
|
---|
74 | if (isnan(x_kk(1,1))),
|
---|
75 | if active_images(kk),
|
---|
76 | fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk)
|
---|
77 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk)
|
---|
78 | end;
|
---|
79 | active_images(kk) = 0;
|
---|
80 | end;
|
---|
81 | if active_images(kk),
|
---|
82 | eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']);
|
---|
83 | else
|
---|
84 | eval(['H_' num2str(kk) ' = NaN*ones(3,3);']);
|
---|
85 | end;
|
---|
86 | end;
|
---|
87 |
|
---|
88 | check_active_images;
|
---|
89 |
|
---|
90 | % initial guess for principal point and distortion:
|
---|
91 |
|
---|
92 | if ~exist('nx'), [ny,nx] = size(I); end;
|
---|
93 |
|
---|
94 | c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image
|
---|
95 | k_init = [0;0;0;0;0]; % initialize to zero (no distortion)
|
---|
96 |
|
---|
97 |
|
---|
98 |
|
---|
99 | % Compute explicitely the focal length using all the (mutually orthogonal) vanishing points
|
---|
100 | % note: The vanihing points are hidden in the planar collineations H_kk
|
---|
101 |
|
---|
102 | A = [];
|
---|
103 | b = [];
|
---|
104 |
|
---|
105 | % matrix that subtract the principal point:
|
---|
106 | Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1];
|
---|
107 |
|
---|
108 | for kk=1:n_ima,
|
---|
109 |
|
---|
110 | if active_images(kk),
|
---|
111 |
|
---|
112 | eval(['Hkk = H_' num2str(kk) ';']);
|
---|
113 |
|
---|
114 | Hkk = Sub_cc * Hkk;
|
---|
115 |
|
---|
116 | % Extract vanishing points (direct and diagonals):
|
---|
117 |
|
---|
118 | V_hori_pix = Hkk(:,1);
|
---|
119 | V_vert_pix = Hkk(:,2);
|
---|
120 | V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2;
|
---|
121 | V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2;
|
---|
122 |
|
---|
123 | V_hori_pix = V_hori_pix/norm(V_hori_pix);
|
---|
124 | V_vert_pix = V_vert_pix/norm(V_vert_pix);
|
---|
125 | V_diag1_pix = V_diag1_pix/norm(V_diag1_pix);
|
---|
126 | V_diag2_pix = V_diag2_pix/norm(V_diag2_pix);
|
---|
127 |
|
---|
128 | a1 = V_hori_pix(1);
|
---|
129 | b1 = V_hori_pix(2);
|
---|
130 | c1 = V_hori_pix(3);
|
---|
131 |
|
---|
132 | a2 = V_vert_pix(1);
|
---|
133 | b2 = V_vert_pix(2);
|
---|
134 | c2 = V_vert_pix(3);
|
---|
135 |
|
---|
136 | a3 = V_diag1_pix(1);
|
---|
137 | b3 = V_diag1_pix(2);
|
---|
138 | c3 = V_diag1_pix(3);
|
---|
139 |
|
---|
140 | a4 = V_diag2_pix(1);
|
---|
141 | b4 = V_diag2_pix(2);
|
---|
142 | c4 = V_diag2_pix(3);
|
---|
143 |
|
---|
144 | A_kk = [a1*a2 b1*b2;
|
---|
145 | a3*a4 b3*b4];
|
---|
146 |
|
---|
147 | b_kk = -[c1*c2;c3*c4];
|
---|
148 |
|
---|
149 |
|
---|
150 | A = [A;A_kk];
|
---|
151 | b = [b;b_kk];
|
---|
152 |
|
---|
153 | end;
|
---|
154 |
|
---|
155 | end;
|
---|
156 |
|
---|
157 |
|
---|
158 | % use all the vanishing points to estimate focal length:
|
---|
159 |
|
---|
160 |
|
---|
161 | % Select the model for the focal. (solution to Gerd's problem)
|
---|
162 | if ~two_focals_init
|
---|
163 | if b'*(sum(A')') < 0,
|
---|
164 | two_focals_init = 1;
|
---|
165 | end;
|
---|
166 | end;
|
---|
167 |
|
---|
168 |
|
---|
169 |
|
---|
170 | if two_focals_init
|
---|
171 | % Use a two focals estimate:
|
---|
172 | f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess
|
---|
173 | else
|
---|
174 | % Use a single focal estimate:
|
---|
175 | f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used
|
---|
176 | end;
|
---|
177 |
|
---|
178 |
|
---|
179 | if ~est_aspect_ratio,
|
---|
180 | f_init(1) = (f_init(1)+f_init(2))/2;
|
---|
181 | f_init(2) = f_init(1);
|
---|
182 | end;
|
---|
183 |
|
---|
184 | alpha_init = 0;
|
---|
185 |
|
---|
186 | %f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used
|
---|
187 |
|
---|
188 |
|
---|
189 | % Global calibration matrix (initial guess):
|
---|
190 |
|
---|
191 | KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1];
|
---|
192 | inv_KK = inv(KK);
|
---|
193 |
|
---|
194 |
|
---|
195 | cc = c_init;
|
---|
196 | fc = f_init;
|
---|
197 | kc = k_init;
|
---|
198 | alpha_c = alpha_init;
|
---|
199 |
|
---|
200 |
|
---|
201 | fprintf(1,'\n\nCalibration parameters after initialization:\n\n');
|
---|
202 | fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc);
|
---|
203 | fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc);
|
---|
204 | fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi);
|
---|
205 | fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',kc);
|
---|