單片空間后方交會(huì)C程序

#include"stdio.h"#include"stdlib.h"#include"math.h"#include<iostream.h>#definef153.840e-3#definex00#definey00#definem02500#definepi3.1415926voidMatrixReverse(double*nm,double*m,intr,intc){//求轉(zhuǎn)置矩陣 inti,j; for(i=0;i<r;i++) { for(j=0;j<c;j++) { *(nm+j*r+i)=*(m+i*c+j); } }}boolRowMul(double*m,intr,intc,doubleratio){//求矩陣的倍數(shù) inti; for(i=0;i<c;i++) { *(m+r*c+i)*=ratio; } returntrue;}boolRowSwap(double*m,intr1,intr2,intc){//矩陣行交換 inti; doubletemp; for(i=0;i<c;i++) { temp=*(m+r1*c+i); *(m+r1*c+i)=*(m+r2*c+i); *(m+r2*c+i)=temp; } returntrue;}boolRowMinus(double*m,intr1,intr2,intc){//矩陣行相減 inti; for(i=0;i<c;i++) { *(m+r1*c+i)-=*(m+r2*c+i); } returntrue;}voidMatrixPlus(double*m1,double*m2,intr,intc){//矩陣相加 inti,j; for(i=0;i<r;i++) for(j=0;j<c;j++) { *(m1+i*c+j)+=*(m2+i*c+j); }}voidMatrixMul(double*nm,double*m1,double*m2,intr,intc,intcr){//求兩個(gè)矩陣的積 inti,j,k; for(i=0;i<r;i++) for(j=0;j<c;j++) { *(nm+i*c+j)=0; for(k=0;k<cr;k++) { *(nm+i*c+j)+=*(m1+i*cr+k)**(m2+k*c+j); } }}voidMatrixInverse(double*nm,double*m,intrc){//求矩陣的逆 double*nme=newdouble[2*rc*rc]; inti,j; for(i=0;i<rc;i++) { for(j=0;j<2*rc;j++) { if(i==(j-rc))*(nme+i*2*rc+j)=1; elseif(j<rc)*(nme+i*2*rc+j)=*(m+i*rc+j); else*(nme+i*2*rc+j)=0; } } for(i=0;i<rc;i++) { if(*(nme+i*2*rc+i)==0) { for(j=i;j<rc;j++) { if(*(nme+j*2*rc+i)) { RowSwap(nme,i,j,2*rc); break; } } } doubleratio; for(j=i+1;j<rc;j++) { if(*(nme+j*2*rc+i)) { ratio=*(nme+i*2*rc+i)/(*(nme+j*2*rc+i)); RowMul(nme,j,2*rc,ratio); RowMinus(nme,j,i,2*rc); } } } for(i=0;i<rc;i++) { doubleratio; for(j=i+1;j<rc;j++) { if(*(nme+(rc-1-j)*2*rc+(rc-1-i))) { ratio=*(nme+(rc-1-i)*2*rc+(rc-1-i))/(*(nme+(rc-1-j)*2*rc+(rc-1-i))); RowMul(nme,(rc-1-j),2*rc,ratio); RowMinus(nme,(rc-1-j),(rc-1-i),2*rc); } } } for(i=0;i<rc;i++) { doubleratio; ratio=1/(*(nme+i*2*rc+i)); RowMul(nme,i,2*rc,ratio); } for(i=0;i<rc;i++) for(j=0;j<rc;j++) *(nm+i*rc+j)=*(nme+i*2*rc+(rc+j)); delete[]nme;}voidmain(){ intcounter=0; FILE*fp=fopen("考試所用數(shù)據(jù).DAT","r"); fscanf(fp,"%d",&counter); doublexx[100],yy[100],XX[100],YY[100],ZZ[100],PID[100]; for(intc=0;c<counter;c++) { fscanf(fp,"%lf%lf%lf%lf%lf%lf",&PID[c],&xx[c],&yy[c],&YY[c],&XX[c],&ZZ[c]); xx[c]/=1000; yy[c]/=1000; } fclose(fp); printf("像片的內(nèi)方位元數(shù):x0=y0=0；航攝儀焦距:f=153.84mm。\n攝影比例尺:1/m=1/2500\n\n"); doublephi,omega,kappa,XS,YS,ZS; doubledphi,domega,dkappa; phi=omega=kappa=XS=YS=ZS=0; intj; for(j=0;j<counter;j++) { XS+=XX[j]; YS+=YY[j]; } XS/=counter;//初始值 YS/=counter; ZS+=f*m0; dphi=domega=dkappa=1; intcount=0; while((dphi>0.00003||domega>0.00003||dkappa>0.00003)) {//迭代驗(yàn)證 count++; inti; doubleATA[36]={0}; doubleATL[6]={0}; doublea1=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa); doublea2=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa); doublea3=-sin(phi)*cos(omega); doubleb1=cos(omega)*sin(kappa); doubleb2=cos(omega)*cos(kappa); doubleb3=-sin(omega); doublec1=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa); doublec2=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa); doublec3=cos(phi)*cos(omega); for(i=0;i<counter;i++) {//逐點(diǎn)法化 doubleX,Y,Z,x,y; doublefX,fY,fZ; doubleid; id=PID[i]; x=xx[i]; y=yy[i]; X=XX[i]; Y=YY[i]; Z=ZZ[i]; fX=a1*(X-XS)+b1*(Y-YS)+c1*(Z-ZS); fY=a2*(X-XS)+b2*(Y-YS)+c2*(Z-ZS); fZ=a3*(X-XS)+b3*(Y-YS)+c3*(Z-ZS); doubleA[12],AT[12],L[2],temp1[36],temp2[6]; L[0]=x+f*fX/fZ; L[1]=y+f*fY/fZ; A[0]=(a1*f+a3*(x-x0))/(fZ); A[1]=(b1*f+b3*(x-x0))/(fZ); A[2]=(c1*f+c3*(x-x0))/(fZ); A[3]=(y-y0)*sin(omega)-((x-x0)*((x-x0)*cos(kappa)-(y-y0)*sin(kappa))/f+f*cos(kappa))*cos(omega); A[4]=-f*sin(kappa)-(x-x0)*((x-x0)*sin(kappa)+(y-y0)*cos(kappa))/f; A[5]=y-y0; A[6]=(a2*f+a3*(y-y0))/(fZ); A[7]=(b2*f+b3*(y-y0))/(fZ); A[8]=(c2*f+c3*(y-y0))/(fZ); A[9]=-(x-x0)*sin(omega)-((y-y0)*((x-x0)*cos(kappa)-(y-y0)*sin(kappa))/f-f*sin(kappa))*cos(omega); A[10]=-f*cos(kappa)-(y-y0)*((x-x0)*sin(kappa)+(y-y0)*cos(kappa))/f; A[11]=x0-x; MatrixReverse(AT,A,2,6); MatrixMul(temp1,AT,A,6,6,2);//求一個(gè)點(diǎn)的ATA MatrixMul(temp2,AT,L,6,1,2);//求一個(gè)點(diǎn)的ATL MatrixPlus(ATA,temp1,6,6);//系數(shù)矩陣相加 MatrixPlus(ATL,temp2,6,1); } //解法方程 doubletemp[6],temp0[36]; MatrixInverse(temp0,ATA,6); MatrixMul(temp,temp0,ATL,6,1,6); XS+=temp[0]; YS+=temp[1]; ZS+=temp[2]; phi+=temp[3]; omega+=temp[4]; kappa+=temp[5]; if(temp[3]<0) dphi=temp[3]*(-1); elsedphi=temp[3]; if(temp[4]<0) domega=temp[4]*(-1); elsedomega=temp[4]; if(temp[5]<0) dkappa=temp[5]*(-1); elsedkappa=temp[5]; //精度評定 doubleMM[6],VTV[1],M0; for(i=0;i<6;i++) { MM[i]=sqrt(temp0[i*6+i]); } VTV[0]=0; for(i=0;i<counter;i++) {//VTV doubleX,Y,Z,x,y; doublefX,fY,fZ; doubletempV[2],tempVT[2],tempVTV[1]; x=xx[i]; y=yy[i]; X=XX[i]; Y=YY[i]; Z=ZZ[i]; fX=a1*(X-XS)+b1*(Y-YS)+c1*(Z-ZS); fY=a2*(X-XS)+b2*(Y-YS)+c2*(Z-ZS); fZ=a3*(X-XS)+b3*(Y-YS)+c3*(Z-ZS); tempV[0]=x+f*fX/fZ; tempV[1]=y+f*fY/fZ; MatrixReverse(tempVT,tempV,2,1); MatrixMul(tempVTV,tempVT,tempV,1,1,2); MatrixPlus(VTV,tempVTV,1,1); } M0=sqrt(VTV[0]/(counter*2-6)); printf("輸入坐標(biāo)點(diǎn)個(gè)數(shù)：%d\n\n",counter); printf("計(jì)算結(jié)果：\n(1)迭代次數(shù)：%d\n",count); printf("(2)外方位元素計(jì)算值:\nphi=%lfomega=%lfkappa=%lf\nXs