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

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