Structure and Motion Toolkit in MATLAB

Philip Torr (view profile)

04 Mar 2004 (Updated )

Structure and Motion Toolkit in MATLAB.

torr_errh(h, x1,y1,x2,y2, no_matches,m3)
%	By Philip Torr 2002
%
% %designed for the good of the world by Philip Torr
% copyright Philip Torr and Microsoft Corp 2002
% linear estimation of H

%this uses a costly but accurrate 1st order approximation to the optimal reprojection error
%as described in
%
%
% @article{Torr99c,
%         author = "Torr, P. H. S.   and Zisserman, A",
%         title ="MLESAC: A New Robust Estimator with Application to Estimating Image Geometry ",
%         journal = "CVIU",
%         Volume = {78},
%         number = 1,
%         pages = {138-156},
%         year = 2000}
%
% %MAPSAC is the Bayesian version of MLESAC, and it is easier to pronounce!
% it is described in:
%
% @article{Torr02d,
%         author = "Torr, P. H. S.",
%         title ="Bayesian Model Estimation and  Selection for Epipolar Geometry and
% Generic Manifold Fitting",
%         journal = "IJCV",
%         Volume = {?},
%         number = ?,
%         pages = {?},
%         url = "http://research.microsoft.com/~philtorr/",
%         year = 2002}
%

function e = torr_errh(h, x1,y1,x2,y2, no_matches,m3)
%disp('estimating error on h')
%this is transfer error from nx1 y1 to nx2 y2

A(:,1) = x1(:) .*m3;
A(:,2) = y1(:) .*m3;
A(:,3) = m3 *m3;

A(:,4) = 0;
A(:,5) = 0;
A(:,6) = 0;

A(:,7) = x1(:) .* x2(:);
A(:,8) = y1(:) .* x2(:);
A(:,9) = m3    .* x2(:);

B(:,4) = x1(:) .*m3;
B(:,5) = y1(:) .*m3;
B(:,6) = m3 *m3;

B(:,1) = 0;
B(:,2) = 0;
B(:,3) = 0;

B(:,7) = x1(:) .* y2(:);
B(:,8) = y1(:) .* y2(:);
B(:,9) = m3    .* y2(:);

r1 = A * h;
r2 = B * h;

%dx
dA1 =    h(1)* m3 + h(7) * x2(:);
%dy
dA2 =      h(2) * m3  + h(8) * x2(:);
%dx2
dA3  =    h(7) * x1(:) +   h(8) * y1(:) +   h(9) * m3;
%dy2
dA4 = 0;

%x1
dB1 =  h(4)*m3 + h(7)*y2(:);
%y1
dB2 =  h(5)*m3 + h(8)*y2(:);
%x2
dB3 =  0.0;
%y2
dB4 = h(7)*x1(:)+h(8)*y1(:)+h(9) * m3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%should compare this to one below %%%%%%%%%%
magG1 = sqrt(dA1.^2 + dA2.^2 + dA3.^2 + dA4.^2);
magG2 = sqrt(dB1.^2 + dB2.^2 + dB3.^2 + dB4.^2);
magG1G2 = dA1 .* dB1 + dA2 .* dB2 + dA3 .* dB3 + dA4 .* dB4;

angle = acos(magG1G2 ./ (magG1 .* magG2));

%    r1
%    magG1
D1 = r1 ./ magG1;
D2 = r2 ./ magG2;

sina = sin(angle);
%remove divide by zero
sina(find(sina == 0)) = 0.00000001;

r = (D1 .* D1 + D2 .* D2 - 2 *  D1 .* D2 .* cos (angle)) ./ sina;
e = r;
% this is squared residual?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%should compare this to one below %%%%%%%%%%

%   C = [dA; dB];
%   G = C * C';
%   H = G^-1;	%Inverse Hessian to get covariance matrix
%   rv = [ r1 r2];
%   r = rv * H * rv';
%   e(i) = sqrt(r);

%e(i) = sqrt((tx2(i) - x2(i))^2 + (ty2(i) - y2(i))^2);
%sse = norm(e);
%disp('finished estimating error on f')