source: trunk/src/toolbox_calib/compute_extrinsic_init.m @ 935

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

geometry cqlib updated

File size: 5.0 KB
Line 
1function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c),
2
3%compute_extrinsic
4%
5%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c)
6%
7%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
8%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
9%Works with planar and non-planar structures.
10%
11%INPUT: x_kk: Feature locations on the images
12%       X_kk: Corresponding grid coordinates
13%       fc: Camera focal length
14%       cc: Principal point coordinates
15%       kc: Distortion coefficients
16%       alpha_c: Skew coefficient
17%
18%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
19%        Tckk: 3D translation vector attached to the grid positions in space
20%        Rckk: 3D rotation matrices corresponding to the omc vectors
21%
22%Method: Computes the normalized point coordinates, then computes the 3D pose
23%
24%Important functions called within that program:
25%
26%normalize_pixel: Computes the normalize image point coordinates.
27%
28%pose3D: Computes the 3D pose of the structure given the normalized image projection.
29%
30%project_points.m: Computes the 2D image projections of a set of 3D points
31
32
33
34if nargin < 6,
35   alpha_c = 0;
36        if nargin < 5,
37        kc = zeros(5,1);
38        if nargin < 4,
39        cc = zeros(2,1);
40        if nargin < 3,
41                fc = ones(2,1);
42                if nargin < 2,
43                error('Need 2D projections and 3D points (in compute_extrinsic.m)');
44                return;
45                end;
46        end;
47        end;
48        end;
49end;
50
51
52%keyboard;
53
54% Compute the normalized coordinates:
55
56xn = normalize_pixel(x_kk,fc,cc,kc,alpha_c);
57
58
59
60Np = size(xn,2);
61
62%% Check for planarity of the structure:
63%keyboard;
64
65X_mean = mean(X_kk')';
66
67Y = X_kk - (X_mean*ones(1,Np));
68
69YY = Y*Y';
70
71[U,S,V] = svd(YY);
72
73r = S(3,3)/S(2,2);
74
75%keyboard;
76
77
78if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity
79   
80   %fprintf(1,'Planar structure detected: r=%f\n',r);
81
82   % Transform the plane to bring it in the Z=0 plane:
83   
84   R_transform = V';
85   
86   %norm(R_transform(1:2,3))
87   
88   if norm(R_transform(1:2,3)) < 1e-6,
89      R_transform = eye(3);
90   end;
91   
92   if det(R_transform) < 0, R_transform = -R_transform; end;
93   
94        T_transform = -(R_transform)*X_mean;
95
96        X_new = R_transform*X_kk + T_transform*ones(1,Np);
97   
98   
99   % Compute the planar homography:
100   
101   H = compute_homography(xn,X_new(1:2,:));
102   
103   % De-embed the motion parameters from the homography:
104   
105   sc = mean([norm(H(:,1));norm(H(:,2))]);
106   
107   H = H/sc;
108   
109   % Extra normalization for some reasons...
110   %H(:,1) = H(:,1)/norm(H(:,1));
111   %H(:,2) = H(:,2)/norm(H(:,2));
112   
113   if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!!
114       
115       % Case#1: no opposite sign:
116       
117       omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
118       Rckk1 = rodrigues(omckk1);
119       Tckk1 = H(:,3);
120       
121       Hs1 = [Rckk1(:,1:2) Tckk1];
122       xn1 = Hs1*[X_new(1:2,:);ones(1,Np)];
123       xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)];
124       e1 = xn1 - xn;
125       
126       % Case#2: opposite sign:
127       
128       omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]);
129       Rckk2 = rodrigues(omckk2);
130       Tckk2 = -H(:,3);
131       
132       Hs2 = [Rckk2(:,1:2) Tckk2];
133       xn2 = Hs2*[X_new(1:2,:);ones(1,Np)];
134       xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)];
135       e2 = xn2 - xn;
136       
137       if 1, %norm(e1) < norm(e2),
138           omckk = omckk1;
139           Tckk = Tckk1;
140           Rckk = Rckk1;
141       else
142           omckk = omckk2;
143           Tckk = Tckk2;
144           Rckk = Rckk2;
145       end;
146       
147   else
148       
149       u1 = H(:,1);
150       u1 = u1 / norm(u1);
151       u2 = H(:,2) - dot(u1,H(:,2)) * u1;
152       u2 = u2 / norm(u2);
153       u3 = cross(u1,u2);
154       RRR = [u1 u2 u3];
155       omckk = rodrigues(RRR);
156
157       %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);
158       Rckk = rodrigues(omckk);
159       Tckk = H(:,3);
160       
161   end;
162   
163     
164   
165   %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform
166   
167   Tckk = Tckk + Rckk* T_transform;
168   Rckk = Rckk * R_transform;
169
170   omckk = rodrigues(Rckk);
171   Rckk = rodrigues(omckk);
172   
173   
174else
175   
176   %fprintf(1,'Non planar structure detected: r=%f\n',r);
177
178   % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!):
179   % The DLT method is applied here!!
180   
181   J = zeros(2*Np,12);
182       
183        xX = (ones(3,1)*xn(1,:)).*X_kk;
184        yX = (ones(3,1)*xn(2,:)).*X_kk;
185       
186        J(1:2:end,[1 4 7]) = -X_kk';
187        J(2:2:end,[2 5 8]) = X_kk';
188        J(1:2:end,[3 6 9]) = xX';
189        J(2:2:end,[3 6 9]) = -yX';
190        J(1:2:end,12) = xn(1,:)';
191        J(2:2:end,12) = -xn(2,:)';
192        J(1:2:end,10) = -ones(Np,1);
193        J(2:2:end,11) = ones(Np,1);
194       
195        JJ = J'*J;
196        [U,S,V] = svd(JJ);
197   
198   RR = reshape(V(1:9,12),3,3);
199   
200   if det(RR) < 0,
201      V(:,12) = -V(:,12);
202      RR = -RR;
203   end;
204   
205   [Ur,Sr,Vr] = svd(RR);
206   
207   Rckk = Ur*Vr';
208   
209   sc = norm(V(1:9,12)) / norm(Rckk(:));
210   Tckk = V(10:12,12)/sc;
211   
212        omckk = rodrigues(Rckk);
213   Rckk = rodrigues(omckk);
214   
215end;
Note: See TracBrowser for help on using the repository browser.