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

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

《相对定向绝对定向后方交会.docx》由会员分享，可在线阅读，更多相关《相对定向绝对定向后方交会.docx（23页珍藏版）》请在冰豆网上搜索。

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

#include

#include

#include

usingnamespacestd;

//-------------------------------------------------------------------------------------------------

structKBJ

{

doublex;

doubley;

};

structKZD

{

intnum;

doublex;

doubley;

doublez;

};

structXKZD

{

intnum;

doublex;

doubley;

doublez;

doublex1;

doubley1;

};

structXP

{

intnum;

doublex;

doubley;

doublep;

doubleq;

};

structHFJH//前方交会

{

doublex;

doubley;

doublez;

doublef;

doublew;

doublek;

doublejdx;

doublejdy;

doublejdz;

doublejdf;

doublejdw;

doublejdk;

};

structXD//相对定向

{

doubleu;

doublev;

doublef;

doublew;

doublek;

doublejdu;

doublejdv;

doublejdf;

doublejdw;

doublejdk;

};

structXXD

{

doublex1;

doubley1;

doublez1;

doublex2;

doubley2;

doublez2;

doubleN1;

doubleN2;

doubleQ;

};

structJD//绝对定向

{

doublef;

doublew;

doublek;

doublel;

doubledx;

doubledy;

doubledz;

doublejdf;

doublejdw;

doublejdk;

doublejdl;

doublejdx;

doublejdy;

doublejdz;

};

structGS

{

doublex;

doubley;

doublez;

doublef;

doublew;

doublek;

doubledx;

doubledy;

doubledz;

doublejdx;

doublejdy;

doublejdz;

doublejdf;

doublejdw;

doublejdk;

doublejddx;

doublejddy;

doublejddz;

};

//-------------------------------------------------------------------------------------------------

structGSF

{

intkzd;

intnum;

doublex;

doubley;

doublez;

doublex1;

doubley1;

doublex2;

doubley2;

};

KBJkbj[2];//存放框标距

KZD*kzd;//存放控制点

XP*xp;//存放左右相片数据

HFJHhfjh[2];//存放前方交会数据，一个存放左相片数据，一个存放右相片数据

KZD*qfjh;//存放前方交会数据结果

KZD*xddx;//存放相对定向数据

XDxdwc;//存放相对定向数据

KZD*xdkzd;

KZD*jddx;//存放绝对定向

JDjdwc;//存放绝对定向的误差

GSgsz,gsy;//存放光束法左右相片的数据

KZD*gszjg,*gsyjg;//光束法的结果

intn;//存放控制点的个数

intm;//存放相片的数据个数

doublezhuju;//主距

doublebilichi=12000;//比例尺

//-------------------------------------------------------------------------------------------------

voidtranspose（double*m1,double*m2,inttem1,inttem2）//矩阵转置

{

for（inti=0;i

for（intj=0;j

m2[j*tem1+i]=m1[i*tem2+j];

return;

}

voidinv（double*a,intn1）/*正定矩阵求逆*/

{

inti,j,k;

for（k=0;k

{

for（i=0;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;i

{

if（i!

=k）

{

for（j=0;j

{

if（j!

=k）

*（a+i*n1+j）+=*（a+k*n1+j）**（a+i*n1+k）;

}

}

}

for（j=0;j

{

if（j!

=k）

*（a+k*n1+j）*=*（a+k*n1+k）;

}

}

}

voidmult（double*m1,double*m2,double*result,inti_1,intj_12,intj_2）//矩阵相乘

{

inti,j,k;

for（i=0;i

for（j=0;j

{

result[i*j_2+j]=0.0;

for（k=0;k

result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];

}

return;

}

voidjs_hfjh（inttem）

{

XKZD*xkzd=newXKZD[n];//新控制点

inttem1=0;

if（tem==0）

{

for（inti=0;i

for（intj=0;j

if（kzd[i].num==xp[j].num）

{

xkzd[tem1].num=xp[j].num;

xkzd[tem1].x=kzd[i].x;

xkzd[tem1].y=kzd[i].y;

xkzd[tem1].z=kzd[i].z;

xkzd[tem1].x1=（xp[j].x-100）/1000;

xkzd[tem1++].y1=（100-xp[j].y）/1000;

}

}

if（tem==1）

{

for（inti=0;i

for（intj=0;j

if（kzd[i].num==xp[j].num）

{

xkzd[tem1].num=xp[j].num;

xkzd[tem1].x=kzd[i].x;

xkzd[tem1].y=kzd[i].y;

xkzd[tem1].z=kzd[i].z;

xkzd[tem1].x1=（xp[j].x-100-xp[j].p）/1000;

xkzd[tem1++].y1=（100-xp[j].y-xp[j].q）/1000;

}

}

for（inti=0;i

{

hfjh[tem].x+=xkzd[i].x;

hfjh[tem].y+=xkzd[i].y;

hfjh[tem].z+=xkzd[i].z;

}

hfjh[tem].x/=tem1;

hfjh[tem].y/=tem1;

hfjh[tem].z=hfjh[tem].z/tem1+bilichi*zhuju;

hfjh[tem].f=0.0;

hfjh[tem].w=0.0;

hfjh[tem].k=0.0;//六个外方位元素的初始值

//-------------------------------------------------------------------------------------------------

doubleX[6]={1,1,1,1,1,1};//存放外方位元素改正值

doublea[3],b[3],c[3];//存放旋转矩阵

double*A=newdouble[12*tem1];

double*B=newdouble[12*tem1];

double*l=newdouble[2*tem1];

doubleC[36],D[6];

double*Xo=newdouble[tem1];

double*Yo=newdouble[tem1];

double*Zo=newdouble[tem1];

//-------------------------------------------------------------------------------------------------

while（fabs（X[0]）>1e-6||fabs（X[1]）>1e-6||fabs（X[2]）>1e-6||fabs（X[3]）>1e-6||fabs（X[4]）>1e-6||fabs（X[5]）>1e-6）

{

a[0]=cos（hfjh[tem].f）*cos（hfjh[tem].k）-sin（hfjh[tem].f）*sin（hfjh[tem].w）*sin（hfjh[tem].k）;

a[1]=-cos（hfjh[tem].f）*sin（hfjh[tem].k）-sin（hfjh[tem].f）*sin（hfjh[tem].w）*cos（hfjh[tem].k）;

a[2]=-sin（hfjh[tem].f）*cos（hfjh[tem].w）;

b[0]=cos（hfjh[tem].w）*sin（hfjh[tem].k）;

b[1]=cos（hfjh[tem].w）*cos（hfjh[tem].k）;

b[2]=-sin（hfjh[tem].w）;

c[0]=sin（hfjh[tem].f）*cos（hfjh[tem].k）+cos（hfjh[tem].f）*sin（hfjh[tem].w）*sin（hfjh[tem].k）;

c[1]=-sin（hfjh[tem].f）*sin（hfjh[tem].k）+cos（hfjh[tem].f）*sin（hfjh[tem].w）*cos（hfjh[tem].k）;

c[2]=cos（hfjh[tem].f）*cos（hfjh[tem].w）;//计算旋转矩阵R

//-------------------------------------------------------------------------------------------------

for（i=0;i

{

Xo[i]=-zhuju*（a[0]*（xkzd[i].x-hfjh[tem].x）+b[0]*（xkzd[i].y-hfjh[tem].y）+c[0]*（xkzd[i].z-hfjh[tem].z））/（a[2]*（xkzd[i].x-hfjh[tem].x）+b[2]*（xkzd[i].y-hfjh[tem].y）+c[2]*（xkzd[i].z-hfjh[tem].z））;

Yo[i]=-zhuju*（a[1]*（xkzd[i].x-hfjh[tem].x）+b[1]*（xkzd[i].y-hfjh[tem].y）+c[1]*（xkzd[i].z-hfjh[tem].z））/（a[2]*（xkzd[i].x-hfjh[tem].x）+b[2]*（xkzd[i].y-hfjh[tem].y）+c[2]*（xkzd[i].z-hfjh[tem].z））;

Zo[i]=a[2]*（xkzd[i].x-hfjh[tem].x）+b[2]*（xkzd[i].y-hfjh[tem].y）+c[2]*（xkzd[i].z-hfjh[tem].z）;

A[12*i+0]=（a[0]*zhuju+a[2]*xkzd[i].x1）/Zo[i];

A[12*i+1]=（b[0]*zhuju+b[2]*xkzd[i].x1）/Zo[i];

A[12*i+2]=（c[0]*zhuju+c[2]*xkzd[i].x1）/Zo[i];

A[12*i+3]=xkzd[i].y1*sin（hfjh[tem].w）-（xkzd[i].x1*（xkzd[i].x1*cos（hfjh[tem].k）-xkzd[i].y1*sin（hfjh[tem].k））/zhuju+zhuju*cos（hfjh[tem].k））*cos（hfjh[tem].w）;

A[12*i+4]=-zhuju*sin（hfjh[tem].k）-xkzd[i].x1*（xkzd[i].x1*sin（hfjh[tem].k）+xkzd[i].y1*cos（hfjh[tem].k））/zhuju;

A[12*i+5]=xkzd[i].y1;

A[12*i+6]=（a[1]*zhuju+a[2]*xkzd[i].y1）/Zo[i];

A[12*i+7]=（b[1]*zhuju+b[2]*xkzd[i].y1）/Zo[i];

A[12*i+8]=（c[1]*zhuju+c[2]*xkzd[i].y1）/Zo[i];

A[12*i+9]=-xkzd[i].x1*sin（hfjh[tem].w）-（xkzd[i].y1*（xkzd[i].x1*cos（hfjh[tem].k）-xkzd[i].y1*sin（hfjh[tem].k））/zhuju-zhuju*sin（hfjh[tem].k））*cos（hfjh[tem].w）;

A[12*i+10]=-zhuju*cos（hfjh[tem].k）-xkzd[i].y1*（xkzd[i].x1*sin（hfjh[tem].k）+xkzd[i].y1*cos（hfjh[tem].k））/zhuju;

A[12*i+11]=-xkzd[i].x1;

l[2*i]=xkzd[i].x1-Xo[i];

l[2*i+1]=xkzd[i].y1-Yo[i];

}//计算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）;

hfjh[tem].x+=X[0];

hfjh[tem].y+=X[1];

hfjh[tem].z+=X[2];

hfjh[tem].f+=X[3];

hfjh[tem].w+=X[4];

hfjh[tem].k+=X[5];

}

doublelinshi=0;

for（i=0;i<2*tem1;i++）

{

linshi+=l[i]*l[i];

}

linshi=sqrt（linshi/（2*tem1-6））;

hfjh[tem].jdx=sqrt（C[0]）*linshi;

hfjh[tem].jdy=sqrt（C[7]）*linshi;

hfjh[tem].jdz=sqrt（C[14]）*linshi;

hfjh[tem].jdf=sqrt（C[21]）*linshi;

hfjh[tem].jdw=sqrt（C[28]）*linshi;

hfjh[tem].jdk=sqrt（C[35]）*linshi;

}

voidjs_qfjh（）

{

doubleBx=hfjh[1].x-hfjh[0].x;

doubleBy=hfjh[1].y-hfjh[0].y;

doubleBz=hfjh[1].z-hfjh[0].z;

doublea[3],b[3],c[3],d[3],e[3],f[3];

//-------------------------------------------------------------------------------------------------

a[0]=cos（hfjh[0].f）*cos（hfjh[0].k）-sin（hfjh[0].f）*sin（hfjh[0].w）*sin（hfjh[0].k）;

a[1]=-cos（hfjh[0].f）*sin（hfjh[0].k）-sin（hfjh[0].f）*sin（hfjh[0].w）*cos（hfjh[0].k）;

a[2]=-sin（hfjh[0].f）*cos（hfjh[0].w）;

b[0]=cos（hfjh[0].w）*sin（hfjh[0].k）;

b[1]=cos（hfjh[0].w）*cos（hfjh[0].k）;

b[2]=-sin（hfjh[0].w）;

c[0]=sin（hfjh[0].f）*cos（hfjh[0].k）+cos（hfjh[0].f）*sin（hfjh[0].w）*sin（hfjh[0].k）;

c[1]=-sin（hfjh[0].f）*sin（hfjh[0].k）+cos（hfjh[0].f）*sin（hfjh[0].w）*sin（hfjh[0].k）;

c[2]=cos（hfjh[0].f）*cos（hfjh[0].w）;

//R1

//-------------------------------------------------------------------------------------------------

d[0]=cos（hfjh[1].f）*cos（hfjh[1].k）-sin（hfjh[1].f）*sin（hfjh[1].w）*sin（hfjh[1].k）;

d[1]=-cos（hfjh[1].f）*sin（hfjh[1].k）-sin（hfjh[1].f）*sin（hfjh[1].w）*cos（hfjh[1].k）;

d[2]=-sin（hfjh[1].f）*cos（hfjh[1].w）;

e[0]=cos（hfjh[1].w）*sin（hfjh[1].k）;

e[1]=cos（hfjh[1].w）*cos（hfjh[1].k）;

e[2]=-sin（hfjh[1].w）;

f[0]=sin（hfjh[1].f）*cos（hfjh[1].k）+cos（hfjh[0].f）*sin（hfjh[0].w）*sin（hfjh[0].k）;

f[1]=-sin（hfjh[1].f）*sin（hfjh[1].k）+cos（hfjh[0].f）*sin（hfjh[0].w）*sin（hfjh[0].k）;

f[2]=cos（hfjh[1].f）*cos（hfjh[1].w）;

//R2

//-------------------------------------------------------------------------------------------------

for（inti=0;i

{

doubleX1=a[0]*（xp[i].x-100）/1000+a[1]*（100-xp[i].y）/1000-a[2]*zhuju;

doubleY1=b[0]*（xp[i].x-100）/1000+b[1]*（100-xp[i].y）/1000-b[2]*zhuju;

doubleZ1=c[0]*（xp[i].x-100）/1000+c[1]*（100-xp[i].y）/1000-c[2]*zhuju;

doubleX2=d[0]*（xp[i].x-100-xp[i].p）/1000+d[1]*（110-xp[i].y-xp[i].q）/1000-d[2]*zhuju;

doubleY2=e[0]*（xp[i].x-100-xp[i].p）/1000+e[1]*（110-xp[i].y-xp[i].q）/1000-e[2]*zhuju;

doubleZ2=f[0]*（xp[i].x-100-xp[i].p）/1000+f[1]*（110-xp[i].y-xp[i].q）/1000-f[2]*zhuju;

doubleN1=（Bx*Z2-Bz*X2）/（X1*Z2-X2*Z1）;

doubleN2=（Bx*Z2-Bz*X2）/（X1*Z2-X2*Z1）;

qfjh[i].num=xp[i].num;

qfjh[i].x=hfjh[0].x+N1*X1;

qfjh[i].y=hfjh[0].y+N1*Y1;

qfjh[i].z=hfjh[0].z+N1*Z1;

//cout<

}

}

//-------------------------------------------------------------------------------------------------

voidjs_xddx（）

{

xdwc.f=0;

xdwc.k=0;

xdwc.u=0;

xdwc.v=0;

xdwc.w=0;

doubleBx=xp[0].p/1000;

double*A=newdouble[m*5];//A

double*L=newdouble[m];//L

double*B=newdouble[m*5];//At

doubleC[25];//AtA

double*D=newdouble[m*5];//AtAA

doubleX[5]={1,1,1,1,1};

doublea[3],b[3],c[3];

XXD*point=newXXD[m];

//-----------------------------------------