📄 摄影测量后方交会.cpp
字号:
#include<math.h>
#include<iostream.h>
class pc
{
public:
double
angle1,//外方位角1
angle2,//外方位角2
angle3,//外方位角3
Xs0,Ys0,Zs0,//摄影中心近似坐标
du,fen,miao,//弧度对应的角度
Pi,//3.14
*_X_Y_Z,//4*3
*a,//A矩阵 8*6
*at,//A矩阵的转置 6*8
*a_at,//6*6
*QIUNI_at_a,//6*6
*QIUNI_at_a_at,//6*8
*L,//8*1
*XX,//6*1 最终的改正数
m,//比例尺
f,//航测主距
*X,//存储已知四对坐标
*R, //存储a1,a2,a3,b1,b2,b3,c1,c2,c3,R矩阵
addx,addy,addz,add1,add2,add3;//坐标以及角度的改正值
pc()////////////////////确定已知值////////////////////
{
at=new double[6*8];a_at=new double[6*6];QIUNI_at_a=new double[6*6];QIUNI_at_a_at=new double[6*8];L=new double[8*1];XX=new double[6*1];
a=NULL;
m=30000;//比例尺
f=150;//航测主距
Pi=3.1415927;
X=new double [4*5];
///////////////已知四对坐标
X[0]=-91.596;X[1]=-74.859; X[2]= 100000000.00;X[3]=137500000.00;X[4]=11000.00;
X[5]=-94.230;X[6]=81.446; X[7]= 100000000.00;X[8]=142500000.00;X[9]=36000.00;
X[10]=95.207;X[11]=-75.521;X[12]=106000000.00;X[13]=137500000.00;X[14]=42000.00;
X[15]=96.797;X[16]=83.077; X[17]=106000000.00;X[18]=142500000.00;X[19]=56000.00;
//外方位元素的初始值
Xs0=103000000.00; Ys0=140000000.00; Zs0=4500000;
angle1=0; angle2=0; angle3=0;
//外方未元素的改正值
addx=0;addy=0;addz=0;add1=0;add2=0;add3=0;
}
void getdata()//////////////////////////////输入已知数据
{
int i,j;
_X_Y_Z= new double [4*3];// 四组数据，每组数据对应一个 _X,_Y,_Z.
Xs0+=addx; Ys0+=addy; Zs0+=addz;
angle1+=add1;angle2+=add2;angle3+=add3;
////////////////////////////////////////////////////////确定旋转矩阵
R=new double[3*3];// a1,a2,a3,
// b1,b2,b3
// c1,c2,c3
R[0]=cos(angle1)*cos(angle3)-sin(angle1)*sin(angle2)*sin(angle3);
R[1]=-cos(angle1)*sin(angle3)-sin(angle1)*sin(angle2)*cos(angle3);
R[2]=-sin(angle1)*cos(angle2);
R[3]=cos(angle2)*sin(angle3);
R[4]=cos(angle2)*cos(angle3);
R[5]=-sin(angle2);
R[6]=sin(angle1)*cos(angle3)+cos(angle1)*sin(angle2)*sin(angle3);
R[7]=-sin(angle1)*sin(angle3)+cos(angle1)*sin(angle2)*cos(angle3);
R[8]=cos(angle1)*cos(angle2);
_X_Y_Z[0]=R[0]*(X[2]-Xs0)+R[3]*(X[3]-Ys0)+R[6]*(X[4]-Zs0); _X_Y_Z[1]=R[1]*(X[2]-Xs0)+R[4]*(X[3]-Ys0)+R[7]*(X[4]-Zs0); _X_Y_Z[2]=R[2]*(X[2]-Xs0)+R[5]*(X[3]-Ys0)+R[8]*(X[4]-Zs0);
_X_Y_Z[3]=R[0]*(X[7]-Xs0)+R[3]*(X[8]-Ys0)+R[6]*(X[9]-Zs0); _X_Y_Z[4]=R[1]*(X[7]-Xs0)+R[4]*(X[8]-Ys0)+R[7]*(X[9]-Zs0); _X_Y_Z[5]=R[2]*(X[7]-Xs0)+R[5]*(X[8]-Ys0)+R[8]*(X[9]-Zs0);
_X_Y_Z[6]=R[0]*(X[12]-Xs0)+R[3]*(X[13]-Ys0)+R[6]*(X[14]-Zs0); _X_Y_Z[7]=R[1]*(X[12]-Xs0)+R[4]*(X[13]-Ys0)+R[7]*(X[14]-Zs0); _X_Y_Z[8]=R[2]*(X[12]-Xs0)+R[5]*(X[13]-Ys0)+R[8]*(X[14]-Zs0);
_X_Y_Z[9]=R[0]*(X[17]-Xs0)+R[3]*(X[18]-Ys0)+R[6]*(X[19]-Zs0); _X_Y_Z[10]=R[1]*(X[17]-Xs0)+R[4]*(X[18]-Ys0)+R[7]*(X[19]-Zs0); _X_Y_Z[11]=R[2]*(X[17]-Xs0)+R[5]*(X[18]-Ys0)+R[8]*(X[19]-Zs0);
///////////////////////////////////////////////////////////////////////////////////////////
//X[]代表已知点
a=new double[8*6];/////////以下输入A矩阵///////////////////////////
for(i=0;i<=7;i++)
for(j=0;j<=2;j++)
a[i*6+j]=(f*R[3*j+i%2]+R[3*j+2]*X[i+3*(i/2)])/_X_Y_Z[2+3*(i/2)];
for(i=0;i<=7;++(++i))
{
a[i*6+3]=X[i+1+i/2*3]*sin(angle2)-( X[i+i/2*3]*(X[i+i/2*3]*cos(angle3)-X[i+1+i/2*3]*sin(angle3))/f+f* cos(angle3) )*cos(angle2);
a[i*6+4]=-f*sin(angle3)-X[i+i/2*3]*(X[i+i/2*3]*sin(angle3)+X[i+1+i/2*3]*cos(angle3))/f;
a[i*6+5]=X[i+1+i/2*3];
a[i*6+9]=-X[i+i/2*3]*sin(angle2)-( X[i+1+i/2*3]*(X[i+i/2*3]*cos(angle3)-X[i+1+i/2*3]*sin(angle3))/f-f* sin(angle3) )*cos(angle2);
a[i*6+10]=-f*cos(angle3)-X[i+1+i/2*3]*(X[i+i/2*3]*sin(angle3)+X[i+1+i/2*3]*cos(angle3))/f;
a[i*6+11]=-X[i+i/2*3];
}////////////////////////////////输入L(8*1)矩阵 //////////////////////////
for(i=0;i<8;i++)
L[i]=X[i+i/2*3]+f*_X_Y_Z[i/2*3+i%2]/_X_Y_Z[i/2*3+2];
}
void time(double *e,double *f,double *g,int m,int n,int k)//矩阵乘积
{
int i,j;
for (i=0; i<=m-1; i++)
for (j=0; j<=k-1; j++)
{
g[i*k+j]=0;
for(int l=0;l<n;l++)
g[i*k+j]+=e[i*n+l]*f[l*k+j];
}
}
void zhuanzhi(double *w,double *ww,int m,int n)//矩阵转置
{
int i,j;
for(i=0;i<m;i++)
for(j=0;j<n;j++)
ww[j*m+i]=w[i*n+j];
}
void qiuni(double *pp,double *mm,int ss)//求逆函数
{
double d,p;
int *is,*js,i,j,k,l,u,v;
is=new int[ss] ;
js=new int[ss] ;
for (k=0; k<=ss-1; k++)
{ d=0.0;
for (i=k; i<=ss-1; i++)
for (j=k; j<=ss-1; j++)
{ l=i*ss+j; p=fabs(pp[l]);
if (p>d) { d=p; is[k]=i; js[k]=j;}
}//a[i][j] 最大，d=该最大值
if (d+1.0==1.0)
{ delete []is;
delete []js;
cout<<"该矩阵无法求逆："<<endl;
}
if (is[k]!=k)
for (j=0; j<=ss-1; j++)
{ u=k*ss+j; v=is[k]*ss+j;
p=pp[u]; pp[u]=pp[v]; pp[v]=p;
}
if (js[k]!=k)
for (i=0; i<=ss-1; i++)
{ u=i*ss+k; v=i*ss+js[k];
p=pp[u]; pp[u]=pp[v]; pp[v]=p;
}
l=k*ss+k;
pp[l]=1.0/pp[l];
for (j=0; j<=ss-1; j++)
if (j!=k)
{ u=k*ss+j; pp[u]=pp[u]*pp[l];}
for (i=0; i<=ss-1; i++)
if (i!=k)
for (j=0; j<=ss-1; j++)
if (j!=k)
{ u=i*ss+j;
pp[u]=pp[u]-pp[i*ss+k]*pp[k*ss+j];
}
for (i=0; i<=ss-1; i++)
if (i!=k)
{ u=i*ss+k; pp[u]=-pp[u]*pp[l];}
}
for (k=ss-1; k>=0; k--)
{ if (js[k]!=k)
for (j=0; j<=ss-1; j++)
{ u=k*ss+j; v=js[k]*ss+j;
p=pp[u]; pp[u]=pp[v]; pp[v]=p;
}
if (is[k]!=k)
for (i=0; i<=ss-1; i++)
{ u=i*ss+k; v=i*ss+is[k];
p=pp[u]; pp[u]=pp[v]; pp[v]=p;
}
}
for(i=0;i<ss;i++)
for(j=0;j<ss;j++)
mm[i*ss+j]=pp[i*ss+j];
}
void display(double *mm,int m,int n)//输出矩阵
{
int i,j;
for(i=0;i<m;i++)
{for(j=0;j<n;j++)
cout<<mm[i*n+j]<<" ";
cout<<endl;
}
}
void change(double hudu)/////////////////弧度角度转换
{
double p;
p=fabs(hudu)/hudu;
du=floor(fabs(hudu)*180/Pi);
fen=floor((fabs(hudu)*180/Pi-du)*60);
miao=(fabs(hudu)*648000/Pi-du*3600-fen*60);
cout<<du*p<<"度"<<fen*p<<"分"<<miao*p<<"秒"<<endl;
}
~pc()
{
delete []X;
delete []R;
delete []_X_Y_Z;
delete []a;
delete []at;
delete []a_at;
delete []QIUNI_at_a;
delete []QIUNI_at_a_at;
delete []L;
delete []XX;
}
};
void main()
{
double define;//改正数的限差
int i=0;//统计循环次数
pc k;
cout<<"以下是外方位元素的初始值："<<endl;//显示外方位角的初始值
cout<<"Xs的初始值: "<<k.Xs0<<" 毫米"<<endl;cout<<"Ys的初始值: "<<k.Ys0<<" 毫米"<<endl;cout<<"Zs的初始值: "<<k.Zs0<<" 毫米"<<endl;
cout<<"φ的初始值: "<<k.angle1<<endl;cout<<"ω的初始值: "<<k.angle2<<endl;cout<<"κ的初始值: "<<k.angle3<<endl;
k.addx=1000;//先设定为一个较大的数，以便进行下面的迭代过程
cout<<"**********以下是迭代过程**********"<<endl;
cout<<"请输入Xs的改正数应满足的限差值(毫米): "; cin>>define;
while(fabs(k.addx)>define)//////////////////确定迭代条件，以下是迭代过程/////////////////////
{
k.getdata();
k.zhuanzhi(k.a,k.at,8,6);
k.time(k.at,k.a,k.a_at,6,8,6);
k.qiuni(k.a_at,k.QIUNI_at_a,6);
k.time(k.QIUNI_at_a,k.at,k.QIUNI_at_a_at,6,6,8);
k.time(k.QIUNI_at_a_at,k.L,k.XX,6,8,1);
k.addx=k.XX[0];k.addy=k.XX[1];k.addz=k.XX[2];k.add1=k.XX[3];k.add2=k.XX[4];k.add3=k.XX[5];
++i;
cout<<endl;///////////////////////////////////////////////////输出每一次迭代后的改正数
cout<<"经过第"<<i<<"次迭代外方位元素的改正数："<<endl;
cout<<"Xs的改正数: "<<k.addx<<" 毫米"<<endl;
cout<<"Ys的改正数: "<<k.addy<<" 毫米"<<endl;
cout<<"Zs的改正数: "<<k.addz<<" 毫米"<<endl;
cout<<"φ的改正数: "<<k.add1<<" 转换为角度："; k.change(k.add1);
cout<<"ω的改正数: "<<k.add2<<" 转换为角度："; k.change(k.add2);
cout<<"κ的改正数: "<<k.add3<<" 转换为角度："; k.change(k.add3);
cout<<"//////////////////////////////////////////"<<endl;
}
////////////////////////////输出外方位元素的最终改正值
cout<<"一共迭代了"<<i<<"次："<<" "<<"以下是外方位元素的最终改正值："<<endl;
cout<<"Xs的改正值: "<<k.Xs0<<" 毫米"<<endl;
cout<<"Ys的改正值: "<<k.Ys0<<" 毫米"<<endl;
cout<<"Zs的改正值: "<<k.Zs0<<" 毫米"<<endl;
cout<<"φ的改正值: "<<k.angle1<<" 转换为角度："; k.change(k.angle1);
cout<<"ω的改正值: "<<k.angle2<<" 转换为角度："; k.change(k.angle2);
cout<<"κ的改正值: "<<k.angle3<<" 转换为角度："; k.change(k.angle3);
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -