resection.cpp

利用四个已知控制点及其对应像点坐标解求六个外方位元素（即空间后方交会）

#include "stdio.h"
#include "math.h"
#include "stdlib.h"
//确定线元素的初始值
double average(double a[],int n)
{
	double aver,s=0;
    int i;

    for(i=0;i<n;i++)
	    s=s+a[i];
	    aver=s/4;
       return aver;
} 
//矩阵相乘
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()
{
	int i,j;
    double m=50000,f=0.15324,xo=0,yo=0;//已知内方位元素及摄影比例尺
	double avx,avy,avz;
	double az[4];
	double x[4],y[4];//计算所得像点坐标的近似值
	double a1,a2,a3,b1,b2,b3,c1,c2,c3;
	double A[8][6],AT[6][8];//误差方程式系数矩阵及其转置
	double B[6][6],C[48],Xa[6];
	double V[8],VT[8],v1[8];
	double D[1];
	double o,r;
	double l[8],Q[6],mm[6];
	double xn[4]={-0.08615,-0.05340,-0.01478,0.01046};//影像坐标
	double yn[4]={-0.06899,0.08221,-0.07663,0.06443};
	double X[4]={36589.41,37631.08,39100.97,40426.54};//地面坐标
	double Y[4]={25273.32,31324.51,24934.98,30319.81};
	double Z[4]={2195.17,728.69,2386.50,757.31};
	double p,w,k,Xs,Ys,Zs;//外方位元素
	p=w=k=0;//角元素初始值
	Xs=average(X,4);//用求均值函数计算线元素出数初始值
	Ys=average(Y,4);
	Zs=m*f;
	j=0;
	do
	{	
		if(j>60) break;
		a1=cos(p)*cos(k)-sin(p)*sin(w)*sin(k);
		a2=-cos(p)*sin(k)-sin(p)*sin(w)*cos(k);
		a3=-sin(p)*cos(w);
		b1=cos(w)*sin(k);     //求旋转矩阵的9个元素
		b2=cos(w)*cos(k);
		b3=-sin(w);
		c1=sin(p)*cos(k)+cos(p)*sin(w)*sin(k);
		c2=-sin(p)*sin(k)+cos(p)*sin(w)*cos(k);
		c3=cos(p)*cos(w);
		for(i=0;i<4;i++)
		{
			avx=a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs);
			avy=a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs);
			avz=a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs);
			x[i]=-f*avx/avz;
			y[i]=-f*avy/avz;//共线条件方程计算像点坐标的近似值
		}
	
		for(i=0;i<4;i++)
			az[i]=a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs);
		//求误差方程式的系数
		A[0][0]=(a1*f+a3*xn[0])/az[0];
		A[2][0]=(a1*f+a3*xn[1])/az[1];
		A[4][0]=(a1*f+a3*xn[2])/az[2];
		A[6][0]=(a1*f+a3*xn[3])/az[3];
		A[0][1]=(b1*f+b3*xn[0])/az[0];
		A[2][1]=(b1*f+b3*xn[1])/az[1];
		A[4][1]=(b1*f+b3*xn[2])/az[2];
		A[6][1]=(b1*f+b3*xn[3])/az[3];
		A[0][2]=(c1*f+c3*xn[0])/az[0];
		A[2][2]=(c1*f+c3*xn[1])/az[1];
		A[4][2]=(c1*f+c3*xn[2])/az[2];
		A[6][2]=(c1*f+c3*xn[3])/az[3];
		A[0][3]=yn[0]*sin(w)-(xn[0]*(xn[0]*cos(k)-yn[0]*sin(k))/f+f*cos(k))*cos(w);
		A[2][3]=yn[1]*sin(w)-(xn[1]*(xn[1]*cos(k)-yn[1]*sin(k))/f+f*cos(k))*cos(w);
		A[4][3]=yn[2]*sin(w)-(xn[2]*(xn[2]*cos(k)-yn[2]*sin(k))/f+f*cos(k))*cos(w);
		A[6][3]=yn[3]*sin(w)-(xn[3]*(xn[3]*cos(k)-yn[3]*sin(k))/f+f*cos(k))*cos(w);
		A[0][4]=-f*sin(k)-xn[0]*(xn[0]*sin(k)+yn[0]*cos(k))/f;
		A[2][4]=-f*sin(k)-xn[1]*(xn[1]*sin(k)+yn[1]*cos(k))/f; 
		A[4][4]=-f*sin(k)-xn[2]*(xn[2]*sin(k)+yn[2]*cos(k))/f;
		A[6][4]=-f*sin(k)-xn[3]*(xn[3]*sin(k)+yn[3]*cos(k))/f;
		A[0][5]=yn[0];
		A[2][5]=yn[1];
		A[4][5]=yn[2];
		A[6][5]=yn[3];
		A[1][0]=(a2*f+a3*yn[0])/az[0];
		A[3][0]=(a2*f+a3*yn[1])/az[1];
		A[5][0]=(a2*f+a3*yn[2])/az[2];
		A[7][0]=(a2*f+a3*yn[3])/az[3];
		A[1][1]=(b2*f+b3*yn[0])/az[0];
		A[3][1]=(b2*f+b3*yn[1])/az[1];
		A[5][1]=(b2*f+b3*yn[2])/az[2];
		A[7][1]=(b2*f+b3*yn[3])/az[3];
		A[1][2]=(c2*f+c3*yn[0])/az[0];
		A[3][2]=(c2*f+c3*yn[1])/az[1];
		A[5][2]=(c2*f+c3*yn[2])/az[2];
		A[7][2]=(c2*f+c3*yn[3])/az[3];
		A[1][3]=-xn[0]*sin(w)-(yn[0]*(xn[0]*cos(k)-yn[0]*sin(k))/f-f*sin(k))*cos(w);
		A[3][3]=-xn[1]*sin(w)-(yn[1]*(xn[1]*cos(k)-yn[1]*sin(k))/f-f*sin(k))*cos(w);
		A[5][3]=-xn[2]*sin(w)-(yn[2]*(xn[2]*cos(k)-yn[2]*sin(k))/f-f*sin(k))*cos(w);
		A[7][3]=-xn[3]*sin(w)-(yn[3]*(xn[3]*cos(k)-yn[3]*sin(k))/f-f*sin(k))*cos(w);
	    A[1][4]=-f*cos(k)-yn[0]*(xn[0]*sin(k)+yn[0]*cos(k))/f;
		A[3][4]=-f*cos(k)-yn[1]*(xn[1]*sin(k)+yn[1]*cos(k))/f;
		A[5][4]=-f*cos(k)-yn[2]*(xn[2]*sin(k)+yn[2]*cos(k))/f;
		A[7][4]=-f*cos(k)-yn[3]*(xn[3]*sin(k)+yn[3]*cos(k))/f;
	    A[1][5]=-xn[0];
		A[3][5]=-xn[1];
		A[5][5]=-xn[2];
		A[7][5]=-xn[3];
		for(i=0;i<8;i++)     //求常数项矩阵
		{
			if(i%2==0)  l[i]=xn[i/2]-x[i/2];
			else  l[i]=yn[(i-1)/2]-y[(i-1)/2];
		}
		transpose(&A[0][0],&AT[0][0],8,6);  //求A的转置矩阵
	    mult(&AT[0][0],&A[0][0],&B[0][0],6,8,6);
		invers_matrix(&B[0][0],6);
		mult(&B[0][0],&AT[0][0],C,6,6,8);
		mult(C,l,Xa,6,8,1);//法方程系数矩阵
		Xs+=Xa[0]; Ys+=Xa[1]; Zs+=Xa[2];    //求6个外方位元素
		p+=Xa[3]; w+=Xa[4]; k+=Xa[5];
	    mult(&A[0][0],Xa,v1,8,6,1);
		for(i=0;i<8;i++)
		{ 
			V[i]=v1[i]-l[i];
		}
		transpose(V,VT,8,1); //求V的转置矩阵
	    mult(VT,V,D,1,8,1);
		r=fabs(D[0]/2);
		o=sqrt(r);
		j=j+1;
	}
	while((fabs(Xa[0])>0.5)||(fabs(Xa[1])>0.5)||(fabs(Xa[2])>0.5)||(fabs(Xa[3])>0.000001)||(fabs(Xa[4])>0.000001)||(fabs(Xa[5])>0.000001));
	printf("R矩阵为：\n");
	printf("%.5lf",a1);printf("  %.5lf",a2);printf("  %.5lf\n",a3);
	printf("%.5lf",b1);printf("  %.5lf",b2);printf("  %.5lf\n",b3);
	printf("%.5lf",c1);printf("  %.5lf",c2);printf("  %.5lf\n",c3);
	printf("\n迭代次数是: %d\n",j-1);
	printf("\n外方位线元素为：\n");
	printf("Xs=%.2lf\n",Xs);
	printf("Ys=%.2lf\n",Ys);
	printf("Zs=%.2lf\n",Zs);
	printf("\n外方位角元素为：\n");
	printf("p=%.5lf\n",p);
	printf("w=%.5lf\n",w);
	printf("k=%.5lf\n",k);
	printf("\n单位权中误差为：\n");
	printf("%.12lf\n",o);
	printf("\n外方位元素 Xs,Ys,Zs，p,w,k,的精度分别为：\n");
	for(i=0;i<6;i++)//计算各外方位元素精度
	{
		Q[i]=fabs(B[i][i]);
		mm[i]=o*sqrt(Q[i]);
		printf("%.5lf\n",mm[i]);
	}
}

///////////程序运行结果////////////
/* R矩阵为：
0.99771  0.06753  0.00399
-0.06753  0.99772  -0.00211
-0.00412  0.00184  0.99999

迭代次数是: 3

外方位线元素为：
Xs=39795.45
Ys=27476.46
Zs=7572.69

外方位角元素为：
p=-0.00399
w=0.00211
k=-0.06758

单位权中误差为：
0.000007259424

外方位元素 Xs,Ys,Zs，p,w,k,的精度分别为：
1.10739
1.24952
0.48813
0.00018
0.00016
0.00007
Press any key to continue*/