相对定向绝对定向后方交会.docx

1、相对定向绝对定向后方交会#include#include#includeusing namespace std;/-struct KBJ double x; double y;struct KZD int num; double x; double y; double z;struct XKZD int num; double x; double y; double z; double x1; double y1;struct XP int num; double x; double y; double p; double q;struct HFJH/后方交会 double x; double

2、 y; double z; double f; double w; double k; double jdx; double jdy; double jdz; double jdf; double jdw; double jdk;struct XD/相对定向 double u; double v; double f; double w; double k; double jdu; double jdv; double jdf; double jdw; double jdk;struct XXD double x1; double y1; double z1; double x2; double

3、 y2; double z2; double N1; double N2; double Q;struct JD/绝对定向 double f; double w; double k; double l; double dx; double dy; double dz; double jdf; double jdw; double jdk; double jdl; double jdx; double jdy; double jdz;struct GS double x; double y; double z; double f; double w; double k; double dx; d

4、ouble dy; double dz; double jdx; double jdy; double jdz; double jdf; double jdw; double jdk; double jddx; double jddy; double jddz;/-struct GSF int kzd; int num; double x; double y; double z; double x1; double y1; double x2; double y2;KBJ kbj2;/存放框标距KZD *kzd;/存放控制点XP *xp;/存放左右相片数据HFJH hfjh2;/存放后方交会数

5、据，一个存放左相片数据，一个存放右相片数据KZD *qfjh;/存放前方交会数据结果KZD *xddx;/存放相对定向数据XD xdwc;/存放相对定向数据KZD *xdkzd;KZD *jddx;/存放绝对定向JD jdwc;/存放绝对定向的误差GS gsz,gsy;/存放光束法左右相片的数据KZD *gszjg,*gsyjg;/光束法的结果int n;/存放控制点的个数int m;/存放相片的数据个数double zhuju;/主距double bilichi=12000;/比例尺/-void transpose(double *m1,double *m2,int tem1,int tem

6、2) /矩阵转置 for(int i=0;item1;i+) for(int j=0;jtem2;j+) m2j*tem1+i=m1i*tem2+j; return;void inv(double *a,int n1)/*正定矩阵求逆*/ int i,j,k; for(k=0;kn1;k+) for(i=0;in1;i+) if(i!=k) *(a+i*n1+k)=-*(a+i*n1+k)/(*(a+k*n1+k); *(a+k*n1+k)=1/(*(a+k*n1+k); for(i=0;in1;i+) if(i!=k) for(j=0;jn1;j+) if(j!=k) *(a+i*n1+j)

7、+=*(a+k*n1+j)* *(a+i*n1+k); for(j=0;jn1;j+) if(j!=k) *(a+k*n1+j)*=*(a+k*n1+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 js_hfjh(int

8、tem) XKZD *xkzd=new XKZDn;/新控制点 int tem1=0; if(tem=0) for(int i=0;in;i+) for(int j=0;jm;j+) if(kzdi.num=xpj.num) xkzdtem1.num=xpj.num; xkzdtem1.x=kzdi.x; xkzdtem1.y=kzdi.y; xkzdtem1.z=kzdi.z; xkzdtem1.x1=(xpj.x-100)/1000; xkzdtem1+.y1=(100-xpj.y)/1000; if(tem=1) for(int i=0;in;i+) for(int j=0;jm;j+)

9、 if(kzdi.num=xpj.num) xkzdtem1.num=xpj.num; xkzdtem1.x=kzdi.x; xkzdtem1.y=kzdi.y; xkzdtem1.z=kzdi.z; xkzdtem1.x1=(xpj.x-100-xpj.p)/1000; xkzdtem1+.y1=(100-xpj.y-xpj.q)/1000; for(int i=0;i1e-6|fabs(X1)1e-6|fabs(X2)1e-6|fabs(X3)1e-6|fabs(X4)1e-6|fabs(X5)1e-6) a0=cos(hfjhtem.f)*cos(hfjhtem.k)-sin(hfjht

10、em.f)*sin(hfjhtem.w)*sin(hfjhtem.k); a1=-cos(hfjhtem.f)*sin(hfjhtem.k)-sin(hfjhtem.f)*sin(hfjhtem.w)*cos(hfjhtem.k); a2=-sin(hfjhtem.f)*cos(hfjhtem.w); b0=cos(hfjhtem.w)*sin(hfjhtem.k); b1=cos(hfjhtem.w)*cos(hfjhtem.k); b2=-sin(hfjhtem.w); c0=sin(hfjhtem.f)*cos(hfjhtem.k)+cos(hfjhtem.f)*sin(hfjhtem.

11、w)*sin(hfjhtem.k); c1=-sin(hfjhtem.f)*sin(hfjhtem.k)+cos(hfjhtem.f)*sin(hfjhtem.w)*cos(hfjhtem.k); c2=cos(hfjhtem.f)*cos(hfjhtem.w);/计算旋转矩阵R/- for(i=0;item1;i+) Xoi=-zhuju*(a0*(xkzdi.x-hfjhtem.x)+b0*(xkzdi.y-hfjhtem.y)+c0*(xkzdi.z-hfjhtem.z)/(a2*(xkzdi.x-hfjhtem.x)+b2*(xkzdi.y-hfjhtem.y)+c2*(xkzdi.z

12、-hfjhtem.z); Yoi=-zhuju*(a1*(xkzdi.x-hfjhtem.x)+b1*(xkzdi.y-hfjhtem.y)+c1*(xkzdi.z-hfjhtem.z)/(a2*(xkzdi.x-hfjhtem.x)+b2*(xkzdi.y-hfjhtem.y)+c2*(xkzdi.z-hfjhtem.z); Zoi=a2*(xkzdi.x-hfjhtem.x)+b2*(xkzdi.y-hfjhtem.y)+c2*(xkzdi.z-hfjhtem.z); A12*i+0=(a0*zhuju+a2*xkzdi.x1)/Zoi; A12*i+1=(b0*zhuju+b2*xkzd

13、i.x1)/Zoi; A12*i+2=(c0*zhuju+c2*xkzdi.x1)/Zoi; A12*i+3=xkzdi.y1*sin(hfjhtem.w)-(xkzdi.x1*(xkzdi.x1*cos(hfjhtem.k)-xkzdi.y1*sin(hfjhtem.k)/zhuju+zhuju*cos(hfjhtem.k)*cos(hfjhtem.w); A12*i+4=-zhuju*sin(hfjhtem.k)-xkzdi.x1*(xkzdi.x1*sin(hfjhtem.k)+xkzdi.y1*cos(hfjhtem.k)/zhuju; A12*i+5=xkzdi.y1; A12*i+

14、6=(a1*zhuju+a2*xkzdi.y1)/Zoi; A12*i+7=(b1*zhuju+b2*xkzdi.y1)/Zoi; A12*i+8=(c1*zhuju+c2*xkzdi.y1)/Zoi; A12*i+9=-xkzdi.x1*sin(hfjhtem.w)-(xkzdi.y1*(xkzdi.x1*cos(hfjhtem.k)-xkzdi.y1*sin(hfjhtem.k)/zhuju-zhuju*sin(hfjhtem.k)*cos(hfjhtem.w); A12*i+10=-zhuju*cos(hfjhtem.k)-xkzdi.y1*(xkzdi.x1*sin(hfjhtem.k

15、)+xkzdi.y1*cos(hfjhtem.k)/zhuju; A12*i+11=-xkzdi.x1; l2*i=xkzdi.x1-Xoi; l2*i+1=xkzdi.y1-Yoi; /计算A，L/- transpose(A,B,2*tem1,6); mult(B,A,C,6,2*tem1,6); mult(B,l,D,6,2*tem1,1); inv(C,6); mult(C,D,X,6,6,1); hfjhtem.x+=X0; hfjhtem.y+=X1; hfjhtem.z+=X2; hfjhtem.f+=X3; hfjhtem.w+=X4; hfjhtem.k+=X5; double

16、 linshi=0; for(i=0;i2*tem1;i+) linshi+=li*li; linshi=sqrt(linshi/(2*tem1-6); hfjhtem.jdx=sqrt(C0)*linshi; hfjhtem.jdy=sqrt(C7)*linshi; hfjhtem.jdz=sqrt(C14)*linshi; hfjhtem.jdf=sqrt(C21)*linshi; hfjhtem.jdw=sqrt(C28)*linshi; hfjhtem.jdk=sqrt(C35)*linshi;void js_qfjh() double Bx=hfjh1.x-hfjh0.x; doub

17、le By=hfjh1.y-hfjh0.y; double Bz=hfjh1.z-hfjh0.z; double a3,b3,c3,d3,e3,f3;/- a0=cos(hfjh0.f)*cos(hfjh0.k)-sin(hfjh0.f)*sin(hfjh0.w)*sin(hfjh0.k); a1=-cos(hfjh0.f)*sin(hfjh0.k)-sin(hfjh0.f)*sin(hfjh0.w)*cos(hfjh0.k); a2=-sin(hfjh0.f)*cos(hfjh0.w); b0=cos(hfjh0.w)*sin(hfjh0.k); b1=cos(hfjh0.w)*cos(hf

18、jh0.k); b2=-sin(hfjh0.w); c0=sin(hfjh0.f)*cos(hfjh0.k)+cos(hfjh0.f)*sin(hfjh0.w)*sin(hfjh0.k); c1=-sin(hfjh0.f)*sin(hfjh0.k)+cos(hfjh0.f)*sin(hfjh0.w)*sin(hfjh0.k); c2=cos(hfjh0.f)*cos(hfjh0.w); /R1/- d0=cos(hfjh1.f)*cos(hfjh1.k)-sin(hfjh1.f)*sin(hfjh1.w)*sin(hfjh1.k); d1=-cos(hfjh1.f)*sin(hfjh1.k)-

19、sin(hfjh1.f)*sin(hfjh1.w)*cos(hfjh1.k); d2=-sin(hfjh1.f)*cos(hfjh1.w); e0=cos(hfjh1.w)*sin(hfjh1.k); e1=cos(hfjh1.w)*cos(hfjh1.k); e2=-sin(hfjh1.w); f0=sin(hfjh1.f)*cos(hfjh1.k)+cos(hfjh0.f)*sin(hfjh0.w)*sin(hfjh0.k); f1=-sin(hfjh1.f)*sin(hfjh1.k)+cos(hfjh0.f)*sin(hfjh0.w)*sin(hfjh0.k); f2=cos(hfjh1

20、.f)*cos(hfjh1.w); /R2/- for(int i=0;im;i+) double X1=a0*(xpi.x-100)/1000+a1*(100-xpi.y)/1000-a2*zhuju; double Y1=b0*(xpi.x-100)/1000+b1*(100-xpi.y)/1000-b2*zhuju; double Z1=c0*(xpi.x-100)/1000+c1*(100-xpi.y)/1000-c2*zhuju; double X2=d0*(xpi.x-100-xpi.p)/1000+d1*(110-xpi.y-xpi.q)/1000-d2*zhuju; doubl

21、e Y2=e0*(xpi.x-100-xpi.p)/1000+e1*(110-xpi.y-xpi.q)/1000-e2*zhuju; double Z2=f0*(xpi.x-100-xpi.p)/1000+f1*(110-xpi.y-xpi.q)/1000-f2*zhuju; double N1=(Bx*Z2-Bz*X2)/(X1*Z2-X2*Z1); double N2=(Bx*Z2-Bz*X2)/(X1*Z2-X2*Z1); qfjhi.num=xpi.num; qfjhi.x=hfjh0.x+N1*X1; qfjhi.y=hfjh0.y+N1*Y1; qfjhi.z=hfjh0.z+N1*Z1;/ coutX1 Y1 Z1 X2 Y2 Z2 N1 N2endl; /-void js_xddx() xdwc.f=0; xdwc.k=0; xdwc.u=0; xdwc.v=0; xdwc.w=0; double Bx=xp0.p/1000; double *A=new double m*5;/A double *L=new double m;/L double *B=new double m*5;/At doub