📄 back.cpp
字号:
#include <iostream.h>
#include "math.h"
#include <stdlib.h>
#include <stdio.h>
int m=50000;//比例尺分母
double f=0.15324;
double xx=0;
double yy=0;
long double phi;
long double omega;
long double kappa;
double xs;
double ys;
double zs;
double limitmargin=0.1*3.1415926/10800;
/**************************************************************************/
/*通过控制点的像坐标和地面坐标计算法方程的系数矩阵和常数项
*/
/*************************************************************************/
void backresection(double *A, double *l,int costart,int vstart,double obs_x,
double obs_y,double X,double Y,double Z)
{
double a1;
double a2;
double a3;
double b1;
double b2;
double b3;
double c1;
double c2;
double c3;
double est_x;
double est_y;
double lx;
double ly;
double a11;
double a12;
double a13;
double a21;
double a22;
double a23;
double a14;
double a15;
double a16;
double a24;
double a25;
double a26;
a1=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);
a2=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);
a3=-sin(phi)*cos(omega);
b1=cos(phi)*sin(kappa);
b2=cos(omega)*cos(kappa);
b3=-sin(omega);
c1=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);
c2=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);
c3=cos(phi)*cos(omega);
est_x=-f*(a1*(X-xs)+b1*(Y-ys)+c1*(Z-zs))/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
est_y=-f*(a2*(X-xs)+b2*(Y-ys)+c2*(Z-zs))/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a11=(a1*f+a3*est_x)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a12=(b1*f+b3*est_x)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a13=(c1*f+c3*est_x)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a21=(a2*f+a3*est_y)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a22=(b2*f+b3*est_y)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a23=(c2*f+c3*est_y)/(a3*(X-xs)+b3*(Y-ys)+c3*(Z-zs));
a14=est_y*sin(omega)-(est_x/f*(est_x*cos(kappa)-est_y*sin(kappa))+f*cos(kappa))*cos(omega);
a15=-f*sin(kappa)-est_x/f*(est_x*sin(kappa)+est_y*cos(kappa));
a16=est_y;
a24=-est_y*sin(omega)-(est_x/f*(est_x*cos(kappa)-est_y*sin(kappa))-f*sin(kappa))*cos(omega);
a25=-f*cos(kappa)-est_y/f*(est_x*sin(kappa)+est_y*cos(kappa));
a26=-est_x;
lx=obs_x-est_x;
ly=obs_y-est_y;
A[costart+0]=a11;
A[costart+1]=a12;
A[costart+2]=a13;
A[costart+3]=a14;
A[costart+4]=a15;
A[costart+5]=a16;
A[costart+6]=a21;
A[costart+7]=a22;
A[costart+8]=a23;
A[costart+9]=a24;
A[costart+10]=a25;
A[costart+11]=a26;
l[vstart+0]=lx;
l[vstart+1]=ly;
}
//功 能： 矩阵求逆
//输 入： 矩阵a，阶数n。
//输 出： 返回0（成功），1（失败），经求逆后，a矩阵为逆矩阵
int InverseMatrix(double *a, int n)
{
int *is,*js;
int i,j,k,l,u,v;
double temp,max_v;
is=(int *)malloc(n*sizeof(int));
js=(int *)malloc(n*sizeof(int));
if(is==NULL||js==NULL) return(0);
for(k=0;k<n;k++)
{
max_v=0.0;
for(i=k;i<n;i++)
{
for(j=k;j<n;j++)
{
temp=fabs(a[i*n+j]);
if(temp>max_v)
{
max_v=temp; is[k]=i; js[k]=j;
}
}
}
if(max_v==0.0)
{
free(is); free(js);
return(0);
}
if(is[k]!=k)
{
for(j=0;j<n;j++)
{
u=k*n+j; v=is[k]*n+j;
temp=a[u]; a[u]=a[v]; a[v]=temp;
}
}
if(js[k]!=k)
{
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+js[k];
temp=a[u]; a[u]=a[v]; a[v]=temp;
}
}
l=k*n+k;
a[l]=1.0/a[l];
for(j=0;j<n;j++)
{
if(j!=k)
{
u=k*n+j;
a[u]*=a[l];
}
}
for(i=0;i<n;i++)
{
if(i!=k)
{
for(j=0;j<n;j++)
{
if(j!=k)
{
u=i*n+j;
a[u]-=a[i*n+k]*a[k*n+j];
}
}
}
}
for(i=0;i<n;i++)
{
if(i!=k)
{
u=i*n+k;
a[u]*=-a[l];
}
}
}
for(k=n-1;k>=0;k--)
{
if(js[k]!=k)
{
for(j=0;j<n;j++)
{
u=k*n+j; v=js[k]*n+j;
temp=a[u]; a[u]=a[v]; a[v]=temp;
}
}
if(is[k]!=k)
{
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+is[k];
temp=a[u]; a[u]=a[v]; a[v]=temp;
}
}
}
free(is); free(js);
return(1);
}
//功 能： 矩阵转置
//输 入： 矩阵a（mxn阶）转置后存在矩阵at（nxm阶）中
//输 出： void，结果在矩阵at中
void TransposeMatrix(double *a, double *at, int m, int n)
{
int i,j;
for(i=0;i<n;i++)
for(j=0;j<m;j++)
*(at+i*m+j)=*(a+j*n+i);
}
//功 能： 矩阵相乘
//输 入： 矩阵a（mxn阶）乘以矩阵b（nxk阶）等于矩阵c（mxk阶）。
//输 出： void，结果在矩阵c中
void MultiplyMatrix(double *a, double *b, double *c, int m, int n, int k)
{
int i,j,ii;
for(i=0;i<m;i++)
{
for(j=0;j<k;j++)
{
*(c+i*k+j)=0;
for(ii=0;ii<n;ii++)
*(c+i*k+j)=*(c+i*k+j)+*(a+i*n+ii)**(b+ii*k+j);
}
}
}
void main()
{
int i=0;
double dxs;
double dys;
double dzs;
double dphi=0.1;
double domega=0.1;
double dkappa=0.1;
double *A=new double[48];
double *l=new double[8];
double *At=new double[48];
double *AtA=new double[36];
double *AtAAt=new double[48];
double *AtAAtl=new double[6];
xs=(36589.41+37631.08+39100.97+40426.54)/4;
ys=(25273.32+31324.51+24934.98+30319.81)/4;
zs=m*f;
phi=0;
omega=0;
kappa=0;
while(dphi>=limitmargin || domega>=limitmargin || dkappa>=limitmargin)
{
backresection(A,l,0,0,-0.08615,-0.06899,36589.41,25273.32,2195.17);
backresection(A,l,12,2,-0.05340,0.08221,37631.08,31324.51,728.69);
backresection(A,l,24,4,-0.01478,-0.07663,39100.97,24934.98,2386.50);
backresection(A,l,36,6,0.01046,0.06443,40426.54,30319.81,757.31);
TransposeMatrix(A, At, 8,6);
MultiplyMatrix(At,A,AtA,6,8,6);
InverseMatrix(AtA,6);
MultiplyMatrix(AtA,At,AtAAt,6,6,8);
MultiplyMatrix(AtAAt,l,AtAAtl,6,8,1);
dxs=AtAAtl[0];
dys=AtAAtl[1];
dzs=AtAAtl[2];
dphi=AtAAtl[3];
domega=AtAAtl[4];
dkappa=AtAAtl[5];
xs+=dxs;
ys+=dys;
zs+=dzs;
phi+=dphi;
omega+=domega;
kappa+=dkappa;
i+=1;
}
int j;
int k;
double a1=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);
double a2=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);
double a3=-sin(phi)*cos(omega);
double b1=cos(phi)*sin(kappa);
double b2=cos(omega)*cos(kappa);
double b3=-sin(omega);
double c1=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);
double c2=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);
double c3=cos(phi)*cos(omega);
double a[3][3]={{a1,a2,a3},{b1,b2,b3},{c1,c2,c3}};
cout<<"外方位元素的计算结果为:"<<endl;
printf(" xs=");
printf("%.2lf\n",xs);
printf(" ys=");
printf("%.2lf\n",ys);
printf(" zs=");
printf("%.2lf\n",zs);
// printf(" phi=");
// printf("%.2lf\n",sin(phi));
// printf(" omega=");
// printf("%.2lf\n",sin(omega));
// printf(" kappa=");
// printf("%.2lf\n",sin(kappa));
cout<<"旋转矩阵R为："<<endl;
for(j=0;j<3;j++)
{ for(k=0;k<3;k++)
{
cout<<a[j][k]<<" ";
}
cout<<endl;
}
printf("共迭代的次数为：");
printf("%d",i);
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -