back111222.cpp

单张像片空间后方交会程序

#include "iostream.h"
#include "math.h"

#define f 0.15324
#define x0 0
#define y0 0

void plus(double *a,double *b, double*c,int m1,int n1, int m2,int n2)
{
	int i,j,k;
	
	if (n1!=m2)
	{
		cout<<"can not plus!"<<endl;
		return ;
	}

	for(i=0;i<m1;i++)
		for(j=0;j<n2;j++)
			*(c+j+i*n2)=0;

	for(i=0;i<m1;i++)
		for(j=0;j<n2;j++)
			for(k=0;k<n1;k++)
				*(c+j+i*n2)+=*(a+k+i*n1)*(*(b+j+k*n2));


}
/******************************
   矩阵转置
   矩阵a 为m*n的矩阵，
   返回矩阵a_t n*m
******************************/

void transpose(double *a, double*a_t,int m,int n)
{
	int i,j;
	for(i=0;i<m;i++)
		for(j=0;j<n;j++)
		{
			*(a_t+i+j*m)=*(a+j+i*n);
		}

	return;

}
/******************************
   矩阵求逆
   矩阵a 为m*n的矩阵，
   返回矩阵e 为a的逆矩阵
   矩阵a可逆的充要条件是a是方阵且a为非奇异矩阵
******************************/


void inverse(double *a,double*e,int m,int n)
{
	int i,j;
	double temp;
	double temp2;
	
	/*若该矩阵不是方阵，则不能求逆*/
	if(m!=n)
	{
		cout<<"a不是方阵，不能求逆！"<<endl;
		return;
	}

/*判断该矩阵行列式的值，若值为0，则不能求逆 注释掉的部分有错误*/
/*	double value_a=1;
	
	for(i=0;i<m;i++)
	{
		temp=*(a+i+i*m);
		for(j=0;j<m;j++)
		{
			*(a+j+i*m)/=temp;
		}
	
		for(int k=i+1;k<m;k++)
		{
			temp2=*(a+i+k*m);
			for(j=0;j<n;j++)
			{	
    			*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
			}
		}

		value_a*=temp;
		
	}
	if (value_a==0)
	{
		cout<<"奇异矩阵不能求逆！"<<endl;
		return false;
	}
*/		
	
	for(i=0;i<m;i++)
		for(j=0;j<n;j++)
		{
			if(i==j) *(e+j+i*m)=1;
			else     *(e+j+i*m)=0;
		}
		
	for(i=0;i<m;i++)
	{
		temp=*(a+i+i*m);
	    for(j=0;j<m;j++)
		{
			*(a+j+i*m)/=temp;
	    	*(e+j+i*m)/=temp;
		}

		for(int k=i+1;k<m;k++)
		{
			temp2=*(a+i+k*m);
			for(j=0;j<n;j++)
			{	
				*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
         		*(e+j+k*m)=*(e+j+k*m)-temp2*(*(e+j+i*m));
			}
		}
	 }

	for(i=m-1;i>0;i--)
	{
		for(j=0;j<m;j++)
		for(int k=i-1;k>=0;k--)
		{
			temp2=*(a+i+k*m);
			for(j=0;j<n;j++)
			{
				*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
     			*(e+j+k*m)=*(e+j+k*m)-temp2*(*(e+j+i*m));
			}
		}
	}
	
}

double Ab(double a)
{
	double b;
	if(a>=0)
		b=a;
	else b=-a;
	return b;
}

/******************************
主函数
******************************/

void main()
{
	int i,j;                         //循环变量
	int num=0;                        //迭代次数
	double scale;                        //比例尺分母
	double Zs,Xs,Ys,fai,w,k;         //外方位元素
	double temp=0;                   //临时变量
	double x[4]={-0.08615,-0.05340,-0.01478,0.01046};   //相片坐标观测值
	double y[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 a1,a2,a3,b1,b2,b3,c1,c2,c3;                  //矩阵R系数
	double XX[4],YY[4],ZZ[4];                                    //共线方程分子分母
	double x_x[4],y_y[4];                      //像点坐标的近似值
	double dXs,dYs,dZs,dfai,dw,dk;     //外方位元素改正数
	double A[8][6],L[8][1];          //总误差方程式
	/*误差方程式计算中间变量*/
	double AT[6][8];         //A的转置
	double ATA[6][6];        //AT*A
	double ATA_1[6][6];      //ATA_1
	double ATL[6][8];        //AT*A_1*AT  
	double DX[6][1];
	/*精度*/
	double v=0,m0,m[6];
 
	//计算得到近似比例尺
	double s[3];         //计算出三段距离，得出比例尺，取平均  
	for(i=0;i<3;i++)
	{
		s[i]=sqrt((Y[i]-Y[i+1])*(Y[i]-Y[i+1])+(X[i]-X[i+1])*(X[i]-X[i+1]))/sqrt((y[i]-y[i+1])*(y[i]-y[i+1])+(x[i]-x[i+1])*(x[i]-x[i+1]));
		temp+=s[i];
	}
	scale=temp/3;
	scale=40000;
	
    //给定外方位元素的初值
	fai=0;
	w=0;
	k=0;
	Zs=scale*f+(Z[0]+Z[1]+Z[2]+Z[3])*0.25;
	Xs=(X[0]+X[1]+X[2]+X[3])*0.25;
	Ys=(Y[0]+Y[1]+Y[2]+Y[3])*0.25;


	//进行迭代
	while (Ab(dfai)>=0.0017 || Ab(dw)>=0.0017 || Ab(dk)>=0.0017 || num==0)
	{
		num+=1;
		//计算旋转矩阵R
	    a1=cos(fai)*cos(k)-sin(fai)*sin(w)*sin(k);
	    a2=-cos(fai)*sin(k)-sin(fai)*sin(w)*cos(k);
	    a3=-sin(fai)*cos(w);
        b1=cos(w)*sin(k);
    	b2=cos(w)*cos(k);
    	b3=-sin(w);
    	c1=sin(fai)*cos(k)+cos(fai)*sin(w)*sin(k);
    	c2=-sin(fai)*sin(k)+cos(fai)*sin(w)*cos(k);
    	c3=cos(fai)*cos(w);

    	//通过共线方程计算像点坐标的近似值
    	for(i=0;i<4;i++)
		{
    		XX[i]=a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs);
    		YY[i]=a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs);
        	ZZ[i]=a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs);	
	    	x_x[i]=-f*(a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs))/(a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs))+x0;
	    	y_y[i]=-f*(a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs))/(a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs))+y0;
		}
		
    	//计算误差方程式的系数和常数项，组成误差方程式
    	for(i=0;i<4;i++)
		{
/*	    	A[2*i][0]=(a1*f+a3*x_x[i])/ZZ[i];
	    	A[2*i][1]=(b1*f+b3*x_x[i])/ZZ[i];
	    	A[2*i][2]=(c1*f+c3*x_x[i])/ZZ[i];
		    A[2*i+1][0]=(a2*f+a3*y_y[i])/ZZ[i];
		    A[2*i+1][1]=(b2*f+b3*y_y[i])/ZZ[i];
		    A[2*i+1][2]=(c2*f+c3*y_y[i])/ZZ[i];
		    A[2*i][3]=y_y[i]*sin(w)-(x_x[i]*(x_x[i]*cos(k)-y_y[i]*sin(k))/f+f*cos(k))*cos(w);
		    A[2*i][4]=-f*sin(k)-x_x[i]*(x_x[i]*sin(k)+y_y[i]*cos(k))/f;
		    A[2*i][5]=y_y[i];
		    A[2*i+1][3]=-x_x[i]*sin(w)-((x_x[i]*cos(k)-y_y[i]*sin(k))*y_y[i]/f-f*sin(k))*cos(w);
	    	A[2*i+1][4]=-f*cos(k)-(x_x[i]*sin(k)+y_y[i]*cos(k))*y_y[i]/f;
		    A[2*i+1][5]=-x_x[i];
		    L[2*i][0]=x[i]-x_x[i];
		    L[2*i+1][0]=y[i]-y_y[i];
*/	
			A[2*i][0]=(a1*f+a3*(x[i]-x0))/ZZ[i];
	    	A[2*i][1]=(b1*f+b3*(x[i]-x0))/ZZ[i];
	    	A[2*i][2]=(c1*f+c3*(x[i]-x0))/ZZ[i];
		    A[2*i+1][0]=(a2*f+a3*(y[i]-y0))/ZZ[i];
		    A[2*i+1][1]=(b2*f+b3*(y[i]-y0))/ZZ[i];
		    A[2*i+1][2]=(c2*f+c3*(y[i]-y0))/ZZ[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;
		    A[2*i+1][3]=-(x[i]-x0)*sin(w)-(((x[i]-x0)*cos(k)-(y[i]-y0)*sin(k))*(y[i]-y0)/f-f*sin(k))*cos(w);
	    	A[2*i+1][4]=-f*cos(k)-((x[i]-x0)*sin(k)+y[i]*cos(k))*(y[i]-y0)/f;
		    A[2*i+1][5]=-(x[i]-x0);
		    L[2*i][0]=x[i]-x_x[i];
		    L[2*i+1][0]=y[i]-y_y[i];

		}		

		for(i=0;i<4;i++)
			cout<<x_x[i]<<endl;
		cout<<endl;
		for(i=0;i<4;i++)
			cout<<y_y[i]<<endl;
		cout<<endl;
		for(i=0;i<8;i++)
			cout<<L[i][0]<<endl;
		cout<<endl;
    	//计算法方程的系数矩阵
		transpose(*A,*AT,8,6);
		plus(*AT,*A,*ATA,6,8,8,6);
		inverse(*ATA,*ATA_1,6,6);
		plus(*ATA_1,*AT,*ATL,6,6,6,8);
		plus(*ATL,*L,*DX,6,8,8,1);


		dXs=DX[0][0];
		dYs=DX[1][0];
		dZs=DX[2][0];
		dfai=DX[3][0];
		dw=DX[4][0];
		dk=DX[5][0];
		
    	Xs+=dXs;
    	Ys+=dYs;
    	Zs+=dZs;
    	fai+=dfai;
    	w+=dw;
    	k+=dk;

	
    	if(num>=100)      //迭代次数超过100次，退出
    		break;
	}
	cout<<"外方位元素为："<<endl;
	cout<<"Xs:"<<Xs<<"  Ys:"<<Ys<<"  Zs:"<<Zs<<"  fai:"<<fai<<"  w:"<<w<<"  k:"<<k<<endl<<endl;
	
	double R[3][3]={a1,a2,a3,b1,b2,b3,c1,c2,c3};
	cout<<"旋转矩阵为："<<endl;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
			cout<<R[i][j]<<"  "; 
		cout<<endl;
	}
	
	for(i=0;i<8;i++)
		v+=L[i][0]*L[i][0];
	//cout<<v<<endl;
	m0=sqrt(v/(2*4-6));
	cout<<"dXs,dYs,dZs,dfai,dw,dk的精度分别是："<<endl;
	for(i=0;i<6;i++)
	{
		m[i]=sqrt(ATA_1[i][i])*m0;
	    cout<<m[i]<<endl;
	}
	
}