-
Notifications
You must be signed in to change notification settings - Fork 0
/
comp_distortion_oulu.m
52 lines (40 loc) · 1.41 KB
/
comp_distortion_oulu.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
function [x] = comp_distortion_oulu(xd,k);
%comp_distortion_oulu.m
%
%[x] = comp_distortion_oulu(xd,k)
%
%Compensates for radial and tangential distortion. Model From Oulu university.
%For more informatino about the distortion model, check the forward projection mapping function:
%project_points.m
%
%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix)
% k: Distortion coefficients (radial and tangential) (4x1 vector)
%
%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix)
%
%Method: Iterative method for compensation.
%
%NOTE: This compensation has to be done after the subtraction
% of the principal point, and division by the focal length.
if length(k) == 1,
[x] = comp_distortion(xd,k);
else
k1 = k(1);
k2 = k(2);
k3 = k(5);
p1 = k(3);
p2 = k(4);
x = xd; % initial guess
for kk=1:20,
r_2 = sum(x.^2);
% fprintf('=== r_2: %f ',r_2(1));
k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3;
% fprintf('=== k_radial: %f ',k_radial(1));
delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2);
p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];
% fprintf('=== delta_x: %f %f',delta_x(1,1), delta_x(2,1));
x = (xd - delta_x)./(ones(2,1)*k_radial);
% fprintf('=== x: %f %f\n',x(1,1), x(2,1));
end;
end;
end