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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
function [Irec] = rect(I,R,f,c,k,alpha,KK_new);
if nargin < 5,
k = 0;
if nargin < 4,
c = [0;0];
if nargin < 3,
f = [1;1];
if nargin < 2,
R = eye(3);
if nargin < 1,
error('ERROR: Need an image to rectify');
break;
end;
end;
end;
end;
end;
if nargin < 7,
if nargin < 6,
KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1];
else
KK_new = alpha; % the 6th argument is actually KK_new
end;
alpha = 0;
end;
% Note: R is the motion of the points in space
% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame.
if ~exist('KK_new'),
KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1];
end;
[nr,nc] = size(I);
Irec = 255*ones(nr,nc);
[mx,my] = meshgrid(1:nc, 1:nr);
px = reshape(mx',nc*nr,1);
py = reshape(my',nc*nr,1);
rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))];
% Rotation: (or affine transformation):
rays2 = R'*rays;
x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)];
% Add distortion:
k1 = k(1);
k2 = k(2);
p1 = k(3);
p2 = k(4);
r_2 = sum(x.^2);
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,:)];
xd = (ones(2,1)*( 1 + k1 * r_2 + k2 * r_2.^2)) .* x + delta_x;
% Reconvert in pixels:
px2 = f(1)*(xd(1,:)+alpha_c*xd(2,:))+c(1);
py2 = f(2)*xd(2,:)+c(2);
% Interpolate between the closest pixels:
px_0 = floor(px2);
px_1 = px_0 + 1;
alpha_x = px2 - px_0;
py_0 = floor(py2);
py_1 = py_0 + 1;
alpha_y = py2 - py_0;
good_points = find((px_0 >= 0) & (px_1 <= (nc-1)) & (py_0 >= 0) & (py_1 <= (nr-1)));
I_lu = I(px_0(good_points) * nr + py_0(good_points) + 1);
I_ru = I(px_1(good_points) * nr + py_0(good_points) + 1);
I_ld = I(px_0(good_points) * nr + py_1(good_points) + 1);
I_rd = I(px_1(good_points) * nr + py_1(good_points) + 1);
I_interp = (1 - alpha_y(good_points)).*((1 - alpha_x(good_points)).* I_lu + alpha_x(good_points) .* I_ru) + alpha_y(good_points) .* ((1 - alpha_x(good_points)).* I_ld + alpha_x(good_points) .* I_rd);
Irec((px(good_points)-1)*nr + py(good_points)) = I_interp;
return;
% Convert in indices:
fact = 3;
[XX,YY]= meshgrid(1:nc,1:nr);
[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr);
%tic;
Iinterp = interp2(XX,YY,I,XXi,YYi);
%toc
[nri,nci] = size(Iinterp);
ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1;
ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1;
good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri));
|
