后方交会程序实现c语言版.doc

#include #include

#include

#include

#include

#define N 4

#define T 1.41421

void turn(double *A,double A2[],int m,int n) // 计算矩阵的转置

{ int i,j; for(i=0;i

for(j=0;j

A2[j*m+i]=A[i*n+j];

}

void mulAB(double *A,double *B,double *C,int am,int an,int bm,int bn) //

计算两矩阵相乘{ int i,j,l,u;

if(an!=bm)

{ printf("error!cannot do the multiplication.\n");

return; }

for(i=0;i

for(j=0;j

u=i*bn+j;

C[u]=0.0;

for(l=0;l

C[u]+=A[i*an+l]*B[l*bn+j];

} return;

}

double *inv(double *a,int n) // 计算矩阵的逆，本程序的难

,采用高斯约旦全选主元法点

{

int *is,*js,i,j,k,l,u,v;

double d,p;

is=(int*)malloc(n*sizeof(int));

js=(int*)malloc(n*sizeof(int));

for (k=0; k<=n-1; k++)

{

d=0.0;

for (i=k;i

if (p>d) { d=p; is[k]=i; js[k]=j;

}

}

if (d+1.0==1.0)

{ free(is);

free(js);

printf("error not inv\n");

return NULL;

}

if (is[k]!=k)

for (j=0;j

{ u=k*n+j;

v=is[k]*n+j;

p=a[u];

a[u]=a[v];

a[v]=p;

}

if (js[k]!=k)

for (i=0;i

{ u=i*n+k;

v=i*n+js[k];

p=a[u];

a[u]=a[v];

a[v]=p;

}

l=k*n+k;

a[l]=1.0/a[l];

for (j=0;j

{

u=k*n+j;

a[u]=a[u]*a[l];

}

for (i=0;i

if (i!=k)

for (j=0;j

if (j!=k)

a[u]=a[u]-a[i*n+k]*a[k*n+j];

}

for (i=0;i

if (i!=k)

{ u=i*n+k;

a[u]=-a[u]*a[l];

}

}

for (k=n-1;k>=0;k--)

{ if (js[k]!=k)

for (j=0;j<=n-1;j++)

{ u=k*n+j;

v=js[k]*n+j;

a[u]=a[v];

a[v]=p;

}

if (is[k]!=k)

for (i=0;i

{ u=i*n+k;

v=i*n+is[k];

p=a[u];

a[u]=a[v];

a[v]=p;

}

}

free(is);

free(js);

return a;

}

main()// 主函数，空间后方交会的计算{

FILE *fp; // 定义一个文件指针

long m;

int i,j=0,it;

double G[1000];

double

f,t,w,k,limit=0,S1=0.0,S2=0.0,S3=0.0,x[N]={0},y[N]={0},x0[N]={0},y0[N]={0},X[N]={0},Y[N] ={0},Z[N]={0},Xs0,Ys0,Zs0;

double

a[3],b[3],c[3],A[2*N*6],AT[6*2*N],ATA[6*6],*ATA_=NULL,l[2*N],ATl[6],V[6]={0};

double F[6],Qx[6][6],Mi[6][6];

if((fp=fopen("e:\\shuju.txt","r"))==NULL) // 使 fp 指向被打开的

文件

{ // 并判断文件是否打开成功

printf("\nerror on open shuju.txt\n");

getch();

exit(1);

} for(i=0;i

fscanf(fp,"%lf%lf%lf%lf%lf",&x[i],&y[i],&X[i],&Y[i],&Z[i]); 件中的数据赋给主函数定义的变量

printf(" 原始数据 :\n");

printf("x\t\ty\t\tX\t\tY\t\tZ\t\t\n");// 输出文件中的原始数据

for(i=0;i

printf("%.3lf\t\t%.3lf\t\t%.3lf\t%.3lf\t%.3lf\n",x[i],y[i],X[i],Y[i],Z[i]);

printf("\n 请输入摄影机主距和摄影比例尺分母； f,m:");

scanf("%lf%ld",&f,&m); //输入 f,m

f=f/1000.0;

for(i=0;i

{ x[i]/=1000.0;

y[i]/=1000.0;

S1+=X[i];

S2+=Y[i];

S3+=Z[i];

}

Xs0=S1/N;

Ys0=S2/N;

Zs0=m*f+S3/N;

t=0.0;w=0.0;k=0.0; // 计算外方位元素的初始值

while(1)

shuju.txt

// 将 文

printf("\n ------------------------------- 第%d 次计算

----------------------------- \n",j+1);

a[0]=cos(t)*cos(k)-sin(t)*sin(w)*sin(k);

a[1]=-cos(t)*sin(k)-sin(t)*sin(w)*cos(k);

a[2]=-sin(t)*cos(w);

b[0]=cos(w)*sin(k);

b[1]=cos(w)*cos(k);

b[2]=-sin(w);

c[0]=sin(t)*cos(k)+cos(t)*sin(w)*sin(k);

c[1]=-sin(t)*sin(k)+cos(t)*sin(w)*cos(k);

c[2]=cos(t)*cos(w); // 计算旋转矩阵

for(i=0;i

{

x0[i]=-f*(a[0]*(X[i]-Xs0)+b[0]*(Y[i]-Ys0)+c[0]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c [2]*(Z[i]-Zs0));

y0[i]=-f*(a[1]*(X[i]-Xs0)+b[1]*(Y[i]-Ys0)+c[1]*(Z[i]-Zs0))/(a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c

[2]*(Z[i]-Zs0));

//计算像点坐标近似值

G[i]=a[2]*(X[i]-Xs0)+b[2]*(Y[i]-Ys0)+c[2]*(Z[i]-Zs0);

}

for(i=0;i

{

A[i*12+0]=(a[0]*f+a[2]*x[i])/G[i];

A[i*12+1]=(b[0]*f+b[2]*x[i])/G[i];

A[i*12+2]=(c[0]*f+c[2]*x[i])/G[i];

A[i*12+3]=y[i]*sin(w)-(x[i]*(x[i]*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);

A[i*12+4]=-f*sin(k)-x[i]*(x[i]*sin(k)+y[i]*cos(k))/f;

A[i*12+5]=y[i];

A[i*12+6]=(a[1]*f+a[2]*y[i])/G[i];

A[i*12+7]=(b[1]*f+b[2]*y[i])/G[i];

A[i*12+8]=(c[1]*f+c[2]*y[i])/G[i];

A[i*12+9]=-x[i]*sin(w)-(y[i]*(x[i]*cos(k)-y[i]*sin(k))/f-f*sin(k))*cos(w);

A[i*12+10]=-f*cos(k)-y[i]*(x[i]*sin(k)+y[i]*cos(k))/f;