f=0.150;faiz=0;omiz=0;kaz=0;
Xdz=[5083.205,5780.02,5210.879,5909.264];
Ydz=[5852.099,5906.365,4258.446,4314.283];
Zdz=[527.925,571.549,461.81,455.484];
%以上为输入gcp1到gcp4的地面坐标
xxz=[0.016012,0.08856,0.013362,0.08224];
yxz=[0.079963,0.081134,-0.07937,-0.080027];
%右片像点坐标
Xsz=(5083.205+5780.02+5210.879+5909.264)/4;
Ysz=(5852.099+5906.365+4258.446+4314.283)/4;
Sxz=sqrt((xxz(2)-xxz(1))^2+(yxz(2)-yxz(1))^2);
Sdz=sqrt((Xdz(2)-Xdz(1))^2+(Ydz(2)-Ydz(1))^2);
mz=10000
Zsz=mz*f+1/4*(Zdz(1)+Zdz(2)+Zdz(3)+Zdz(4));
Xz=[1;1;1;1;1;1];
while 1
a1z=cos(faiz)*cos(kaz)-sin(faiz)*sin(omiz)*sin(kaz);a2z=-cos(faiz)*sin(kaz)-sin(faiz)*sin(omiz)*cos(kaz);a3z=-sin(faiz)*cos(omiz);
b1z=cos(omiz)*sin(kaz);b2z=cos(omiz)*cos(kaz);b3z=-sin(omiz);
c1z=sin(faiz)*cos(kaz)+cos(faiz)*sin(omiz)*sin(kaz);c2z=-sin(faiz)*sin(kaz)+cos(faiz)*sin(omiz)*cos(kaz);c3z=cos(faiz)*cos(omiz);
R=[a1z,b1z,c1z;a2z,b2z,c2z;a3z,b3z,c3z]
for n=1:1:4
s1z=[Xdz(n)-Xsz,Ydz(n)-Ysz,Zdz(n)-Zsz]';
X_z(n)=[a1z,b1z,c1z]*s1z;
Y_z(n)=[a2z,b2z,c2z]*s1z;
Z_z(n)=[a3z,b3z,c3z]*s1z;
xz(n)=-f*X_z(n)/Z_z(n);
yz(n)=-f*Y_z(n)/Z_z(n);
end
%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)
for p=1:1:4
a11z(p)=1/Z_z(p)*(a1z*f+a3z*xz(p)); a12z(p)=1/Z_z(p)*(b1z*f+b3z*xz(p)); a13z(p)=1/Z_z(p)*(c1z*f+c3z*xz(p));
a21z(p)=1/Z_z(p)*(a2z*f+a3z*yz(p)); a22z(p)=1/Z_z(p)*(b2z*f+b3z*yz(p)); a23z(p)=1/Z_z(p)*(c2z*f+c3z*yz(p));
a14z(p)=yz(p)*sin(omiz)-(xz(p)/f*(xz(p)*cos(kaz)-yz(p)*sin(kaz))+f*cos(kaz))*cos(omiz);
a15z(p)=-f*sin(kaz)-xz(p)/f*(xxz(p)*sin(kaz)+yz(p)*cos(kaz)); %计算A矩阵
a16z(p)=yxz(p);
a24z(p)=-xxz(p)*sin(omiz)-(yz(p)/f*(xz(p)*cos(kaz)-yz(p)*sin(kaz))-f*sin(kaz))*cos(omiz);
a25z(p)=-f*cos(kaz)-yz(p)/f*(xz(p)*sin(kaz)+yz(p)*cos(kaz));
a26z(p)=-xxz(p) ;
%右片
end
Az=[a11z(1),a12z(1),a13z(1),a14z(1),a15z(1),a16z(1);a21z(1),a22z(1),a23z(1),a24z(1),a25z(1),a26z(1);a11z(2),a12z(2),a13z(2),a14z(2),a15z(2),a16z(2);a21z(2),a22z(2),a23z(2),a24z(2),a25z(2),a26z(2);a11z(3),a12z(3),a13z(3),a14z(3),a15z(3),a16z(3);a21z(3),a22z(3),a23z(3),a24z(3),a25z(3),a26z(3);a11z(4),a12z(4),a13z(4),a14z(4),a15z(4),a16z(4);a21z(4),a22z(4),a23z(4),a24z(4),a25z(4),a26z(4)]
lxz=xxz-xz;
lyz=yxz-yz;
%右片
Lz=[lxz(1),lyz(1),lxz(2),lyz(2),lxz(3),lyz(3),lxz(4),lyz(4)]'
Xz=inv(Az'*Az)*Az'*Lz
dxz=Xz(1,1);dyz=Xz(2,1);dzz=Xz(3,1);dfz=Xz(4,1);doz=Xz(5,1);dkz=Xz(6,1);
Xsz=Xsz+dxz;
Ysz=dyz+Ysz ;
Zsz=dzz+Zsz;
faiz=faiz+dfz;
omiz=omiz+doz;
kaz=kaz+dkz;
if all(abs(Xz)<0.0003)
break;
end
end
f=0.152;fai=0;omi=0;ka=0;
Xd=[5083.205,5780.02,5210.879,5909.264];
Yd=[5852.099,5906.365,4258.446,4314.283];
Zd=[527.925,571.549,461.81,455.484];
%以上为输入gcp1到gcp4的地面坐标
xx=[-0.07393,-0.005252,-0.079122,-0.009887];
yx=[0.078706,0.078184,-0.078879,-0.080089];
%右片像点坐标
Xs=(5083.205+5780.02+5210.879+5909.264)/4;
Ys=(5852.099+5906.365+4258.446+431
4.283)/4;
Sx=sqrt((xx(2)-xx(1))^2+(yx(2)-yx(1))^2);
Sd=sqrt((Xd(2)-Xd(1))^2+(Yd(2)-Yd(1))^2);
m=10000
Zs=m*f+1/4*(Zd(1)+Zd(2)+Zd(3)+Zd(4));
X=[1;1;1;1;1;1];
while 1
a1=cos(fai)*cos(ka)-sin(fai)*sin(omi)*sin(ka);a2=-cos(fai)*sin(ka)-sin(fai)*sin(omi)*cos(ka);a3=-sin(fai)*cos(omi);
b1=cos(omi)*sin(ka);b2=cos(omi)*cos(ka);b3=-sin(omi);
c1=sin(fai)*cos(ka)+cos(fai)*sin(omi)*sin(ka);c2=-sin(fai)*sin(ka)+cos(fai)*sin(omi)*cos(ka);c3=cos(fai)*cos(omi);
R=[a1,b1,c1;a2,b2,c2;a3,b3,c3]
for n=1:1:4
s1=[Xd(n)-Xs,Yd(n)-Ys,Zd(n)-Zs]';
X_(n)=[a1,b1,c1]*s1;
Y_(n)=[a2,b2,c2]*s1;
Z_(n)=[a3,b3,c3]*s1;
x(n)=-f*X_(n)/Z_(n);
y(n)=-f*Y_(n)/Z_(n);
end
%以上循环用于求解像空间坐标x(1),y(1)到x(4),y(4)
for p=1:1:4
a11(p)=1/Z_(p)*(a1*f+a3*x(p)); a12(p)=1/Z_(p)*(b1*f+b3*x(p)); a13(p)=1/Z_(p)*(c1*f+c3*x(p));
a21(p)=1/Z_(p)*(a2*f+a3*y(p)); a22(p)=1/Z_(p)*(b2*f+b3*y(p)); a23(p)=1/Z_(p)*(c2*f+c3*y(p));
a14(p)=y(p)*sin(omi)-(x(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))+f*cos(ka))*cos(omi);
a15(p)=-f*sin(ka)-x(p)/f*(x(p)*sin(ka)+y(p)*cos(ka)); %计算A矩阵
a16(p)=y(p);
a24(p)=-x(p)*sin(omi)-(y(p)/f*(x(p)*cos(ka)-y(p)*sin(ka))-f*sin(ka))*cos(omi);
a25(p)=-f*cos(ka)-y(p)/f*(x(p)*sin(ka)+y(p)*cos(ka));
a26(p)=-x(p) ;
%右片
end
A=[a11(1),a12(1),a13(1),a14(1),a15(1),a16(1);a21(1),a22(1),a23(1),a24(1),a25(1),a26(1);a11(2),a12(2),a13(2),a14(2),a15(2),a16(2);a21(2),a22(2),a23(2),a24(2),a25(2),a26(2);a11(3),a12(3),a13(3),a14(3),a15(3),a16(3);a21(3),a22(3),a23(3),a24(3),a25(3),a26(3);a11(4),a12(4),a13(4),a14(4),a15(4),a16(4);a21(4),a22(4),a23(4),a24(4),a25(4),a26(4)]
lx=xx-x;
ly=yx-y;
%右片
L=[lx(1),ly(1),lx(2),ly(2),lx(3),ly(3),lx(4),ly(4)]'
X=inv(A'*A)*A'*L
dx=X(1,1);dy=X(2,1);dz=X(3,1);df=X(4,1);do=X(5,1);dk=X(6,1);
Xs=Xs+dx;
Ys=dy+Ys ;
Zs=dz+Zs;
fai=fai+df;
omi=omi+do;
ka=ka+dk;
if all(abs(X)<0.0003)
break;
end
end
Vz=Az*Xz-Lz
Xsz=Xsz+dxz
Ysz=dyz+Ysz
Zsz=dzz+Zsz
faiz=faiz+dfz
omiz=omiz+doz
kaz=kaz+dkz
V=A*X-L
Xs=Xs+dx
Ys=dy+Ys
Zs=dz+Zs
fai=fai+df
omi=omi+do
ka=ka+dk
%前方交会
a1z=cos(faiz)*cos(kaz);a2z=-cos(faiz)*sin(kaz);a3z=-sin(faiz);
b1z=cos(omiz)*sin(kaz)-sin(omiz)*sin(faiz)*cos(kaz);b2z=cos(omiz)*cos(kaz)+sin(omiz)*sin(faiz)*sin(kaz);b3z=-sin(omiz)*cos(faiz);
c1z=sin(omiz)*sin(kaz)+cos(omiz)*sin(faiz)*cos(kaz);c2z=sin(omiz)*cos(kaz)-cos(omiz)*sin(faiz)*sin(kaz);c3z=cos(faiz)*cos(omiz);
%求迭代后左片旋转矩阵R1的元素
R1=[a1z,a2z,a3z;b1z,b2z,b3z;c1z,c2z,c3z];
a1=cos(fai)*cos(ka);a2=-cos(fai)*sin(ka);a3=-sin(fai);
b1=cos(omi)*sin(ka)-sin(omi)*sin(fai)*cos(ka);b2=cos(omi)*cos(ka)+sin(omi)*sin(fai)*sin(ka);b3=-sin(omi)*cos(fai);
c1=sin(omi)*sin(ka)+cos(omi)*sin(fai)*cos(ka);c2=sin(omi)*cos(ka)-cos(omi)*sin(fai)*sin(ka);c3=cos(fai)*cos(omi);
%求迭代后右片旋转矩阵R1的元素
R2=[a1,a2,a3;b1,b2,b3;c1,c2,c3];
Bx=Xs-Xsz;
By=Ys-Ysz;
Bz=Zs-Zsz;
xz=[0.051758 0.014618 0.04988 0.086243 0
.048135];
yz=[0.081555 -0.000231 -0.000792 -0.001346 -0.079962];
zz=[-f -f -f -f -f];
x=[-0.039953 -0.0076016 -0.042201 -0.0076706 -0.044438];
y=[0.078463 0.000036 -0.001022 -0.002112 -79.736];
z=[-f -f -f -f -f ];
for p=1:1:5
Xz(p)=a1z*xz(p)+a2z*yz(p)+a3z*zz(p);
Yz(p)=b1z*xz(p)+b2z*yz(p)+b3z*zz(p);
Zz(p)=c1z*xz(p)+c2z*yz(p)+c3z*zz(p);
X2(p)=a1*x(p)+a2*y(p)+a3*z(p);
Y2(p)=a1*x(p)+a2*y(p)+a3*z(p);
Z2(p)=a1*x(p)+a2*y(p)+a3*z(p);
end
Xz=[Xz(1),Xz(2),Xz(3),Xz(4),Xz(5);Yz(1),Yz(2),Yz(3),Yz(4),Yz(5);Zz(1),Zz(2),Zz(3),Zz(4),Zz(5)]
X=[X2(1),X2(2),X2(3),X2(4),X2(5);Y2(1),Y2(2),Y2(3),Y2(4),Y2(5);Z2(1),Z2(2),Z2(3),Z2(4),Z2(5)]
for p=1:1:5
N(p)=(Bx*X(p+10)-Bz*X(p+10))/(Xz(p)*X(p+10)-X(p)*Xz(p+10));
end
N1=[N(1),N(2),N(3),N(4),N(5)]
for p=1:1:5
XA(p)=Xsz+N1(p)*Xz(p);
YA(p)=Ysz+N1(p)*Xz(p+5);
ZA(p)=Zsz+N1(p)*Xz(p+10);
end
XYZ=[XA(1) XA(2) XA(3) XA(4) XA(5);YA(1) YA(2) YA(3) YA(4) YA(5);ZA(1) ZA(2) ZA(3) ZA(4) ZA(5)]