Structure and Motion Toolkit in MATLAB

Philip Torr (view profile)

04 Mar 2004 (Updated )

Structure and Motion Toolkit in MATLAB.

torr_correctx4F(f, nx1,ny1,nx2,ny2, no_matches, m3)
```%	By Philip Torr 2002

%this function corrects all the points in an optimal (first order) manner so that they lie on the manifold
%getting the signs right is a bit tricky but basically the 1st order correction is
% x = x - (grad r) * (r / ( norm(grad r)^2 )

function [corrected_matches,e] = torr_correctx4F(f, nx1,ny1,nx2,ny2, no_matches, m3)

%disp('estimating squared errors on f')
f = f /norm(f);

r =    f(1) .* nx1(:).* nx2(:) +   f(2).* ny1(:).* nx2(:) + f(3) .* m3.* nx2(:);
r = r +   f(4) .* nx1(:).* ny2(:) +   f(5) .* ny1(:).* ny2(:)+   f(6) .* m3.* ny2(:);
r = r +   f(7) .* nx1(:).* m3+   f(8) .* ny1(:).* m3+   f(9) .* m3.* m3;
r2 = r.^2;

fdx1 =  f(1) .* nx2(:) + f(4) .* ny2(:) +   f(7) .* m3;
fdx2 =  f(1) .* nx1(:) + f(2).* ny1(:) + f(3) .* m3;
fdy1 =  f(2).* nx2(:) + f(5) .* ny2(:)+ f(8) .* m3;
fdy2 = f(4) .* nx1(:) + f(5) .* ny1(:)+ f(6) .* m3;

g = (fdx1 .* fdx1 +fdx2 .* fdx2 +fdy1 .* fdy1 +fdy2 .* fdy2);

e = r2./g;

g = sqrt(g);
e = -r./g;
%
corrected_matches(:,1) = nx1(:) + e(:) .* (fdx1(:) ./ g(:));
corrected_matches(:,2) = ny1(:) + e(:) .* (fdy1(:) ./ g(:));
corrected_matches(:,3) = nx2(:) + e(:) .* (fdx2(:) ./ g(:));
corrected_matches(:,4) = ny2(:) + e(:) .* (fdy2(:) ./ g(:));
%
%    corrected_matches(:,1) = nx1(:) + e(:) .* fdx1(:);
%    corrected_matches(:,2) = ny1(:) + e(:) .* fdy1(:);
%    corrected_matches(:,3) = nx2(:) + e(:) .* fdx2(:);
%    corrected_matches(:,4) = ny2(:) + e(:) .* fdy2(:);

e = e.^2;```