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

Last change on this file since 907 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 6.1 KB
Line 
1%=======================================================================
2% Copyright 2008-2014, LEGI UMR 5519 / CNRS UJF 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
51if ~exist('two_focals_init'),
52    two_focals_init = 0;
53end;
54
55if ~exist('est_aspect_ratio'),
56    est_aspect_ratio = 1;
57end;
58
59check_active_images;
60
61if ~exist(['x_' num2str(ind_active(1)) ]),
62    click_calib;
63end;
64
65
66fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active));
67
68
69% Initialize the homographies:
70
71for 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;
86end;
87
88check_active_images;
89
90% initial guess for principal point and distortion:
91
92if ~exist('nx'), [ny,nx] = size(I); end;
93
94c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image
95k_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
102A = [];
103b = [];
104
105% matrix that subtract the principal point:
106Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1];
107
108for 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   
155end;
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)
162if ~two_focals_init
163    if b'*(sum(A')') < 0,
164        two_focals_init = 1;
165    end;
166end;
167
168   
169
170if 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
173else
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
176end;
177
178
179if ~est_aspect_ratio,
180    f_init(1) = (f_init(1)+f_init(2))/2;
181    f_init(2) = f_init(1);
182end;
183
184alpha_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
191KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1];
192inv_KK = inv(KK);
193
194
195cc = c_init;
196fc = f_init;
197kc = k_init;
198alpha_c = alpha_init;
199
200
201fprintf(1,'\n\nCalibration parameters after initialization:\n\n');
202fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ]\n',fc);
203fprintf(1,'Principal point:       cc = [ %3.5f   %3.5f ]\n',cc);
204fprintf(1,'Skew:             alpha_c = [ %3.5f ]   => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi);
205fprintf(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.