后方交会.cpp

基于摄影测量学中四个控制点的单片后方交会

#include "stdio.h"
#include "stdlib.h"
#include "iostream.h"
#include <math.h>
#define N 4 //观测点数为4



void mult(double *m1,double *m2,double *result,int i_1,int j_12,int j_2)//矩阵相乘
{ 
	
	int i,j,k;
	for(i=0;i<i_1;i++)
		for(j=0;j<j_2;j++)
		{
			result[i*j_2+j]=0.0;
			for(k=0;k<j_12;k++)
				result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];
		}
		return;
}


 
int invers_matrix(double *m1,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){
		printf("out of memory!\n");
		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(m1[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);
				printf("invers is not availble!\n");
				return(0);
			}
			if(is[k]!=k)
				for(j=0;j<n;j++){
					u=k*n+j; v=is[k]*n+j;
					temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
				}
				if(js[k]!=k)
					for(i=0;i<n;i++){
						u=i*n+k; v=i*n+js[k];
						temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
					}
					l=k*n+k;
					m1[l]=1.0/m1[l];
					for(j=0;j<n;j++)
						if(j!=k){
							u=k*n+j;
							m1[u]*=m1[l];
						}
						for(i=0;i<n;i++)
							if(i!=k)
								for(j=0;j<n;j++)
									if(j!=k){
										u=i*n+j;
										m1[u]-=m1[i*n+k]*m1[k*n+j];
									}
									for(i=0;i<n;i++)
										if(i!=k){
											u=i*n+k;
											m1[u]*=-m1[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=m1[u]; m1[u]=m1[v]; m1[v]=temp;
			}
			if(is[k]!=k)
				for(i=0;i<n;i++){
					u=i*n+k; v=i*n+is[k];
					temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
				}
	}
	free(is); free(js);
	return(1);
}


 
void transpose(double *m1,double *m2,int m,int n)  //矩阵转置          
{
	int i,j;                                        
	for(i=0;i<m;i++)                                
		for(j=0;j<n;j++)                        
			m2[j*m+i]=m1[i*n+j];            
		return;                                 		
}


void main()
{
	double Xs,Ys,Zs,q,w,k;//像片外方位元素
	double a[3],b[3],c[3];//外方位角元素组成的9个方向余弦
	double x0,y0,f;//相片的内方位元素
	double x[N],y[N];//量测像点坐标
	double X[N],Y[N],Z[N];//像点坐标所对应的物点坐标
	double x1[N],y1[N];//像点坐标计算值
	double m;//摄影比例尺分母
	double L[2*N];
	double XX[6];//外方位元素改正数
	double A[2*N][6];//外方位元素改正数系数
	double X0[N],Y0[N],Z0[N],At[6][2*N],result1[6][6],result2[6][1];
	int i,n=0;
	double sum=0,m0;
	/*---------------输入点地面坐标---------------------*/
	for(i=0;i<N;i++)
	{
		printf("请输入第%d个点的地面坐标：",i+1);
		scanf("%lf%lf%lf",&X[i],&Y[i],&Z[i]);
	}
	/*---------------输入点像片坐标---------------------*/
	for(i=0;i<N;i++)
	{
		printf("请输入第%d个点的像片坐标：",i+1);
		scanf("%lf%lf",&x[i],&y[i]);
	}
	cout<<endl;
	/*-----------------设定外方位元素初始值--------------*/
	x0=0;y0=0;f=153.24;m=50000;
	Xs=0;Ys=0;Zs=f*m/1000;
	q=0;w=0;k=0;
	XX[3]=1;
	/*------------------迭代计算--------------------------*/
	while((XX[3]>0.00001 || XX[4]>0.00001 || XX[5]>0.00001)&&n<100)
	{
		/*----------------旋转矩阵R-----------------------*/
		a[0]=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
		a[1]=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
		a[2]=-sin(q)*cos(w);
		b[0]=cos(w)*sin(k);
		b[1]=cos(w)*cos(k);
		b[2]=-sin(w);
		c[0]=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
		c[1]=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
		c[2]=cos(q)*cos(w);
		
		/*-----------------像点坐标计算值------------------*/
		for(i=0;i<N;i++)
		{
			X0[i]=a[0]*(X[i]-Xs)+b[0]*(Y[i]-Ys)+c[0]*(Z[i]-Zs);
			Y0[i]=a[1]*(X[i]-Xs)+b[1]*(Y[i]-Ys)+c[1]*(Z[i]-Zs);
		    Z0[i]=a[2]*(X[i]-Xs)+b[2]*(Y[i]-Ys)+c[2]*(Z[i]-Zs);
			x1[i]=x0-f*X0[i]/Z0[i];
			y1[i]=y0-f*Y0[i]/Z0[i];
		}
		/*-------------误差方程中各偏导数的值--------------*/
		for(i=0;i<N;i++)
		{
			A[2*i][0]=((a[0]*f+a[2]*(x[i]-x0)))/Z0[i];
			A[2*i][1]=((b[0]*f+b[2]*(x[i]-x0)))/Z0[i];
			A[2*i][2]=((c[0]*f+c[2]*(x[i]-x0)))/Z0[i];
			A[2*i][3]=(y[i]-y0)*sin(w)-((x[i]-x0)*((x[i]-x0)*cos(k)-y[i]*sin(k))/f+f*cos(k))
				    *cos(w);
			A[2*i][4]=-f*sin(k)-(x[i]-x0)*((x[i]-x0)*sin(k)+(y[i]-y0)*cos(k))/f;
			A[2*i][5]=y[i]-y0;
		    L[2*i]=x[i]-x1[i];
	
			A[1+2*i][0]=((a[1]*f+a[2]*(y[i]-y0)))/Z0[i];
			A[1+2*i][1]=((b[1]*f+b[2]*(y[i]-y0)))/Z0[i];
			A[1+2*i][2]=((c[1]*f+c[2]*(y[i]-y0)))/Z0[i];
			A[1+2*i][3]=-(x[i]-x0)*sin(w)-((y[i]-y0)*((x[i]-x0)*cos(k)-(y[i]-y0)*sin(k))/f-f*sin(k))
				    *cos(w);
			A[1+2*i][4]=-f*cos(k)-(y[i]-y0)*((x[i]-x0)*sin(k)+(y[i]-y0)*cos(k))/f;
			A[1+2*i][5]=-x[i]+x0;
			L[1+2*i]=y[i]-y1[i];
		}
		/*-------------------解法方程--------------------*/
		transpose(&A[0][0],&At[0][0],2*N,6);

		mult(&At[0][0],&A[0][0],&result1[0][0],6,2*N,6);

		invers_matrix(&result1[0][0],6);

		mult(&At[0][0],L,&result2[0][0],6,2*N,1);

		mult(&result1[0][0],&result2[0][0],&XX[0],6,6,1);

		Xs+=XX[0];
		Ys+=XX[1];
		Zs+=XX[2];

		q+=XX[3];
		w+=XX[4];
		k+=XX[5];
		n++;
	}
	/*----------------旋转矩阵R-----------------------*/
		a[0]=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);
		a[1]=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);
		a[2]=-sin(q)*cos(w);
		b[0]=cos(w)*sin(k);
		b[1]=cos(w)*cos(k);
		b[2]=-sin(w);
		c[0]=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);
		c[1]=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);
		c[2]=cos(q)*cos(w);
	cout<<"迭代次数为:"<<n<<endl;
	printf("\n像片外方位元素的解\n");
	cout<<"航向顷角q:"<<q<<endl;
	cout<<"旁向倾角w:"<<w<<endl;
	cout<<"像片旋角k:"<<k<<endl;
	printf("Xs=%lf\t\tYs=%lf\t\tZs=%lf\n",Xs,Ys,Zs);
	cout<<endl;
	printf("旋转矩阵R:\n");
	for(i=0;i<3;i++)
		printf("%lf\t",a[i]);
	printf("\n");
	for(i=0;i<3;i++)
		printf("%lf\t",b[i]);
	printf("\n");
	for(i=0;i<3;i++)
		printf("%lf\t",c[i]);
	printf("\n");
	/*-------------------计算单位权中误差---------------*/
	for(i=0;i<2*N;i++)
		sum+=L[i]*L[i];
	m0=sqrt(sum/(2*N-6));

	
	cout<<"单位权中误差m0="<<m0<<endl;
	cout<<"测量精度:"<<endl;
	cout<<"Xs的精度为:"<<m0*sqrt(result1[0][0])<<endl;
	cout<<"Ys的精度为:"<<m0*sqrt(result1[1][1])<<endl;
	cout<<"Zs的精度为:"<<m0*sqrt(result1[2][2])<<endl;
	cout<<"q的精度为:"<<m0*sqrt(result1[3][3])<<endl;
	cout<<"w的精度为:"<<m0*sqrt(result1[4][4])<<endl;
	cout<<"k的精度为:"<<m0*sqrt(result1[5][5])<<endl;

}