zuoye.cpp

基于vc++的有关摄影测量中近似垂直摄影情况下空中后方交会求解的源代码

#include  <stdio.h>
#include  <math.h>
#include  <stdlib.h>
#define N 4
int m;
double xiangzb[4][2],wuzb[4][3],R[3][3];
double X0,Y0,Z0,a,b,c;  //定义初值
double a1,a2,a3,b1,b2,b3,c1,c2,c3;  //定义R矩阵的各个元素
double f=0.15324;  //定义焦距
double tempx[4],tempy[4];   //保存像点坐标的近似值
double temp1[4],temp2[4],temp3[4];
double C[6][12],D[6][8],E[6];   //存放中间结果
double A[4][2][6],L[4][2];
double X[4][6],A1[8][6],L1[8];

void bilichi()   
{
	double xiangju,wuju;
	xiangju=(xiangzb[2][0]-xiangzb[0][0])*(xiangzb[2][0]-xiangzb[0][0])+(xiangzb[2][1]-xiangzb[0][1])*(xiangzb[2][1]-xiangzb[0][1]);
	wuju=(wuzb[2][0]-wuzb[0][0])*(wuzb[2][0]-wuzb[0][0])+(wuzb[2][1]-wuzb[0][1])*(wuzb[2][1]-wuzb[0][1]);
	xiangju=sqrt(xiangju);
	wuju=sqrt(wuju);
	m=(int)(wuju/xiangju/10000+0.5); 
	m=m*10000;
}

void qiuchuzhi()    //求初值
{
	bilichi();
	for(int i=0;i<4;i++)
	{
		X0+=wuzb[i][0];
		Y0+=wuzb[i][1];
		Z0+=wuzb[i][2];
	}
	X0=X0/4;
	Y0=Y0/4;
	Z0=m*f+Z0/4;
}

void qiuR()
{
	R[0][0]=a1=cos(a)*cos(c)-sin(a)*sin(b)*sin(c);
	R[0][1]=a2=-cos(a)*sin(c)-sin(a)*sin(b)*cos(c);
	R[0][2]=a3=-sin(a)*cos(b);
	R[1][0]=b1=cos(b)*sin(c);
	R[1][1]=b2=cos(b)*cos(c);
	R[1][2]=b3=-sin(b);
	R[2][0]=c1=sin(a)*cos(c)+cos(a)*sin(b)*sin(c);
	R[2][1]=c2=-sin(a)*sin(c)+cos(a)*sin(b)*cos(c);
	R[2][2]=c3=cos(a)*cos(b);
}

void qiuxiangzbjinshizhi()
{	
	int i;
	for(i=0;i<4;i++)  //逐点计算像点坐标的近似值
	{
		temp1[i]=a1*(wuzb[i][0]-X0)+b1*(wuzb[i][1]-Y0)+c1*(wuzb[i][2]-Z0);//代入共线方程
		temp2[i]=a2*(wuzb[i][0]-X0)+b2*(wuzb[i][1]-Y0)+c2*(wuzb[i][2]-Z0);
		temp3[i]=a3*(wuzb[i][0]-X0)+b3*(wuzb[i][1]-Y0)+c3*(wuzb[i][2]-Z0);
		tempx[i]=(-1)*f*temp1[i]/temp3[i];//求得像点坐标的近似值
		tempy[i]=(-1)*f*temp2[i]/temp3[i];
	}
}

void wuchafangcheng()
{
	int i,j,k;
	double m[6]={0,0,0,0,0,0};
	double n[6]={0,0,0,0,0,0};
	for(i=0;i<N;i++)
	{	
		A[i][0][0]=(a1*f+a3*tempx[i])/temp3[i];
		A[i][0][1]=(b1*f+b3*tempx[i])/temp3[i];
		A[i][0][2]=(c1*f+c3*tempx[i])/temp3[i];
		A[i][0][3]=tempy[i]*sin(b)-(tempx[i]/f*(tempx[i]*cos(c)-tempy[i]*sin(c))+f*cos(c))*cos(b);
		A[i][0][4]=(-1)*f*sin(c)-tempx[i]/f*(tempx[i]*sin(c)+tempy[i]*cos(c));
		A[i][0][5]=tempy[i];
		A[i][1][0]=(a2*f+a3*tempy[i])/temp3[i];
		A[i][1][1]=(b2*f+b3*tempy[i])/temp3[i];
		A[i][1][2]=(c2*f+c3*tempy[i])/temp3[i];
		A[i][1][3]=-tempx[i]*sin(b)-(tempy[i]/f*(tempx[i]*cos(c)-tempy[i]*sin(c))-f*sin(c))*cos(b);
		A[i][1][4]=(-1)*f*cos(c)-tempy[i]/f*(tempx[i]*sin(c)+tempy[i]*cos(c));
		A[i][1][5]=(-1)*tempx[i];
		L[i][0]=xiangzb[i][0]-tempx[i];
		L[i][1]=xiangzb[i][1]-tempy[i];
	}
	
	for(i=0;i<4;i++)  //将四个控制点的系数矩阵组合成一个矩阵
		for(j=0;j<6;j++)
		{	A1[2*i][j]=A[i][0][j];
		A1[2*i+1][j]=A[i][1][j];
		} 
		
		for(i=0;i<4;i++)  //将L矩阵也类似转化为8*1矩阵
		{
			L1[2*i]=L[i][0];
			L1[2*i+1]=L[i][1];
		}
		
		for(i=0;i<6;i++)     
			for(j=0;j<6;j++)
			{
				C[i][j]=0;
				for(k=0;k<8;k++)
					C[i][j]+=A1[k][i]*A1[k][j];
			}
			
			for(i=0;i<6;i++){C[0][6+i]=0;}C[0][6]=1;
			for(i=0;i<6;i++){C[1][6+i]=0;}C[1][7]=1;
			for(i=0;i<6;i++){C[2][6+i]=0;}C[2][8]=1;
			for(i=0;i<6;i++){C[3][6+i]=0;}C[3][9]=1;
			for(i=0;i<6;i++){C[4][6+i]=0;}C[4][10]=1;
			for(i=0;i<6;i++){C[5][6+i]=0;}C[5][11]=1;
			for(i=0;i<6;i++)
			{
				n[i]=C[i][i];
				for(j=i;j<12;j++)
				{
					if(n[i]==0)
					{
						printf("不存在逆矩阵\n");
						exit(0);
					}
					C[i][j]=C[i][j]/n[i];
				}
				for(j=0;j<6;j++)
				{
					if(i==j)continue;
					m[j]=C[j][i];
					for(k=0;k<12;k++)
						C[j][k]=C[j][k]-C[i][k]*m[j];
				}
			}          //C矩阵的后面部分就是逆矩阵了					
			for(i=0;i<6;i++)
				for(j=0;j<8;j++)
				{		
					D[i][j]=0;
					for(k=0;k<6;k++)
						D[i][j]+=C[i][k+6]*A1[j][k];
				}
				
				for(i=0;i<6;i++)
					for(j=0;j<1;j++)
					{	
						E[i]=0;	 
						for(k=0;k<8;k++)
							E[i]+=D[i][k]*L1[k];
					}
					
					X0=X0+E[0];
					Y0=Y0+E[1];
					Z0=Z0+E[2];
					a=a+E[3];
					b=b+E[4];
					c=c+E[5];
}

void main()
{	
	int i,j,count=0;
	double as,bs,cs;
	xiangzb[0][0]=-0.08615;xiangzb[0][1]=-0.06899;
	xiangzb[1][0]=-0.05340;xiangzb[1][1]=0.08221;
	xiangzb[2][0]=-0.01478;xiangzb[2][1]=-0.07663;
	xiangzb[3][0]=0.01046;xiangzb[3][1]=0.06443;
	wuzb[0][0]=36589.41;wuzb[0][1]=25273.32;wuzb[0][2]=2195.17;
	wuzb[1][0]=37631.08;wuzb[1][1]=31324.51;wuzb[1][2]=728.69;
	wuzb[2][0]=39100.97;wuzb[2][1]=24934.98;wuzb[2][2]=2386.50;
	wuzb[3][0]=40426.54;wuzb[3][1]=30319.81;wuzb[3][2]=757.31;
	qiuchuzhi();
	do
	{
        qiuR();
		qiuxiangzbjinshizhi();
		wuchafangcheng();
		count++;
		as=fabs(E[3]);
		bs=fabs(E[4]);
		cs=fabs(E[5]);
	}while( as>0.0000001 || bs>0.0000001 || cs>0.0000001);
	printf("Xs=%lf\nYs=%lf\nZs=%lf\n",X0,Y0,Z0);
	printf("R矩阵为:\n");
	for(i=0;i<3;i++)
	{	
		for(j=0;j<3;j++)
			printf("%f   ",R[i][j]);
		printf("\n");
	}
}