[37] | 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)
|
---|