用法同 openCV, 使用最小二乘迭代
function corners_tuned = refine_pos( src, corners) % corners: N*2
%UNTITLED Summary of this function goes here
% Detailed explanation goes here
corners_tuned = corners;
% some parameters needed to set;
win.width = 3;
win.height = 3;
eps = 0.00001;
max_iters = 20;
count = size(corners, 1);
% parameters derived;
win_w = win.width*2 + 1;
win_h = win.width*2 + 1;
coeff = 1.0/(win.width*win.width);
maskX = zeros(win_w, 1, 'double');
maskY = zeros(win_h, 1, 'double');
mask = zeros(win_h, win_w, 'double');
for i = -win.width:1:win.width
maskX(i + win.width + 1) = exp(-i*i*coeff);
end
maskY = maskX;
for i = 1:1:win_w
for j = 1:1:win_h
mask(i, j) = maskX(j)*maskY(i);
end
end
for pt_i = 1:1:count
ct = corners(pt_i, :);
iter = 0;
err = 0.0;
nci.x = ct(1, 2);
nci.y = ct(1, 1);
str = sprintf('============== p%d iteration =============', pt_i);
disp(str);
while (1)
% s1: get the sub-pixsel data;
ci.x = nci.x;
ci.y = nci.y;
ix = 1:1:640;
iy = 1:1:480;
src_buffer = zeros(win_h + 2, win_w + 2, 'double');
for i = (-win.height-1):1:(win.height+1)
for j = (-win.width-1):1:(win.width+1)
src_buffer(i + win.height + 1 + 1, j + win.width + 1 + 1) = interp2(ix, iy, src, ci.x + j, ci.y + i);
end
end
% s2: caculate gx & gy
gx = zeros(win_h, win_w, 'double');
gy = zeros(win_h, win_w, 'double');
for i = 2:1:win_h+1
for j = 2:1:win_w+1
gx(i-1, j-1) = (src_buffer(i, j+1) - src_buffer(i, j-1))/2;
gy(i-1, j-1) = (src_buffer(i+1, j) - src_buffer(i-1, j))/2;
end
end
% s3: prcoess gradient
a = 0.0;
b = 0.0;
c = 0.0;
bb1 = 0.0;
bb2 = 0.0;
for i = (-win.height):1:(win.height)
for j = (-win.width):1:(win.width)
ky = i + win.height + 1;
kx = j + win.width + 1;
m = mask(ky, kx);
tgx = gx(ky, kx);
tgy = gy(ky, kx);
gxx = tgx*tgx*m;
gxy = tgx*tgy*m;
gyy = tgy*tgy*m;
a = a + gxx;
b = b + gxy;
c = c + gyy;
bb1 = bb1 + gxx*j + gxy*i;
bb2 = bb2 + gxy*j + gyy*i;
end
end
% s4: get new position and iter once again;
A = [a, b; b, c];
DA = inv(A);
nci.x = ci.x + DA(1,1)*bb1 + DA(1,2)*bb2;
nci.y = ci.y + DA(2,1)*bb1 + DA(2,2)*bb2;
iter = iter + 1;
if (iter >= max_iters)
break;
end
err = (nci.x - ci.x)^2 + (nci.y - ci.y)^2;
if (abs(err) < eps)
break;
end
str = sprintf('[iteration %d]: (%f, %f) , (%f, %f), err = %f', iter, ci.y, nci.y, ci.x, nci.x, err);
disp(str);
end
if ( abs(nci.x - ct(1,2)) < 2 && abs(nci.y - ct(1,1)) < 2 )
str = sprintf('[%f, %f] , [%f %f]', corners_tuned(pt_i,1), nci.y, corners_tuned(pt_i,2), nci.x);
disp(str);
corners_tuned(pt_i,
= [nci.y, nci.x];
end
end
end