1 | function [xd,dxddk] = apply_distortion(x,k)
|
---|
2 |
|
---|
3 |
|
---|
4 | % Complete the distortion vector if you are using the simple distortion model:
|
---|
5 | length_k = length(k);
|
---|
6 | if length_k <5 ,
|
---|
7 | k = [k ; zeros(5-length_k,1)];
|
---|
8 | end;
|
---|
9 |
|
---|
10 |
|
---|
11 | [m,n] = size(x);
|
---|
12 |
|
---|
13 | % Add distortion:
|
---|
14 |
|
---|
15 | r2 = x(1,:).^2 + x(2,:).^2;
|
---|
16 |
|
---|
17 | r4 = r2.^2;
|
---|
18 |
|
---|
19 | r6 = r2.^3;
|
---|
20 |
|
---|
21 |
|
---|
22 | % Radial distortion:
|
---|
23 |
|
---|
24 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;
|
---|
25 |
|
---|
26 | if nargout > 1,
|
---|
27 | dcdistdk = [ r2' r4' zeros(n,2) r6'];
|
---|
28 | end;
|
---|
29 |
|
---|
30 |
|
---|
31 | xd1 = x .* (ones(2,1)*cdist);
|
---|
32 |
|
---|
33 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
|
---|
34 |
|
---|
35 | if nargout > 1,
|
---|
36 | dxd1dk = zeros(2*n,5);
|
---|
37 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
|
---|
38 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;
|
---|
39 | end;
|
---|
40 |
|
---|
41 |
|
---|
42 | % tangential distortion:
|
---|
43 |
|
---|
44 | a1 = 2.*x(1,:).*x(2,:);
|
---|
45 | a2 = r2 + 2*x(1,:).^2;
|
---|
46 | a3 = r2 + 2*x(2,:).^2;
|
---|
47 |
|
---|
48 | delta_x = [k(3)*a1 + k(4)*a2 ;
|
---|
49 | k(3) * a3 + k(4)*a1];
|
---|
50 |
|
---|
51 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
|
---|
52 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
|
---|
53 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);
|
---|
54 |
|
---|
55 | if nargout > 1,
|
---|
56 | ddelta_xdk = zeros(2*n,5);
|
---|
57 | ddelta_xdk(1:2:end,3) = a1';
|
---|
58 | ddelta_xdk(1:2:end,4) = a2';
|
---|
59 | ddelta_xdk(2:2:end,3) = a3';
|
---|
60 | ddelta_xdk(2:2:end,4) = a1';
|
---|
61 | end;
|
---|
62 |
|
---|
63 | xd = xd1 + delta_x;
|
---|
64 |
|
---|
65 | if nargout > 1,
|
---|
66 | dxddk = dxd1dk + ddelta_xdk ;
|
---|
67 | if length_k < 5,
|
---|
68 | dxddk = dxddk(:,1:length_k);
|
---|
69 | end;
|
---|
70 | end;
|
---|
71 |
|
---|
72 |
|
---|
73 | return;
|
---|
74 |
|
---|
75 | % Test of the Jacobians:
|
---|
76 |
|
---|
77 | n = 10;
|
---|
78 |
|
---|
79 | lk = 1;
|
---|
80 |
|
---|
81 | x = 10*randn(2,n);
|
---|
82 | k = 0.5*randn(lk,1);
|
---|
83 |
|
---|
84 | [xd,dxddk] = apply_distortion(x,k);
|
---|
85 |
|
---|
86 |
|
---|
87 | % Test on k: OK!!
|
---|
88 |
|
---|
89 | dk = 0.001 * norm(k)*randn(lk,1);
|
---|
90 | k2 = k + dk;
|
---|
91 |
|
---|
92 | [x2] = apply_distortion(x,k2);
|
---|
93 |
|
---|
94 | x_pred = xd + reshape(dxddk * dk,2,n);
|
---|
95 |
|
---|
96 |
|
---|
97 | norm(x2-xd)/norm(x2 - x_pred)
|
---|