clear all
[FileName,PathName]=uigetfile('*.txt','打开文件');
if FileName==0
return
end
fn=fopen([PathName,FileName]);
%-------------------------------------------------数字内定向----------------------------------------------------
IJ=fscanf(fn,'%f',[2,4]);%4*2矩阵i前j后
IJ=IJ';
[c,r]=size(IJ);
A=[[linspace(1,1,c)]' IJ];
XY=fscanf(fn,'%f',[2,4]); %4*2矩阵x前y后
XY=XY';
NEI=inv(A'*A)*A'*XY; %3*2矩阵
clear c r A IJ XY
%--------------------------------------------------相对定向------------------------------------------------------
f=0.12;
IJ=fscanf(fn,'%f',[2,41]);%input('输入左相片同名点像素坐标矩阵(i前j后)');
IJ=IJ';
[c,r]=size(IJ);
XY=[[linspace(1,1,c)]' IJ]*NEI;
XYZz=[XY [linspace(-f,-f,c)]']'; %XYZz是左片像点坐标
IJ=fscanf(fn,'%f',[2,41]);%input('输入右相片同名点像素坐标矩阵(i前j后)');
IJ=IJ';
XY=[[linspace(1,1,c)]' IJ]*NEI;
XYZy=[XY [linspace(-f,-f,c)]']'; %XYZy右片像点坐标 %只剩下NEI XYZz XYZy f i
phy1=0;
kapa1=0;
phy2=0;
om2=0;
kapa2=0;
dphy1=10;
dkapa1=10;
dphy2=10;
dom2=10;
dkapa2=10;
while abs(dphy1)>=0.0003|abs(dkapa1)>=0.0003|abs(dphy2)>=0.0003|abs(dom2)>=0.0003|abs(dkapa2)>=0.0003
R1=[1 -kapa1 -phy1;kapa1 1 0;phy1 0 1];
R2=[1 -kapa2 -phy2;kapa2 1 -om2;phy2 om2 1];
XYZ1=R1*XYZz;
XYZ2=R2*XYZy;
for i=1:c
A(i,1)=-XYZ1(1,i)*XYZ2(2,i)/XYZ1(3,i);
A(i,2)=XYZ1(1,i);
A(i,3)=XYZ2(1,i)*XYZ1(2,i)/XYZ1(3,i);
A(i,4)=-(XYZ1(3,i)-XYZ1(2,i)*XYZ2(2,i)/XYZ1(3,i));
A(i,5)=-XYZ2(1,i);
l(i,1)=f*XYZ1(2,i)/XYZ1(3,i)-f*XYZ2(2,i)/XYZ2(3,i);
end
d=inv(A'*A)*A'*l;
dphy1=d(1,1);
dkapa1=d(2,1);
dphy2=d(3,1);
dom2=d(4,1);
dkapa2=d(5,1);
phy1=phy1+dphy1;
kapa1=kapa1+dkapa1;
phy2=phy2+dphy2;
om2=om2+dom2;
kapa2=kapa2+dkapa2;
end
clear i c r d A l R1 R2 IJ XY XYZz XYZy XYZ1 XYZ2 dphy1 dkapa1 dphy2 dom2 dkapa2 %只输出NEI f phy1 kapa1 phy2 om2 kapa2
%---------------------------------------------------前方交会----------------------------------------------------
IJ=fscanf(fn,'%f',[2,16]);
IJ=IJ';
[c,r]=size(IJ);
XY=[[linspace(1,1,c)]' IJ]*NEI;
XYZz=[XY [linspace(-f,-f,c)]']';
IJ=fscanf(fn,'%f',[2,16]);
IJ=IJ';
XY=[[linspace(1,1,c)]' IJ]*NEI;
XYZy=[XY [linspace(-f,-f,c)]']';
Bx=0.09216*(1-0.6)*10000;
R1(1,1)=cos(phy1)*cos(kapa1)-sin(phy1)*sin(0)*sin(kapa1);
R1(1,2)=-cos(phy1)*sin(kapa1)-sin(phy1)*sin(0)*cos(kapa1);
R1(1,3)=-sin(phy1)*cos(0);
R1(2,1)=cos(0)*sin(kapa1);
R1(2,2)=cos(0)*cos(kapa1);
R1(2,3)=-sin(0);
R1(3,1)=sin(phy1)*cos(kapa1)+cos(phy1)*sin(0)*sin(kapa1);
R1(3,2)=-sin(phy1)*sin(kapa1)+cos(phy1)*sin(0)*cos(kapa1);
R1(3,3)=cos(phy1)*cos(0);
R2(1,1)=cos(phy2)*cos(kapa2)-sin(phy2)*sin(om2)*sin(kapa2);
R2(1,2)=-cos(phy2)*sin(kapa2)-sin(phy2)*sin(om2)*cos(kapa2);
R2(1,3)=-sin(phy2)*cos(om2);
R2(2,1)=cos(om2)*sin(kapa2);
R2(2,2)=cos(om2)*cos(kapa2);
R2(2,3)=-sin(om2);
R2(3,1)=sin(phy2)*cos(kapa2)+cos(phy2)*sin(om2)*sin(kapa2);
R2(3,2)=-sin(phy2)*sin(kapa2)+cos(phy2)*sin(om2)*cos(kapa2);
R2(3,3)=cos(phy2)*cos(om2);
XYZ1=R1*XYZz;
XYZ2=R2*XYZy;
for i=1:c
N1=Bx*XYZ2(3,i)/(XYZ1(1,i)*XYZ2(3,i)-XYZ2(1,i)*XYZ1(3,i));
N2=Bx*XYZ1(3,i)/(XYZ1(1,i)*XYZ2(3,i)-XYZ2(1,i)*XYZ1(3,i));
XYZa(1,i)=N1*XYZ1(1,i);
XYZa(2,i)=0.5*(N1*XYZ1(2,i)+N2*XYZ2(2,i));
XYZa(3,i)=N1*XYZ1(3,i)+1200;
Q(i,1)=N1*XYZ1(2,i)-N2*XYZ2(2,i);
end
clear i r N1 N2 R1 R2 Bx IJ XY XYZz XYZy XYZ1 XYZ2 %只输出NEI f phy1 kapa1 phy2 om2 kapa2 XYZa
%---------------------------------------------------绝对定向-----------------------------------------------------
XYZ=fscanf(fn,'%f',[3,16]);
XYZ=[XYZ(2,:);XYZ(1,:);XYZ(3,:)];
dx=0;
dy=0;
dz=0;
dk=0;
ex=0;
ey=0;
ez=0;
d(1:7,:)=linspace(10,10,7)';
while abs(d(1,1))>=0.000001|abs(d(2,1))>=0.000001|abs(d(3,1))>=0.000001|abs(d(4,1))>=0.000001|abs(d(5,1))>=0.000001|abs(d(6,1))>=0.000001|abs(d(7,1))>=0.000001
R(1,1)=cos(ey)*cos(ez);
R(1,2)=cos(ex)*sin(ez)+sin(ex)*sin(ey)*cos(ez);
R(1,3)=sin(ex)*sin(ez)-cos(ex)*sin(ey)*cos(ez);
R(2,1)=-cos(ey)*sin(ez);
R(2,2)=cos(ex)*cos(ez)-sin(ex)*sin(ey)*sin(ez);
R(2,3)=sin(ex)*cos(ez)+cos(ex)*sin(ey)*sin(ez);
R(3,1)=sin(ey);
R(3,2)=-sin(ex)*cos(ey);
R(3,3)=cos(ex)*cos(ey);
for i= 1:c
A(3*i-2:3*i,1:3)=eye(3,3);
A(3*i-2:3*i,4:7)=[XYZa(1,i) 0 -XYZa(3,i) XYZa(2,i)
XYZa(2,i) XYZa(3,i) 0 -XYZa(1,i)
XYZa(3,i) -XYZa(2,i) XYZa(1,i) 0];
L(3*i-2:3*i,1)=XYZ(:,i)-(1+dk)*R*XYZa(:,i)-[dx;dy;dz];
end
d=inv(A'*A)*A'*L;
dx=dx+d(1,1);
dy=dy+d(2,1);
dz=dz+d(3,1);
dk=dk+d(4,1);
ex=ex+d(5,1);
ey=ey+d(6,1);
ez=ez+d(7,1);
end
clear i c r d A L R XYZ
%------------------------------------------写文件----------------------------------------------
fud=fopen(strcat(PathName,'计算结果.txt'),'wt');
fprintf(fud,'内定向参数:\n h0 h1 h2\n');
fprintf(fud,'%10.6f %10.6f %10.6f\n',[NEI(1,1) NEI(2,1) NEI(3,1)]);
fprintf(fud,' k0 k1 k2\n');
fprintf(fud,'%10.6f %10.6f %10.6f\n',[NEI(1,2) NEI(2,2) NEI(3,2)]);
fprintf(fud,'相对定向参数:\n Φ1 Κ1 Φ2 Κ2 Ω2\n');
fprintf(fud,'%10.6f %10.6f %10.6f %10.6f %10.6f\n',[phy1 kapa1 phy2 kapa2 om2]);
fprintf(fud,'绝对定向参数:\n ΔX ΔY ΔZ λ εx εy εz\n');
fprintf(fud,'%10.6f %10.6f %10.6f %10.6f %10.6f %10.6f %10.6f\n',[dx dy dz 1+dk ex ey ez]);
fclose all;
open(strcat(PathName,'计算结果.txt'));
