摄影测量空间后交程序.docx

实验内容：用 C 、VB或MATLAB语言编写空间后方交会-空间前方交会程序程序代码：#include stdio.h#include math.h#include iostream.h#define N 4#define M 2*Nvoid transpose(double *m1,double *m2,int m,int n) /矩阵转置 int i,j;for(i=0;im;i+) for(j=0;jn;j+)m2j*m+i=m1i*n+j;return;void inv(double *a,int n)/*正定矩阵求逆*/ int i,j,k; for(k=0;kn;k+) for(i=0;in;i+) if(i!=k) *(a+i*n+k)=-*(a+i*n+k)/(*(a+k*n+k); *(a+k*n+k)=1/(*(a+k*n+k); for(i=0;in;i+) if(i!=k) for(j=0;jn;j+) if(j!=k) *(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k); for(j=0;jn;j+) if(j!=k) *(a+k*n+j)*=*(a+k*n+k); void mult(double *m1,double *m2,double *result,int i_1,int j_12,int j_2)/矩阵相乘 int i,j,k; for(i=0;ii_1;i+) for(j=0;jj_2;j+) resulti*j_2+j=0.0; for(k=0;kj_12;k+) resulti*j_2+j+=m1i*j_12+k*m2j+k*j_2; return;void main() int i,m,count;double mmM+1; double t,w,k,Xs0,Ys0,Zs0,f,S1=0.0,S2=0.0;double H6=1,a3,b3,c3,XoN,YoN,ZoN,A6*M,B6*M,lM,C36,D6; double xN,yN,XN,YN,ZN; coutf; coutm;x0=-73.93;y0=78.706;X0=5083.205;Y0=5852.099;Z0=527.925;x1=-5.252;y1=78.184;X1=5780.02;Y1=5906.365;Z1=571.549;x2=-79.122;y2=-78.879;X2=5210.879;Y2=4258.446;Z2=461.81;x3=-9.887;y3=-80.089;X3=5909.264;Y3=4314.283;Z3=455.484;for(i=0;i0.00001|fabs(H1)0.00001|fabs(H2)0.00001|fabs(H3)0.00001|fabs(H4)0.00001|fabs(H5)0.00001)count+=1;a0=cos(t)*cos(k)-sin(t)*sin(w)*sin(k); a1=-cos(t)*sin(k)-sin(t)*sin(w)*cos(k); a2=-sin(t)*cos(w); b0=cos(w)*sin(k); b1=cos(w)*cos(k); b2=-sin(w); c0=sin(t)*cos(k)+cos(t)*sin(w)*sin(k); c1=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k); c2=cos(t)*cos(w); for(i=0;iN;i+)Xoi=-f*(a0*(Xi-Xs0)+b0*(Yi-Ys0)+c0*(Zi-Zs0)/(a2*(Xi-Xs0)+b2*(Yi-Ys0)+c2*(Zi-Zs0); Yoi=-f*(a1*(Xi-Xs0)+b1*(Yi-Ys0)+c1*(Zi-Zs0)/(a2*(Xi-Xs0)+b2*(Yi-Ys0)+c2*(Zi-Zs0); Zoi=a2*(Xi-Xs0)+b2*(Yi-Ys0)+c2*(Zi-Zs0); A12*i+0=(a0*f+a2*xi)/Zoi; A12*i+1=(b0*f+b2*xi)/Zoi; A12*i+2=(c0*f+c2*xi)/Zoi; A12*i+3=yi*sin(w)-(xi*(xi*cos(k)-yi*sin(k)/f+f*cos(k)*cos(w); A12*i+4=-f*sin(k)-xi*(xi*sin(k)+yi*cos(k)/f; A12*i+5=yi; A12*i+6=(a1*f+a2*yi)/Zoi; A12*i+7=(b1*f+b2*yi)/Zoi; A12*i+8=(c1*f+c2*yi)/Zoi; A12*i+9=-xi*sin(w)-(yi*(xi*cos(k)-yi*sin(k)/f-f*sin(k)*cos(w); A12*i+10=-f*cos(k)-yi*(xi*sin(k)+yi*cos(k)/f; A12*i+11=-xi; l2*i=xi-Xoi; l2*i+1=yi-Yoi; transpose(A,B,M,6); mult(B,A,C,6,M,6); mult(B,l,D,6,8,1); inv(C,6); mult(C,D,H,6,6,1); Xs0+=H0; Ys0+=H1; Zs0+=H2; t+=H3; w+=H4; k+=H5;for(i=0;iM;i+)mmi=0;for(i=0;iM;i+)mm0+=li*li;mm0=sqrt(mm0/(2*N-6);for(i=0;i6;i+)mmi+1=sqrt(C6*i+i)*mm0; cout总的叠代次数为:countendl; cout像主点的空间坐标为：endl; coutXs=Xs0endl; coutYs