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

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

geometry cqlib updated

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