后方交会.cpp

关于解析空三的全过程

#include "stdio.h"
#include "stdlib.h"
#include "malloc.h"
#include "math.h"
#include "iostream.h"
#include "string.h"
#include "brinv.h"

 //求取转置矩阵的函数
void zhuanzhi(double **A,double **AT,int w,int n)
{
	int i,j;
	for(i=0;i<w;i++)
		for(j=0;j<n;j++)
		{
			AT[j][i]=A[i][j];//**求系数矩阵的逆AT******
		}
}
int inverse(double **bb, int n) //求逆函数
{
	int *is,*js,i,j,k,l,u,v;
	double *a=(double*)malloc(sizeof(double)*n*n);
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
			a[n*i+j]=bb[i][j];
		
	}

	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<=n-1; i++)
			for (j=k; j<=n-1; j++)
			{ 
				l=i*n+j; 
				p=fabs(a[l]);
				if (p>d) 
				{ 
					d=p; 
					is[k]=i;
					js[k]=j;
				} 
			}
			if (d+1.0==1.0)//主要是防止计算机的舍入误差造成的错误。
			{ 
				free(is); 
				free(js);
				printf("err**not inv\n");
				return(0);
			}
			if (is[k]!=k) 
				for (j=0; j<=n-1; 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<=n-1; 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<=n-1; j++)
						if (j!=k)
						{ 
							u=k*n+j; 
							a[u]=a[u]*a[l];
						} 
						for (i=0; i<=n-1; i++)
							if (i!=k) 
								for (j=0; j<=n-1; j++)
									if (j!=k)
									{ 
										u=i*n+j;
										a[u]=a[u]-a[i*n+k]*a[k*n+j];
									} 
						for (i=0; i<=n-1; i++)
							if (i!=k)
							{ 
								u=i*n+k; 
								a[u]=-a[u]*a[l];
							}
	}//结束for (k=0; k<=n-1; k++)
	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; 
				p=a[u]; a[u]=a[v]; a[v]=p;
			} 
		if (is[k]!=k)
			for (i=0; i<=n-1; i++)
			{ 
				u=i*n+k; 
				v=i*n+is[k];
				p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
	} 

	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
			bb[i][j]=a[n*i+j];
		
	}
	free(is);
	free(js); 
	return(1); 
} //结束矩阵转置函数inverse()*/


//矩阵相乘的函数
void xiangc(double **A,double **B,double **c,int u,int w,int l)
{
	int i,j,n;
	for(i=0;i<u;i++)
		for(j=0;j<l;j++)
			for(n=0;n<w;n++)
				{
					c[i][j]+=A[i][n]*B[n][j];
				}


}



void main()
{
	//****************第一步*******************//
	
	//***********读出数据，从文本文档里读出数据**********//
	//分别将象点数据读给X数组，地面点数据读给D数组***********//
  
   	
	float **X,**D,f;//X数组代表输入的象点坐标，D代表输入的地面点坐标
	double H;//f表示摄影中心s到像片的垂距,H表示航高


    double temp;//中间变量
    int w,j,i;

    FILE *fp;//文件名
     
	char filename[20],out[20];
	printf("输入要处理的数据所在文件名:");
	gets(filename);
    strcpy(out,filename);
    if((fp=fopen(strcat(filename,".txt"),"r"))==NULL)  //打开文件
	{
		printf("*********读文件不成功！*********\n");
        exit(0);
	} 
    else
    printf("**********读文件成功！************\n\n");
 
    w=0; 
     
    while(!feof(fp))
	{
		for(j=0;j<5;j++)
		{
			fscanf(fp,"%f ",&temp);
		}
        w++;               //先确认读入数据的行数
	}
	w=w-1;
   printf("%d\n",w);
    rewind(fp);

    //分配存储数据的空间
    X=(float**)malloc(sizeof(float)*(w));
    for(i=0;i<w;i++)
	{
        X[i]=(float*)malloc(sizeof(float)*(2));//为二维数组开辟空间
	}
         
	D=(float**)malloc(sizeof(float)*(w));
    for(i=0;i<w;i++)
	{
        D[i]=(float*)malloc(sizeof(float)*(3));//为二维数组开辟空间
	}                                              
  
    while(!feof(fp))
	{
        fscanf(fp,"%f",&f);
        for(i=0;i<w;i++)
		{
            fscanf(fp,"%f",&X[i][0]);
			fscanf(fp,"%f",&X[i][1]);
			fscanf(fp,"%f",&D[i][0]);
			fscanf(fp,"%f",&D[i][1]);
			fscanf(fp,"%f",&D[i][2]);//每一行的前两位读给 象点数组，后三位读给 地面点数组
		}
	}
    fclose(fp);
 
	//验证数据读入后准确与否
    for(i=0;i<w;i++)
	{
		printf("%.2lf     ",X[i][0]);printf("  ");
		printf("%.2lf     ",X[i][1]);printf("  ");
		printf("%.2lf    ",D[i][0]);printf("  ");
		printf("%.2lf    ",D[i][1]);printf("  ");
		printf("%.2lf    ",D[i][2]);printf("  ");
		printf("\n");
	}
               
	         //******************第二步***********//

	//**确定航高H，摄影中心在地面坐标系中的坐标小Xs，Ys，Zs，其中Zs以H为初值，Xs Ys分别以四个角上的控制
	//**点平均值代替初始值//
	double Xs,Ys,Zs;

	Xs=0;
	Ys=0;
	Zs=0;
	for(i=0;i<4;i++)
	{
		Xs+=D[i][0];      
	}

	for(i=0;i<4;i++)
	{
		Ys+=D[i][1];
	}

	Xs=Xs/4;
	Ys=Ys/4;printf("Xs的初始值是：%f    ",Xs);printf("Ys的初始值是：%f    ",Ys);

	//求Z0时我们用公式Z0=H=mf，因此我们必须求出比例尺m
	//求近似m的方法是 同时求出地面两点的距离和相片中对应点的距离 两者之比就是m
    double m,M[7];
   
	M[1]=sqrt((X[1][1]-X[2][1])*(X[1][1]-X[2][1])+(X[1][0]-X[2][0])*(X[1][0]-X[2][0]));
	M[2]=sqrt((X[1][1]-X[3][1])*(X[1][1]-X[3][1])+(X[1][0]-X[3][0])*(X[1][0]-X[3][0]));
    M[3]=sqrt((X[2][1]-X[3][1])*(X[2][1]-X[3][1])+(X[2][0]-X[3][0])*(X[2][0]-X[3][0]));
    M[4]=sqrt((X[0][1]-X[3][1])*(X[0][1]-X[3][1])+(X[0][0]-X[3][0])*(X[0][0]-X[3][0]));
	M[5]=sqrt((X[0][1]-X[2][1])*(X[0][1]-X[2][1])+(X[0][0]-X[2][0])*(X[0][0]-X[2][0]));
    M[6]=sqrt((X[0][1]-X[1][1])*(X[0][1]-X[1][1])+(X[0][0]-X[1][0])*(X[0][0]-X[1][0]));

    M[1]=sqrt((D[1][1]-D[2][1])*(D[1][1]-D[2][1])+(D[1][0]-D[2][0])*(D[1][0]-D[2][0]))/M[1];
	M[2]=sqrt((D[1][1]-D[3][1])*(D[1][1]-D[3][1])+(D[1][0]-D[3][0])*(D[1][0]-D[3][0]))/M[2];
    M[3]=sqrt((D[2][1]-D[3][1])*(D[2][1]-D[3][1])+(D[2][0]-D[3][0])*(D[2][0]-D[3][0]))/M[3];
    M[4]=sqrt((D[0][1]-D[3][1])*(D[0][1]-D[3][1])+(D[0][0]-D[3][0])*(D[0][0]-D[3][0]))/M[4];
    M[5]=sqrt((D[0][1]-D[2][1])*(D[0][1]-D[2][1])+(D[0][0]-D[2][0])*(D[0][0]-D[2][0]))/M[5];
    M[6]=sqrt((D[0][1]-D[1][1])*(D[0][1]-D[1][1])+(D[0][0]-D[1][0])*(D[0][0]-D[1][0]))/M[6];

	m=(M[1]+M[2]+M[3]+M[4]+M[5]+M[6])/6;
	H=m*f;
	Zs=m*f;printf("h=%f \n",f);
    printf("h=%f \n",H);

    double a,b,c;//三个角元素
	//误差方程的系数矩阵
	double **A;
	A=(double**)malloc(sizeof(double)*(2*w));
	for(i=0;i<2*w;i++)
	{
		A[i]=(double*)malloc(sizeof(double)*(6));//为二维数组开辟空间
	}

    //定义一个AT用来存储A的转置结果	
	double **AT;
	AT=(double**)malloc(sizeof(double)*(6));
	for(i=0;i<7;i++)
	{
		AT[i]=(double*)malloc(sizeof(double)*(2*w));//为二维数组开辟空间
	}
    //系数矩阵与其转置矩阵乘积
	double **B;
	B=(double**)malloc(sizeof(double)*(6));
	for(i=0;i<7;i++)
	{
		B[i]=(double*)malloc(sizeof(double)*(6));//为二维数组开辟空间
	}
	//B矩阵求逆矩阵是借用的一个矩阵
	double **BJ;
	BJ=(double**)malloc(sizeof(double)*(6));
	for(i=0;i<6;i++)
	{
		BJ[i]=(double*)malloc(sizeof(double)*(6));//为二维数组开辟空间
	}
	double **B1;
	B1=(double**)malloc(sizeof(double)*(6));
	for(i=0;i<6;i++)
	{
		B1[i]=(double*)malloc(sizeof(double)*(6));//为二维数组开辟空间
	}
	//用矩阵X1来表示计算的近似值
	double **X1;
		X1=(double**)malloc(sizeof(double)*(w));
		for(i=0;i<w;i++)
		{
			X1[i]=(double*)malloc(sizeof(double)*(2));//为二维数组开辟空间
		}
	//用矩阵L来表示计算的近似值与测量值的差//其用二维数组表示但只要他的一列
	double **L;
		L=(double**)malloc(sizeof(double)*(2*w));
		for(i=0;i<2*w;i++)
		{
			L[i]=(double*)malloc(sizeof(double)*(1));//为二维数组开辟空间
		}
	//我们还要用另外一个矩阵  来存AT与L的乘积 C  AT有7行2*w+1列  L有2*w+1行2列  故C应该有7行2列
	double **C;
		C=(double**)malloc(sizeof(double)*(6));
		for(i=0;i<6;i++)
		{
			C[i]=(double*)malloc(sizeof(double)*(1));//为二维数组开辟空间
		}
	//定义一个结果矩阵D 用来存储计算得到改正数
	double **d;
	   d=(double**)malloc(sizeof(double)*(6));
		for(i=0;i<6;i++)
		{
			d[i]=(double*)malloc(sizeof(double)*(1));//为二维数组开辟空间
		}
	//给d数组赋以初值以便使他可以进行下面的迭代
	for(i=0;i<6;i++)
		for(j=0;j<1;j++)
			d[i][j]=2;
		
	//**********第三步*********//
	//求取三个角元素a，b，c
	//在我们这 为简单起见 我们将 三者全初始值看为0  故我们可以求误差方程的系数矩阵A	
	for(i=0;i<w;i++)
	{
		A[2*i][0]=-1*f/H;
		A[2*i][1]=0;
		A[2*i][2]=-1*X[i][0]/H;
		A[2*i][3]=-1*f*(1+X[i][0]*X[i][0]/f/f);
		A[2*i][4]=X[i][0]*X[i][1]*(-1)/f;
		A[2*i][5]=X[i][1];

		A[2*i+1][0]=0;
		A[2*i+1][1]=-1*f/H;
		A[2*i+1][2]=-1*X[i][1]/H;
		A[2*i+1][3]=X[i][0]*X[i][1]*(-1)/f;
		A[2*i+1][4]=-1*f*(1+X[i][1]*X[i][1]/f/f);
		A[2*i+1][5]=-X[i][0];
	}
	printf("\n\n系数矩阵：\n ");
	for(i=0;i<2*w;i++)
	{
		for(j=0;j<6;j++)
		{
		printf("%f   ",A[i][j]);
		}printf("\n");
	}
	printf("\n");
	printf("\n");
	printf("\n");
	printf("\n");
	
    	
	//下面用迭代来进行各个改正数的计算	
    
	do
	{    
		//求系数矩阵转置矩阵与系数矩阵的乘积  的矩阵B
	   //现求系数矩阵的转置，调用函数zhuanzhi
	    zhuanzhi(A,AT,2*w,6);
		printf("A的转置矩阵：\n");
    for(i=0;i<6;i++)
	{
		for(j=0;j<2*w;j++)
		{
		printf("%.3f   ",AT[i][j]);
		}printf("\n");
	}
	printf("\n");
	printf("\n");
	printf("\n");
		//因为B要参加运算故先必须附以初值
        for(i=0;i<6;i++)
		{
			for(j=0;j<6;j++)
			{
		        B[i][j]=0;
			}	
	
		}
		xiangc(AT,A,B,6,w,6);
	printf("A的转置矩阵与其自身的乘积的矩阵B：\n");
	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
		{
		printf("%.3f   ",B[i][j]);
		}printf("\n");
	}
    printf("\n");
	printf("\n");
	printf("\n");	
       /***************第四步求取误差系数L；************/
       //由于在这里我们把三个角元素都很小，故可将共线方程简化成(x)=-f(X-Xs)/(Z-Zs) (y)=-f(Y-Ys)/(Z-Zs)
	   for(i=0;i<w;i++)
	   {
		   X1[i][0]=-1*f*(D[i][0]-Xs)/(D[i][2]-Zs);
		   X1[i][1]=-1*f*(D[i][1]-Ys)/(D[i][2]-Zs);
	   }
	   	
	   //计算L
	   for(i=0;i<w;i++)
	   {
		   L[2*i][0]=X[i][0]-X1[i][0];
		   L[2*i+1][0]=X[i][1]-X1[i][1];
	   }
	   
       //最后我们可以计算d
	   //调用函数相乘
	   //因为C要参加运算故先必须附以初值
      for(i=0;i<6;i++)
		   for(j=0;j<1;j++)
		   {
			  C[i][j]=0;
		   }

       xiangc(AT,L,C,6,2*w,1);
	   	printf("A的转置矩阵与L的乘积的矩阵C：\n");
	   for(i=0;i<6;i++)
	   {  for(j=0;j<1;j++)
		   {
			   printf("%f   ",C[i][j]);
		   }cout<<"\n";
	   }
	   printf("\n");
	   printf("\n");
	   printf("\n");
   //我们在前面求出了AT和A的乘积B，还有AT与L的乘积C
   //下面应该求B的逆矩阵

	inverse(B,6) ;
	printf("得到的逆矩阵B\n");
	int m,n;
	for(m=0;m<6;m++)
	{
		for(n=0;n<6;n++)
		{
			printf("%f   ",B[m][n]);
			
		}printf("\n");
	}
	printf("\n");
	   printf("\n");
	   printf("\n");

	   //然后我们应该求B与C 乘积了
	printf("B与C 乘积的到最后的d矩阵:\n");
    xiangc(B,C,d,6,6,1);
	   for(i=0;i<6;i++)
	   {   for(j=0;j<1;j++)
		   {
			   printf("%.3f   ",d[i][j]);
		   }
		   printf("\n");
	   }
	   printf("\n");
	   printf("\n");

       /*从上面函数调用我们可以得到  最后的改正数，这样我们可以将改正数加入原先的外方位元素
       以便为下次迭代  做准备 或者符合条件  作为输出  */
       Xs=Xs+d[1][0];
       Ys=Ys+d[2][0];
       Zs=Zs+d[3][0];
       a=a+d[4][0];
       b=b+d[5][0];
       c=c+d[6][0];

	}
	while(d[1][0]>0.01||d[2][0]>0.01||d[3][0]>0.01||d[4][0]>0.01||d[5][0]>0.01||d[6][0]>0.01);

	FILE* resul;

	if((resul=fopen(strcat(out,"res.txt"),"w"))==NULL)  //打开文件
	{
		cout<<"Can not open the file!"<<cout;
		return;
	}
	
	
	fprintf(resul,"Xs最后结果是：Xs=%f",Xs);
}