%B. Vandeportaele
%script to illustrate uncertainty propagation for homography (projective
%transformation)
clear all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%define 4 set of 2 2D points correspondances such as
% w.u21 u11
% w.v21 = H . v11
% w. 1
% position u11,v11 in plane 1 is supposed to be perfectly known
u11=0;v11=0;
u12=1;v12=0;
u13=0;v13=1;
u14=1;v14=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%position u21,v21 in plane 2 is associated with a covariance matrix
u21=10;v21=10;
u22=150;v22=40;
u23=30;v23=130;
u24=190;v24=210;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%covariance for the point's positions in plane 2. std dev are square roots
%of the diagonal values
%default value:
Cp=1*eye(8)
%change the value as needed if uncertainty are not the same for all the
%points
if 1
Cp(1,1)=3;
Cp(2,2)=5;
Cp(3,3)=10;
Cp(4,4)=25;
Cp(5,5)=50;
Cp(6,6)=1;
Cp(7,7)=0.1;
Cp(8,8)=0.1;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%store coordinates in matrices
p1=[u11,u12,u13,u14;v11,v12,v13,v14]
p2=[u21,u22,u23,u24;v21,v22,v23,v24]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DLT Algorithm:
[ H] = compute2DHomographyUsingDLT( p1,p2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%check computed H, should be 0
[ err] = check2DHomography(H, p1,p2 )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h11=H(1,1);h12=H(1,2);h13=H(1,3);
h21=H(2,1);h22=H(2,2);h23=H(2,3);
h31=H(3,1);h32=H(3,2);h33=H(3,3);
%compute J numerically then invert the 8x8 matrix to obtain Ji
J =[];
% computed in jaco.m: J=jacobian ([u2, v2], [h11, h12, h13, h21, h22, h23, h31, h32])
for i=1:4
u1=p1(1,i);v1=p1(2,i);
J=[J;
u1/(h31*u1 + h32*v1 + 1), v1/(h31*u1 + h32*v1 + 1), 1/(h31*u1 + h32*v1 + 1), 0, 0, 0, -(u1*(h13 + h11*u1 + h12*v1))/(h31*u1 + h32*v1 + 1)^2, -(v1*(h13 + h11*u1 + h12*v1))/(h31*u1 + h32*v1 + 1)^2;
0, 0, 0, u1/(h31*u1 + h32*v1 + 1), v1/(h31*u1 + h32*v1 + 1), 1/(h31*u1 + h32*v1 + 1), -(u1*(h23 + h21*u1 + h22*v1))/(h31*u1 + h32*v1 + 1)^2, -(v1*(h23 + h21*u1 + h22*v1))/(h31*u1 + h32*v1 + 1)^2];
end
J
detJ=det(J)
%check if detJ is high enough, otherwise it won't be invertible in
%practice
if detJ<0.01
display 'the system looks ill-conditionned'
return;
end
Ji=inv(J)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% In case we want to estimate H from more than 4 correspondances, get from:
% http://stackoverflow.com/questions/30737382/getting-covariance-matrix-when-using-levenberg-marquardt-lsqcurvefit-in-matlab
% I think I have solved this myself but will post how here if anyone else is having the same trouble.
% The covariance matrix can be calculated from the Jacobian by:
% C = inv(J'*J)*MSE
% Where MSE is mean-square error:
% MSE = (R'*R)/(N-p)
% Where R = residuals, N = number of observations, p = number of coefficients estimated.
% Or MSE can be calculated via iteration.
% Hopefully this will help someone else in the future.
% If anyone spots error please let me know. Thanks
%QUESTION????? in that case what is the inverse of the (non square) jacobian for the inverse homography H^-1
%propagation of thee covariance to the coefficients of H
Ch=Ji*Cp*Ji'
detCh=det(Ch) %what is the meaning of that value: look at
%https://math.stackexchange.com/questions/889425/what-does-determinant-of-covariance-matrix-give
%compute the covariance on the coefficients of the 2D points, it should give
%the same value than Cp
Cp2=J*Ch*J'
ErrorCp=Cp-Cp2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot the uncertainty on a grid sampled in plane 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all
figure
hold on
axis equal
for u15=-2:.25:2;
for v15=-2:.25:2;
wuv25=H*[u15;v15;1];
u25=wuv25(1)/wuv25(3);
v25=wuv25(2)/wuv25(3);
Jp=[ u15/(h31*u15 + h32*v15 + 1), v15/(h31*u15 + h32*v15 + 1), 1/(h31*u15 + h32*v15 + 1), 0, 0, 0, -(u15*(h13 + h11*u15 + h12*v15))/(h31*u15 + h32*v15 + 1)^2, -(v15*(h13 + h11*u15 + h12*v15))/(h31*u15 + h32*v15 + 1)^2;
0, 0, 0, u15/(h31*u15 + h32*v15 + 1), v15/(h31*u15 + h32*v15 + 1), 1/(h31*u15 + h32*v15 + 1), -(u15*(h23 + h21*u15 + h22*v15))/(h31*u15 + h32*v15 + 1)^2, -(v15*(h23 + h21*u15 + h22*v15))/(h31*u15 + h32*v15 + 1)^2];
Cp5=Jp*Ch*Jp';
plotcovar(u25,v25,Cp5,'r+','b-');
end
end
%plot the covariance for the four 2D point correspondances used by DLT
for i=1:4
u2=p2(1,i);v2=p2(2,i);
Cpi=Cp(1+(i-1)*2:2+(i-1)*2,1+(i-1)*2:2+(i-1)*2);
plotcovar(u2,v2,Cpi,'g+','g-');
end
%en vert, 4 points (et leur incertitude) utilisés pour estimer l'homographie par Direct Linear Transform
%en rouge, des points auquel on applique l'homographie calculée (interpolation et extrapolation)
%et en bleu l'incertitude de l'homographie estimé propagée à ces points