写得很糟糕
不太好意思拿出来
#include "stdio.h"
#include "math.h"
#include "iostream.h"
#include "math.h"
struct SpatialPoint //空间点
{
double x;
double y;
double z;
};
struct ImagePoint //影像点
{
double x;
double y;
};
struct RotationMatrix //旋转矩阵
{
double a[3];
double b[3];
double c[3];
};
struct ErrorMatrix //误差系数矩阵
{
double a[2][6];
};
struct ErrorMatrixT //误差系数矩阵的转置矩阵
{
double a[6][2];
};
struct LMatrix //L矩阵
{
double l[2][1];
};
struct XMatrix
{
double XS[6][1];
};
void MatrixMulti(double a[6][2],double b[2][6],double c[6][6]) //矩阵相乘函数,结果存在c中，6行2列乘以2行6列
{
int i,j,k;
for(i=0;i<6;i++)
for(j=0;j<6;j++)
c[i][j]=0;
for(i=0;i<6;i++)
for(j=0;j<6;j++)
for(k=0;k<2;k++)
c[i][j]+=a[i][k]*b[k][j];
}
void MatrixMulti1(double a[6][6],double b[6][2],double c[6][2]) //矩阵相乘函数,结果存在c中,6行6列乘以6行2列
{
int i,j,k;
for(i=0;i<6;i++)
for(j=0;j<2;j++)
c[i][j]=0;
for(i=0;i<6;i++)
for(j=0;j<2;j++)
for(k=0;k<6;k++)
c[i][j]+=a[i][k]*b[k][j];
}
void MatrixMulti2(double a[6][2],double b[2][1],double c[6][1]) //矩阵相乘函数,结果存在c中,6行2列乘以2行1列
{
int i,j,k;
for(i=0;i<6;i++)
for(j=0;j<1;j++)
c[i][j]=0;
for(i=0;i<6;i++)
for(j=0;j<1;j++)
for(k=0;k<2;k++)
c[i][j]+=a[i][k]*b[k][j];
}
void ContraryMatrix(double *a,int n) //矩阵求逆
{
int i,j,k;
for(k=0;k<n;k++)
{
for(i=0;i<n;i++)
{
if(i!=k)
*(a+i*n+k)=-*(a+i*n+k)/(*(a+k*n+k));
}
*(a+k*n+k)=1/(*(a+k*n+k));
for(i=0;i<n;i++)
{
if(i!=k)
{
for(j=0;j<n;j++)
{
if(j!=k)
*(a+i*n+j)+=*(a+k*n+j)* *(a+i*n+k);
}
}
}
for(j=0;j<n;j++)
{
if(j!=k)
*(a+k*n+j)*=*(a+k*n+k);
}
}
}
void main()
{
int scale=3000; //比例尺
double x0=0,y0=0,f=0.15324; //内方位元素
double xs0=0,ys0=0,zs0,F0=0,W0=0,K0=0;
//double xs,ys,zs,F,W,K; //要求的外方位元素
double H; //航高
int gg=0;
struct SpatialPoint Sp[4]={{36589.41,25273.32,2195.17}, //地面点坐标
{37631.08,31324.51,728.69},
{39100.97,24934.98,2386.50},
{40426.54,30319.81,757.31}
};
struct ImagePoint Ip[4]={ //影像点坐标
{-86.15,-68.99},
{-53.40,82.21},
{-14.78,-76.63},
{10.46,64.43}
};
int i,j,k;
struct RotationMatrix R; //像空间坐标和像空间辅助坐标的转换矩阵R
struct ImagePoint Ip1[4]; //像点坐标近似值
double X1,Y1,Z1;
struct ErrorMatrix A[4]; //误差系数
struct LMatrix L[4]; //L矩阵
struct XMatrix X; //X矩阵
struct ErrorMatrixT AT[4];
double c[4][6][6];
double d[4][6][6];
double e[4][6][2];
zs0=scale*f; //设置初始值
H=zs0;
for(i=0;i<4;i++)
{
xs0+=Sp[i].x;
ys0+=Sp[i].y;
}
xs0=xs0/4;
ys0=ys0/4;
for(i=0;i<4;i++)
{
Ip[i].x/=1000;
Ip[i].y/=1000;
}
for(i=0;i<6;i++)
X.XS[i][0]=1;
//while(X.XS[3][0]>0.095||X.XS[4][0]>0.001||X.XS[5][0]>0.001)
//{
//计算旋转矩阵R
R.a[0]=cos(F0)*cos(K0)-sin(F0)*sin(W0)*sin(K0);
R.a[1]=-cos(F0)*sin(K0)-sin(F0)*sin(W0)*cos(K0);
R.a[2]=-sin(F0)*cos(W0);
R.b[0]=cos(W0)*sin(K0);
R.b[1]=cos(W0)*cos(K0);
R.b[2]=-sin(W0);
R.c[0]=sin(F0)*cos(K0)+cos(F0)*sin(W0)*sin(K0);
R.c[1]=-sin(F0)*sin(K0)+cos(F0)*sin(W0)*cos(K0);
R.c[2]=cos(F0)*cos(W0);
//逐点计算像点坐标的近似值
for(i=0;i<4;i++)
{
X1=R.a[0]*(Sp[i].x-xs0)+R.b[0]*(Sp[i].y-ys0)+R.c[0]*(Sp[i].z-zs0);
Y1=R.a[1]*(Sp[i].x-xs0)+R.b[1]*(Sp[i].y-ys0)+R.c[1]*(Sp[i].z-zs0);
Z1=R.a[2]*(Sp[i].x-xs0)+R.b[2]*(Sp[i].y-ys0)+R.c[2]*(Sp[i].z-zs0);
Ip1[i].x=x0-f*X1/Z1;
Ip1[i].y=y0-f*Y1/Z1;
}
//逐点计算误差方程式的系数和常数项，组成误差方程式
for(i=0;i<4;i++) //求A矩阵
{
Z1=R.a[2]*(Sp[i].x-xs0)+R.b[2]*(Sp[i].y-ys0)+R.c[2]*(Sp[i].z-zs0);
A[i].a[0][0]=(R.a[0]*f+R.a[2]*(Ip1[i].x-x0))/Z1;
A[i].a[0][1]=(R.b[0]*f+R.b[2]*(Ip1[i].x-x0))/Z1;
A[i].a[0][2]=(R.c[0]*f+R.c[2]*(Ip1[i].x-x0))/Z1;
A[i].a[1][0]=(R.a[1]*f+R.a[2]*(Ip1[i].y-y0))/Z1;
A[i].a[1][1]=(R.b[1]*f+R.b[2]*(Ip1[i].y-y0))/Z1;
A[i].a[1][2]=(R.c[1]*f+R.c[2]*(Ip1[i].y-y0))/Z1;
A[i].a[0][3]=(Ip1[i].y-y0)*sin(W0)-((Ip1[i].x-x0)/f*((Ip1[i].x-x0)*cos(K0)-(Ip1[i].y-y0)*sin(K0))+f*cos(K0))*cos(W0);
A[i].a[0][4]=-f*sin(K0)-((Ip1[i].x-x0)/f)*((Ip1[i].x-x0)*sin(K0)+(Ip1[i].y-y0)*cos(K0));
A[i].a[0][5]=Ip1[i].y-y0;
A[i].a[1][3]=-(Ip1[i].x-x0)*sin(W0)-((Ip1[i].y-y0)/f*((Ip1[i].x-x0)*cos(K0)-(Ip1[i].y-y0)*sin(K0))-f*sin(K0))*cos(W0);
A[i].a[1][4]=-f*cos(K0)-((Ip1[i].y-y0)/f)*((Ip1[i].x-x0)*sin(K0)+(Ip1[i].y-y0)*cos(K0));
A[i].a[1][5]=-(Ip1[i].x-x0);
}
for(i=0;i<4;i++) //求L矩阵
{
L[i].l[0][0]=Ip[i].x-Ip1[i].x;
L[i].l[1][0]=Ip[i].y-Ip1[i].y;
}
for(i=0;i<4;i++) //求A矩阵的转置矩阵
for(j=0;j<2;j++)
for(k=0;k<6;k++)
{
AT[i].a[j][k]=A[i].a[k][j];
}
i=0;
while(fabs(X.XS[3][0])>0.001||fabs(X.XS[4][0])>0.001||fabs(X.XS[5][0])>0.001)
{gg++;
MatrixMulti(AT[i].a,A[i].a,c[i]);
ContraryMatrix(*c[i],6);
MatrixMulti1(c[i],AT[i].a,e[i]);
MatrixMulti2(e[i],L[i].l,X.XS);
i++;
if(i==4)
i=0;
xs0+=X.XS[0][0];
ys0+=X.XS[1][0];
zs0+=X.XS[2][0];
F0+=X.XS[3][0];
W0+=X.XS[4][0];
K0+=X.XS[5][0];
}
/* for(i=0;i<4;i++)
for(j=0;j<6;j++)
{
X[i].XS
*/
/* xs0+=X.XS[0][0];
ys0+=X.XS[1][0];
zs0+=X.XS[2][0];
F0+=X.XS[3][0];
W0+=X.XS[4][0];
K0+=X.XS[5][0];
*/
//}
cout<<"xs0="<<xs0<<endl;
cout<<"ys0="<<ys0<<endl;
cout<<"zs0="<<zs0<<endl;
cout<<R.a[0]<<" "<<R.a[1]<<" "<<R.a[2]<<" "<<endl;
cout<<R.b[0]<<" "<<R.b[1]<<" "<<R.b[2]<<" "<<endl;
cout<<R.c[0]<<" "<<R.c[1]<<" "<<R.c[2]<<" "<<endl;
cout<<gg;
}
