clc;
clear;
M=load('Model.txt');
m1=load('data1.txt');
m2=load('data2.txt');
m3=load('data3.txt');
M1=[M(:,1:2) ; M(:,3:4) ; M(:,5:6) ; M(:,7:8)];
m1=[m1(:,1:2) ; m1(:,3:4) ; m1(:,5:6) ; m1(:,7:8)];
m2=[m2(:,1:2) ; m2(:,3:4) ; m2(:,5:6) ; m2(:,7:8)];
m3=[m3(:,1:2) ; m3(:,3:4) ; m3(:,5:6) ; m3(:,7:8)];
mm(:,:,1)=m1';
mm(:,:,2)=m2';
mm(:,:,3)=m3';
[rows,npts]=size(M1');%npts 为列数
matrixone=ones(1,npts);% 1矩阵
M2=[M1';matrixone];%增加一行 1 1
num=size(mm,3)
for i=1:num
mm(3,:,i)=matrixone;
end
x1=M2;
x2=mm;
Npts = length(x1);
A = zeros(3*Npts,9);
O = [0 0 0];
for n = 1:Npts
X = x1(:,n)';%定义
x = x2(1,n); y = x2(2,n); w = x2(3,n);
A(3*n-2,:) = [ O -w*X y*X];
A(3*n-1,:) = [ w*X O -x*X];
A(3*n ,:) = [-y*X x*X O ]
end
[U,D,V] = svd(A)
% Ax=b x=A\b;
% Extract homography单应性矩阵
H = reshape(V(:,9),3,3)';
H=H/H(3,3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Maximun likelihood estimation for the H最大似然估计
% using the function(10), P7
%options = optimset('LargeScale','off','LevenbergMarquardt','on');
%[x,resnorm,residual,exitflag,output] = lsqnonlin( @simon_H, reshape(H,1,9) , [],[],options,mm, M2);
%H=reshape(x,3,3);
H1=H/H(3,3);
V=[];
v12(:,:)=[H(1,1)*H(2,1), H(1,1)*H(2,2)+H(1,2)*H(2,1), H(1,2)*H(2,2), H(1,3)*H(2,1)+H(1,1)*H(2,3), H(1,3)*H(2,2)+H(1,2)*H(2,3), H(1,3)*H(2,3)];
v11(:,:)=[H(1,1)*H(1,1), H(1,1)*H(1,2)+H(1,2)*H(1,1), H(1,2)*H(1,2), H(1,3)*H(1,1)+H(1,1)*H(1,3), H(1,3)*H(1,2)+H(1,2)*H(1,3), H(1,3)*H(1,3)];
v22(:,:)=[H(2,1)*H(2,1), H(2,1)*H(2,2)+H(2,2)*H(2,1), H(2,2)*H(2,2), H(2,3)*H(2,1)+H(2,1)*H(2,3), H(2,3)*H(2,2)+H(2,2)*H(2,3), H(2,3)*H(2,3)];
V=[V;v12(:,:);v11(:,:)-v22(:,:)]
k=V'*V;
[u,v,d]=svd(k);%奇异值分解[u,s,v]=svd(A),使得A=USV'
[e,d2]=eig(k);%Eigenvector特征向量 [V,D]=eig(A)使得 AV=VD，D是特征值对角阵,V是特征向量阵
b=d(:,6);%b就是论文作中B
v0=(b(2)*b(4)-b(1)*b(5))/(b(1)*b(3)-b(2)^2);
s=b(6)-(b(4)^2+v0*(b(2)*b(4)-b(1)*b(5)))/b(1);
alpha_u=sqrt(s/b(1));
alpha_v=sqrt(s*b(1)/(b(1)*b(3)-b(2)^2));
skewness=-b(2)*alpha_u*alpha_u*alpha_v/s;
u0=skewness*v0/alpha_u-b(4)*alpha_u*alpha_u/s;
A=[alpha_u skewness u0
0 alpha_v v0
0 0 1]
B=rotate(pi/4,pi/6,pi/3);
det(B);inv(B)*B
