📄 houfang1.cpp
字号:
#include<stdio.h>
#include<math.h>
#include<iostream.h>
#include<stdlib.h>
#define N 4
#define pi 3.1415926
#define xiancha 0.01*pi/180/3600 /*限差为0.01秒 */
void T(double *,double *,int,int);
void XX(double *,double *,double *,int,int,int);
void inv(double *,double *,int);
void getAccompany(double *,double *,int);
double det(double *,int );
void main()
{
int i,j,cnt; /* cnt为迭代次数*/
double data1[N][3]={{36589.41,25273.32,2195.17},{37631.08,31324.51,728.69},{39100.97,24934.98,2386.50},{40426.54,30319.81,757.31}};
double data2[N][2]={{-86.1500,-68.9900},{-53.4000,82.2100},{-14.7800,-76.6300},{10.4600,64.4300}};/*单位为毫米*/
double x0,y0,m0,f; /*内方位元素*/
double Xs,Ys,Zs,Omega=0,Fai=0,Kapa=0,dXs=0,dYs=0,dZs=0,dOmega=0,dFai=0,dKapa=0; /*外方位元素及初值化*/
double ATA[6][6],ATAT[6][6],ATAtemp[6][6],ATL[6],ATLtemp[6];
double R[3][3]={1,0,0,0,1,0,0,0,1},dX[6]={0,0,0,0,0,0};
x0=0. ,y0=0. ,f=153.24/1000.; /*内方位元素赋值 */
m0=40000.;
Xs=Ys=Zs=0;
for (i=0;i<N;i++)
{
Xs+=data1[i][0],Ys+=data1[i][1],Zs+=
data1[i][2];
data2[i][0]=data2[i][0]/1000.-x0;
data2[i][1]=data2[i][1]/1000.-y0;
}
// m0=(data1[0][0]-data1[1][0])/(data2[0][0]-data2[1][0]);
/*m0初始值 */
Xs/=N;Ys/=N;Zs=Zs/N+m0*f;
cout<<"初值分别为:Xs="<<Xs<<" Ys="<<Ys<<" Zs="<<Zs<<endl;
cnt=0;
for(i=0;i<6;i++) /* ATA ATL ATAtemp ATLtemp ATAN初值化*/
{
ATL[i]=0;
ATLtemp[i]=0;
for(j=0;j<6;j++)
{
ATA[i][j]=0;
ATAT[i][j]=0;
ATAtemp[i][j]=0;}
}
while((fabs(dOmega)>xiancha||fabs(dFai)>xiancha||fabs(dKapa)>xiancha||(dOmega==0.&&dFai==0.&&dKapa==0.))&&cnt<=10)
{
double A[2][6],AT[6][2],L[2];
double a1,a2,a3,b1,b2,b3,c1,c2,c3;
for(i=0;i<6;i++) ATL[i]=0.; //初值化
for(i=0;i<6;i++)
for(j=0;j<6;j++) ATA[i][j]=0.;
/*旋转矩阵 */
a1=R[0][0];a2=R[0][1];a3=R[0][2];
b1=R[1][0];b2=R[1][1];b3=R[1][2];
c1=R[2][0];c2=R[2][1];c3=R[2][2];
for (i=0;i<N;i++)
{
double X_,Y_,Z_,X,Y,Z,x,y;
double a11,a12,a13,a14,a15,a16,a21,a22,a23,a24,a25,a26;
x=data2[i][0];y=data2[i][1];
X=data1[i][0];Y=data1[i][1];Z=data1[i][2];
X_=a1*(X-Xs)+b1*(Y-Ys)+c1*(Z-Zs);
Y_=a2*(X-Xs)+b2*(Y-Ys)+c2*(Z-Zs);
Z_=a3*(X-Xs)+b3*(Y-Ys)+c3*(Z-Zs);
L[0]=x+X_/Z_*f;
L[1]=y+Y_/Z_*f;
/*构造系数阵 */
a11=(a1*f+a3*x)/Z_; a12=(b1*f+b3*x)/Z_; a13=(c1*f+c3*x)/Z_;
a14=y*sin(Omega)-(x/f*(x*cos(Kapa)-y*sin(Kapa))+f*cos(Kapa))*cos(Omega);
a15=-f*sin(Kapa)-x/f*(x*sin(Kapa)+y*cos(Kapa)); a16=y;
a21=(a2*f+a3*y)/Z_; a22=(b2*f+b3*y)/Z_; a23=(c2*f+c3*y)/Z_;
a24=-x*sin(Omega)-(y/f*(x*cos(Kapa)-y*sin(Kapa))-f*sin(Kapa))*cos(Omega);
a25=-f*cos(Kapa)-y/f*(x*sin(Kapa)+y*cos(Kapa)); a26=-x;
A[0][0]=a11;A[0][1]=a12;A[0][2]=a13;
A[0][3]=a14;A[0][4]=a15;A[0][5]=a16;
A[1][0]=a21;A[1][1]=a22;A[1][2]=a23;
A[1][3]=a24;A[1][4]=a25;A[1][5]=a26;
T(&A[0][0],&AT[0][0],2,6);
XX(&AT[0][0],&A[0][0],&ATAtemp[0][0],6,2,6);
XX(&AT[0][0],&L[0],&ATLtemp[0],6,2,1);
for (int i1=0;i1<6;i1++)
{
ATL[i1]+=ATLtemp[i1];
for (int j1=0;j1<6;j1++)
ATA[i1][j1]+=ATAtemp[i1][j1];
}
}
inv(&ATA[0][0],&ATAT[0][0],6);
XX(&ATAT[0][0],&ATL[0],&dX[0],6,6,1);
dXs=dX[0];dYs=dX[1];dZs=dX[2];dFai=dX[3];dOmega=dX[4];dKapa=dX[5];
Xs+=dXs;Ys+=dYs;Zs+=dZs;Fai+=dFai;Omega+=dOmega;Kapa+=dKapa;
//计算旋转矩阵
R[0][0]=cos(Fai)*cos(Kapa)-sin(Fai)*sin(Omega)*sin(Kapa);
R[0][1]=-cos(Fai)*sin(Kapa)-sin(Fai)*sin(Omega)*cos(Kapa);
R[0][2]=-sin(Fai)*cos(Omega);
R[1][0]=cos(Omega)*sin(Kapa);
R[1][1]=cos(Omega)*cos(Kapa);
R[1][2]=-sin(Omega);
R[2][0]=sin(Fai)*cos(Kapa)+cos(Fai)*sin(Omega)*sin(Kapa);
R[2][1]=-sin(Fai)*sin(Kapa)+cos(Fai)*sin(Omega)*cos(Kapa);
R[2][2]=cos(Fai)*cos(Omega);
cnt=cnt+1;
}
cout<<"迭代次数:cnt="<<cnt;
if(cnt>10) cout<<" 迭代出错!!!"<<endl;
cout<<"限差为:"<<xiancha<<endl;
cout<<" Xs="<<Xs<<" Ys="<<Ys<<" Zs="<<Zs<<endl;
cout<<" Fai="<<Fai<<" Omega="<<Omega<<" Kapa="<<Kapa<<endl;
cout<<"对应的改正数为: "<<endl;
for (i=0;i<6;i++)
{
if(i==3) cout<<endl;
cout<<" "<<dX[i];
}
cout<<endl;
cout<<"旋转矩阵: R="<<endl;
for (i=0;i<3;i++)
{
for (j=0;j<3;j++)
cout<<" "<<R[i][j];
cout<<endl;
}
}
//矩阵辅助运算函数
void inv(double *A,double *B,int a) //求逆矩阵
{
double tp,s;
tp=1.0/A[0];
if(a==1) {*B=tp;return;}
s=det(A,a);
getAccompany(A,B,a);
tp=1.0/s;
for(int i=0;i<a*a;i++)
B[i]=B[i]*tp;
}
void T(double *A,double *temp,int a,int b)
{
if(a==0||b==0) {*temp=0;return;}
if(a==1&&b==1) {*temp=*A;return;}
for(int i=0;i<b;i++)
{
for(int j=0;j<a;j++)
{
temp[i*a+j]=A[j*b+i];
}
}
}
void XX(double *A,double *B,double *C,int a,int b,int c)
{
for(int i=0;i<a;i++)
{
for(int j=0;j<c;j++)
{
double total=0.0;
for(int n=0;n<b;n++)
{
total+=A[i*b+n]*B[n*c+j];
}
C[i*c+j]=total;
}
}
}
double det(double *aa,int n)
{
int i,j,row,col,k;
double max,temp,det=1.,fuhao;
double *a=(double*)malloc(n*n*sizeof(double));
for(k=0;k<n*n;k++)
{
a[k]=aa[k];
}
for(k=0;k<n;k++)
{
max=0;row=col=k;fuhao=1.0;
for(i=k;i<n;i++)
for(j=k;j<n;j++)
{
temp=fabs(a[i*n+j]);
if(max<temp) { max=temp;row=i;col=j;}
}
if(max==0) return 0;
if(row!=k)
{ for(j=0;j<n;j++)
{temp=a[row*n+j];a[row*n+j]=a[k*n+j];a[k*n+j]=temp;}
fuhao=-fuhao;
}
if(col!=k)
{
for(i=0;i<n;i++)
{temp=a[i*n+col];a[i*n+col]=a[i*n+k];a[i*n+k]=temp;}
fuhao=-fuhao;
}
det*=fuhao*a[k*n+k];
for(j=k+1;j<n;j++) a[k*n+j]/=a[k*n+k];
for(j=k+1;j<n;j++)
for(i=k+1;i<n;i++)
a[i*n+j]-=a[i*n+k]*a[k*n+j];
for(i=0;i<k;i++) a[i*n+k]=0;
a[k*n+k]=1;
}
free(a);
return det;
}
void getAccompany(double *A,double *B,int a)
{
double *Bt=(double*)malloc(a*a*sizeof(double));
for(int i=0;i<a;i++)
{
for(int j=0;j<a;j++)
{
int num=0;
double *tp=(double*)malloc((a-1)*(a-1)*sizeof(double));
for(int g=0;g<a;g++)
{
if(g==i) continue;
for(int h=0;h<a;h++)
{
if(h==j) continue;
else
{
tp[num]=A[g*a+h];
num++;
}
}
}
int tip;
double fuhao,s;
tip=i+j+2;
fuhao=1.0;
if(tip%2==1) fuhao=-1.0;
s=det(tp,a-1);
Bt[i*a+j]=fuhao*s;
free(tp);
}
}
T(Bt,B,a,a);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -