%用模擬圖像計算基本矩陣
clear;
close;
clc;
clf;
% 設(shè)置三維模擬立方體的參數(shù)
length1=1020;length2=720;
position1=[0 900 3300];position2=[0 -900 2800];
angle1=[pi/5 pi/3 0];angle2=[pi/4 0 pi/6];


% 生成立方體坐標(biāo)矩陣
X1=model(length1,position1,angle1);
X2=model(length2,position2,angle2);


% 顯示兩個3D模擬立方體
hold on;
plotobject(X1,'r');
plotobject(X2,'b');
hold off;
title('3-D模擬立方體')
XLABEL('X-axis')
YLABEL('Y-axis')
ZLABEL('Z-axis')
axis equal;
grid on;


% 設(shè)置攝像機內(nèi)參和外參
K=[500 0 1500;0 500 200;0 0 1];
angle=[pi/3 0 0];
position=[0 3500*sqrt(3)/2 3500/2];

% 生成兩個攝像機矩陣 其中第一個攝像機的坐標(biāo)系與世界坐標(biāo)系重合
[P1,P2]=camera(K,position,angle);


% 分別用兩個攝像機矩陣對兩個立方體進(jìn)行投影 生成兩幅二維圖像
x1=imposition(X1,P1,8)
x11=imposition(X2,P1,8)
x2=imposition(X1,P2,8);
x22=imposition(X2,P2,8);

figure;
% 顯示第一個攝像機的投影圖像
subplot(1,2,1);
hold on;
axis equal;
grid;
title('2D模擬圖像1');
XLABEL('X-axis');
YLABEL('Y-axis');
plotobject(x1,'r');
plotobject(x11,'b');

% 顯示第二個攝像機的投影圖像   
subplot(1,2,2);
hold on;
axis equal;
grid ;
title('2D模擬圖像2');
XLABEL('X-axis');
YLABEL('Y-axis');
plotobject(x2,'r');
plotobject(x22,'b');


%兩個立方體（16點）的結(jié)果

Q1=[x1 x11];
Q2=[x2 x22];
% Q1=x1;
% Q2=x2;
file=fopen('matches.txt','w');
Q=[Q1;Q2];
fprintf(file,'%14.7f %14.7f %14.7f %14.7f \n',Q(1:4,:)');
fclose(file);

Q1=[Q1(1,:);Q1(2,:)];
[T1,Q11]=unit(Q1,16);
Q2=[Q2(1,:);Q2(2,:)];
[T2,Q12]=unit(Q2,16);

f=solution(Q11,Q12,16)
% x1=[x1(1,:);x1(2,:)];
% [t1,x1]=unit(x1,k);
% x2=[x2(1,:);x2(2,:)];
% [t2,x2]=unit(x2,k);


% 用16對匹配點建立方程組求解F
F=solution(Q1,Q2,16)
% F(1,:)=F(1,:)/F(3,3);
% F(2,:)=F(2,:)/F(3,3);
% F(3,1)=F(3,1)/F(3,3);
% F(3,2)=F(3,2)/F(3,3);
% F(3,3)=1;

file=fopen('data_F.txt','w');
fprintf(file,'%14.7f %14.7f %14.7f\n',F(:,1:3)');
fclose(file);
% if(rank(f)~=2)
%     [U D V]=svd(f);
%     f=U*[D(1,1) 0 0;0 D(2,2) 0;0 0 0]*V;
% end
% F=t2'*f*t1
% % F=T2'*f*T1;