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

Last change on this file since 907 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 10.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 [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)
20
21%project_points2.m
22%
23%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
24%
25%Projects a 3D structure onto the image plane.
26%
27%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
28%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
29%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
30%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
31%       c: principal point location in pixel units (2x1 vector)
32%       k: Distortion coefficients (radial and tangential) (4x1 vector)
33%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
34%
35%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
36%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
37%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
38%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
39%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
40%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
41%
42%Definitions:
43%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
44%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
45%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
46%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
47%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
48%call r^2 = a^2 + b^2.
49%The distorted point coordinates are: xd = [xx;yy] where:
50%
51%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
52%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
53%
54%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion
55%
56%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
57%
58%xxp = f(1)*(xx + alpha*yy) + c(1)
59%yyp = f(2)*yy + c(2)
60%
61%
62%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
63%
64%
65%Important function called within that program:
66%
67%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
68%
69%rigid_motion.m: Computes the rigid motion transformation of a given structure
70
71
72if nargin < 7,
73    alpha = 0;
74    if nargin < 6,
75        k = zeros(5,1);
76        if nargin < 5,
77            c = zeros(2,1);
78            if nargin < 4,
79                f = ones(2,1);
80                if nargin < 3,
81                    T = zeros(3,1);
82                    if nargin < 2,
83                        om = zeros(3,1);
84                        if nargin < 1,
85                            error('Need at least a 3D structure to project (in project_points.m)');
86                            return;
87                        end;
88                    end;
89                end;
90            end;
91        end;
92    end;
93end;
94
95
96[m,n] = size(X);
97
98if nargout > 1,
99    [Y,dYdom,dYdT] = rigid_motion(X,om,T);
100else
101    Y = rigid_motion(X,om,T);
102end;
103
104
105inv_Z = 1./Y(3,:);
106
107x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;
108
109
110bb = (-x(1,:) .* inv_Z)'*ones(1,3);
111cc = (-x(2,:) .* inv_Z)'*ones(1,3);
112
113if nargout > 1,
114    dxdom = zeros(2*n,3);
115    dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
116    dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);
117
118    dxdT = zeros(2*n,3);
119    dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
120    dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);
121end;
122
123
124% Add distortion:
125
126r2 = x(1,:).^2 + x(2,:).^2;
127
128if nargout > 1,
129    dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
130    dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);
131end;
132
133
134r4 = r2.^2;
135
136if nargout > 1,
137    dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
138    dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;
139end
140
141r6 = r2.^3;
142
143if nargout > 1,
144    dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;
145    dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;
146end;
147
148% Radial distortion:
149
150cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;
151
152if nargout > 1,
153    dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;
154    dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;
155    dcdistdk = [ r2' r4' zeros(n,2) r6'];
156end;
157
158xd1 = x .* (ones(2,1)*cdist);
159
160if nargout > 1,
161    dxd1dom = zeros(2*n,3);
162    dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
163    dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
164    coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
165    dxd1dom = dxd1dom + coeff.* dxdom;
166
167    dxd1dT = zeros(2*n,3);
168    dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
169    dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
170    dxd1dT = dxd1dT + coeff.* dxdT;
171
172    dxd1dk = zeros(2*n,5);
173    dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
174    dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;
175end;
176
177
178% tangential distortion:
179
180a1 = 2.*x(1,:).*x(2,:);
181a2 = r2 + 2*x(1,:).^2;
182a3 = r2 + 2*x(2,:).^2;
183
184delta_x = [k(3)*a1 + k(4)*a2 ;
185    k(3) * a3 + k(4)*a1];
186
187
188%ddelta_xdx = zeros(2*n,2*n);
189aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
190bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
191cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);
192
193if nargout > 1,
194    ddelta_xdom = zeros(2*n,3);
195    ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
196    ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);
197
198    ddelta_xdT = zeros(2*n,3);
199    ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
200    ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);
201
202    ddelta_xdk = zeros(2*n,5);
203    ddelta_xdk(1:2:end,3) = a1';
204    ddelta_xdk(1:2:end,4) = a2';
205    ddelta_xdk(2:2:end,3) = a3';
206    ddelta_xdk(2:2:end,4) = a1';
207end;
208
209
210xd2 = xd1 + delta_x;
211
212if nargout > 1,
213    dxd2dom = dxd1dom + ddelta_xdom ;
214    dxd2dT = dxd1dT + ddelta_xdT;
215    dxd2dk = dxd1dk + ddelta_xdk ;
216end;
217
218
219% Add Skew:
220
221xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];
222
223% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha
224if nargout > 1,
225    dxd3dom = zeros(2*n,3);
226    dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
227    dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
228    dxd3dT = zeros(2*n,3);
229    dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
230    dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
231    dxd3dk = zeros(2*n,5);
232    dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
233    dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
234    dxd3dalpha = zeros(2*n,1);
235    dxd3dalpha(1:2:2*n,:) = xd2(2,:)';
236end;
237
238
239
240% Pixel coordinates:
241if length(f)>1,
242    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
243    if nargout > 1,
244        coeff = reshape(f*ones(1,n),2*n,1);
245        dxpdom = (coeff*ones(1,3)) .* dxd3dom;
246        dxpdT = (coeff*ones(1,3)) .* dxd3dT;
247        dxpdk = (coeff*ones(1,5)) .* dxd3dk;
248        dxpdalpha = (coeff) .* dxd3dalpha;
249        dxpdf = zeros(2*n,2);
250        dxpdf(1:2:end,1) = xd3(1,:)';
251        dxpdf(2:2:end,2) = xd3(2,:)';
252    end;
253else
254    xp = f * xd3 + c*ones(1,n);
255    if nargout > 1,
256        dxpdom = f  * dxd3dom;
257        dxpdT = f * dxd3dT;
258        dxpdk = f  * dxd3dk;
259        dxpdalpha = f .* dxd3dalpha;
260        dxpdf = xd3(:);
261    end;
262end;
263
264if nargout > 1,
265    dxpdc = zeros(2*n,2);
266    dxpdc(1:2:end,1) = ones(n,1);
267    dxpdc(2:2:end,2) = ones(n,1);
268end;
269
270
271return;
272
273% Test of the Jacobians:
274
275n = 10;
276
277X = 10*randn(3,n);
278om = randn(3,1);
279T = [10*randn(2,1);40];
280f = 1000*rand(2,1);
281c = 1000*randn(2,1);
282k = 0.5*randn(5,1);
283alpha = 0.01*randn(1,1);
284
285[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);
286
287
288% Test on om: OK
289
290dom = 0.000000001 * norm(om)*randn(3,1);
291om2 = om + dom;
292
293[x2] = project_points2(X,om2,T,f,c,k,alpha);
294
295x_pred = x + reshape(dxdom * dom,2,n);
296
297
298norm(x2-x)/norm(x2 - x_pred)
299
300
301% Test on T: OK!!
302
303dT = 0.0001 * norm(T)*randn(3,1);
304T2 = T + dT;
305
306[x2] = project_points2(X,om,T2,f,c,k,alpha);
307
308x_pred = x + reshape(dxdT * dT,2,n);
309
310
311norm(x2-x)/norm(x2 - x_pred)
312
313
314
315% Test on f: OK!!
316
317df = 0.001 * norm(f)*randn(2,1);
318f2 = f + df;
319
320[x2] = project_points2(X,om,T,f2,c,k,alpha);
321
322x_pred = x + reshape(dxdf * df,2,n);
323
324
325norm(x2-x)/norm(x2 - x_pred)
326
327
328% Test on c: OK!!
329
330dc = 0.01 * norm(c)*randn(2,1);
331c2 = c + dc;
332
333[x2] = project_points2(X,om,T,f,c2,k,alpha);
334
335x_pred = x + reshape(dxdc * dc,2,n);
336
337norm(x2-x)/norm(x2 - x_pred)
338
339% Test on k: OK!!
340
341dk = 0.001 * norm(k)*randn(5,1);
342k2 = k + dk;
343
344[x2] = project_points2(X,om,T,f,c,k2,alpha);
345
346x_pred = x + reshape(dxdk * dk,2,n);
347
348norm(x2-x)/norm(x2 - x_pred)
349
350
351% Test on alpha: OK!!
352
353dalpha = 0.001 * norm(k)*randn(1,1);
354alpha2 = alpha + dalpha;
355
356[x2] = project_points2(X,om,T,f,c,k,alpha2);
357
358x_pred = x + reshape(dxdalpha * dalpha,2,n);
359
360norm(x2-x)/norm(x2 - x_pred)
Note: See TracBrowser for help on using the repository browser.