www.gusucode.com > VC++空间后方交会的3种形式实例源码-源码程序 > VC++空间后方交会的3种形式实例源码-源码程序/code/Resection/SpaceResection.cpp

    // SpaceResection.cpp
// 单像空间后方交会解算航空相片外方位元素
// 武汉大学.遥感信息工程学院.卢昊
//Download by http://www.NewXing.com
#include <fstream.h>
#include <iostream.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <stdio.h>
#include "MatrixOperation.h"

#define N 4		//控制点数
#define  PI 3.1415926
#define MAXITERATION 8	//最大允许迭代次数

struct EOEO		// elements of exterior orientation
{
	double Xs;
	double Ys;
	double Zs;
	double omega;
	double phi;
	double kappa;
};

struct SourceData		//保存原始数据
{
	double x;
	double y;
	double X;
	double Y;
	double Z;
};

//函数声明
void InitInterface();
void InitData(double PhotographicScale, double focus, char* filename);
bool CheckPrecision(double* deta);
void Iterator(EOEO *eoeo, SourceData* sd, double PhotographicScale, double focus);
void OutputResult(EOEO *eoeo, double* RotationMatrix, double* Pricision, double m0);


//////////////////////////////////////////////////////////////////////////
// 函数功能：初始化坐标数据
// 参数说明：sd：保存原始数据的结构体数组，PhotographicScale：摄影比例尺，focus：摄影机主距
//			 filename：保存坐标数据的文件名
//////////////////////////////////////////////////////////////////////////
void InitData(SourceData* sd, char* filename)
{
	//打开文件以备读取数据
	cout <<"开始读取数据..."<<endl;
	ifstream datafile(filename,ios::in | ios::nocreate);
	if( !datafile)
	{
		cout<<"打开文件失败, 文件可能不存在"<<endl;
		system("pause");
		_exit(1);
	}

	memset(sd, 0, N*sizeof(SourceData));
	
	int i;
	for (i = 0; i != N; i ++)
	{
		datafile >> sd[i].x >> sd[i].y >> sd[i].X >> sd[i].Y >> sd[i].Z;
	}
	datafile.close();
/*
	SourceData sd[N];
	sd[0].x = -86.15 ; sd[0].y = -68.99; sd[0].X = 36589.41; sd[0].Y = 25273.32; sd[0].Z = 2195.17;
	sd[1].x = -53.40 ; sd[1].y = 82.21; sd[1].X = 37631.08; sd[1].Y = 31324.51; sd[1].Z = 728.69;
	sd[2].x = -14.78 ; sd[2].y = -76.63; sd[2].X = 39100.97; sd[2].Y = 24934.98; sd[2].Z = 2386.50;
	sd[3].x = 10.46 ; sd[3].y = 64.43; sd[3].X = 40426.54; sd[3].Y = 30319.81; sd[3].Z = 757.31;
*/
	//外方位元素大小初始化
/*	eoeo->omega = 0;
	eoeo->phi = 0;
	eoeo->kappa = 0;
	eoeo->Zs = PhotographicScale * focus /1000;

	for (i = 0; i != N; i ++)
	{
		sd[i].x /= 1000.0;
		sd[i].y /= 1000.0;
		eoeo->Xs += sd[i].X;
		eoeo->Ys += sd[i].Y;
	}
	eoeo->Xs /= N;
	eoeo->Ys /= N;*/
	cout <<"读取数据完毕..."<<endl;
}


//////////////////////////////////////////////////////////////////////////
// 函数功能：检查改正数是否已达精度要求
// 参数说明：deta：保存改正数的数组
//////////////////////////////////////////////////////////////////////////
bool CheckPrecision(double* deta)
{
	//2.9088820866572159615394846141477e-5 即 0.1′的弧度表示
	bool ret;
	ret = (fabs(deta[0])<0.000001 && fabs(deta[1])<0.000001	&& fabs(deta[2])<0.000001 && \
		fabs(deta[3])<2.90888208656e-5 && fabs(deta[4])<2.90888208656e-5 && \
		fabs(deta[5])<2.90888208656e-5);
	return ret;
}


//////////////////////////////////////////////////////////////////////////
// 函数功能：迭代器，计算的主体部分
// 参数说明：sd：保存原始数据的结构体数组，PhotographicScale：摄影比例尺，focus：摄影机主距
//////////////////////////////////////////////////////////////////////////
void Iterator(SourceData sd[N], double PhotographicScale, double Focus)
{
	double phi, omega, kappa, Xs, Ys, Zs;
	phi = omega = kappa =0.0;
	Xs = Ys = Zs = 0.0;

	for (int k=0;k<N;k++)
	{
		sd[k].x /= 1000.0;
		sd[k].y /= 1000.0;
		Xs += sd[k].X;
		Ys += sd[k].Y;
	}
	Xs /= N;
	Ys /= N;
	
	double f = Focus / 1000.0;
	double m = PhotographicScale;
	Zs =m*f;
	
	cout<<endl<<"六元素初始化值:"<<endl;
	cout <<"Xs: "<<Xs<<'\t'<<"Ys: "<<Ys<<'\t'<<"Zs: "<<Zs<<endl;
	cout <<"phi: "<<phi<<"\t\t"<<"omega: "<<omega<<"\t"<<"kappa: "<<kappa<<endl<<endl;

	//声明并初始化六元素改正数矩阵
	double deta[6] = {1,1,1,1,1,1};

	double x0(0);
	double y0(0);//内方位元素
	double X0[N] = {0.0};
	double Y0[N] = {0.0};
	double Z0[N] = {0.0};

	//声明旋转矩阵
	double R[9];

	double A[2*6] = {0.0};
	double AT[6*2] = {0.0};

	double Buf1[36] = {0.0};
	double Buf2[36] = {0.0};//ATA累加

	double Buf3[6] = {0.0};
	double Buf4[6] = {0.0};//ATL累加
	
	double Buf5[8*6] = {0.0};//存储8×6的A矩阵，没办法
	double Buf6[8*1] = {0.0};//存储8×1的L矩阵，同上
	double V[8*1] = {0.0};

	double ATA[36] = {0.0};
	double ATL[6] = {0.0};
	double L[2] = {0.0};


	int iCount = 0;//迭代次数
	cout << "开始迭代计算..."<<endl;
	while(!CheckPrecision(deta))
	{
		cout <<endl<<"第 "<< ++iCount<<"次迭代:"<<endl;

		if (iCount == MAXITERATION)
		{
			cout << ">>>迭代次数超限，可能不收敛"<<endl;
			break;
		}

		//每次迭代之前必须清空两个保存累加值的矩阵ATA与ATL
		for (int i = 0; i != 36; i ++)
		{
			ATA[i] = 0.0;
			if (i < 6)
			{
				ATL[i] = 0.0;
			}
		}

		//计算旋转矩阵
		R[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
		R[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
		R[2] = -sin(phi)*cos(omega);
		R[3] = cos(omega)*sin(kappa);
		R[4] = cos(omega)*cos(kappa);
		R[5] = -sin(omega);
		R[6] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
		R[7] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
		R[8] = cos(phi)*cos(omega);

		for (i = 0; i != N; i ++)
		{
			Z0[i] = R[2]*(sd[i].X-Xs) + R[5]*(sd[i].Y-Ys) + R[8]*(sd[i].Z-Zs);
			X0[i] = x0 - f*(R[0]*(sd[i].X-Xs) + R[3]*(sd[i].Y-Ys) + R[6]*(sd[i].Z-Zs)) / Z0[i];
			Y0[i] = y0 - f*(R[1]*(sd[i].X-Xs) + R[4]*(sd[i].Y-Ys) + R[7]*(sd[i].Z-Zs)) / Z0[i];

			A[0] = ((R[0]*f + R[2]*(sd[i].x-x0)) / Z0[i]);
			A[1] = ((R[3]*f + R[5]*(sd[i].x-x0)) / Z0[i]);
			A[2] = ((R[6]*f + R[8]*(sd[i].x-x0)) / Z0[i]);
			A[3] = ((sd[i].y-y0)*sin(omega) - ((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\
				- (sd[i].y-y0)*sin(kappa))/f + f*cos(kappa))*cos(omega));
			A[4] = (-f*sin(kappa) - (sd[i].x-x0)*((sd[i].x-x0)*sin(kappa) + (sd[i].y-y0)*cos(kappa))/f);
			A[5] = (sd[i].y - y0);
			A[6] = ((R[1]*f + R[2]*(sd[i].y-y0)) /Z0[i]);
			A[7] = ((R[4]*f + R[5]*(sd[i].y-y0)) /Z0[i]);
			A[8] = ((R[7]*f + R[8]*(sd[i].y-y0)) /Z0[i]);
			A[9] = (-(sd[i].x-x0)*sin(omega) - ((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\
				- (sd[i].y-y0)*sin(kappa))/f - f*sin(kappa))*cos(omega));
			A[10] = (-f*cos(kappa) - (sd[i].y-y0)*((sd[i].x-x0)*sin(kappa) + (sd[i].y-y0)*cos(kappa))/f);
			A[11] = (-(sd[i].x-x0));

			//该循环保存A矩阵，最后评定精度用
			for (int l=0;l<12;l++)
			{
				Buf5[12*i+l] = A[l];
			}
			
			//所谓的逐步法化，即要在循环内部就将ATA计算出来累加，下面的L矩阵类似
			MatrixTranspose(A,AT,2,6);
			MatrixMultiply(AT,A,Buf1,6,2,6);
			MatrixCopy(ATA, Buf2, 36);
			MatrixAdd(Buf1,Buf2,ATA,36);	// 为逐步法化后的ATA矩阵累加

			L[0] = (sd[i].x - X0[i]);
			L[1] = (sd[i].y - Y0[i]);

			//保存L矩阵，最后评定精度用
			for (l=0;l<2;l++)
			{
				Buf6[2*i+l] = L[l];
			}

			MatrixMultiply(AT,L,Buf3,6,2,1);
			MatrixCopy(ATL, Buf4, 6);
			MatrixAdd(Buf3,Buf4,ATL,6);			
		}//for

		//“逐步法化”的另一处不同，出循环即可直接计算ATA逆乘ATL
		MatrixInversion(ATA,6);
		MatrixMultiply(ATA,ATL,deta,6,6,1);

		//deta即为改正数
		Xs += deta[0];
		Ys += deta[1];
		Zs += deta[2];
		phi += deta[3];
		omega += deta[4];
		kappa += deta[5];

		cout<<"改正数值:"<<endl;
		for (i=0; i != 6; i ++)
		{
			cout <<"deta["<<i<<"]: "<<deta[i]<<endl;
		}
		cout << endl<<"六元素值:"<<endl;
		cout <<"Xs: "<<Xs<<endl<<"Ys: "<<Ys<<endl<<"Zs: "<<Zs<<endl;
		cout <<"kappa: "<<kappa<<endl<<"omega: "<<omega<<endl<<"phi: "<<phi<<endl<<endl;
	}//while

	EOEO eoeo;
	eoeo.kappa = kappa;
	eoeo.omega = omega;
	eoeo.phi = phi;
	eoeo.Xs = Xs;
	eoeo.Ys = Ys;
	eoeo.Zs = Zs;

	cout << ">>>正常退出迭代"<<endl<<endl<<endl;

	//精度评定
	double Q[6] = {0.0};
	for (int h=0;h<6;h++)
	{
		Q[h] = ATA[h*6+h];
	}

	MatrixMultiply(Buf5,deta,V,8,6,1);//V=Ax-L
	MatrixMinus(V,Buf6,V,8);

	double m0(0);//单位权中误差
	double VSum(0);//[vv]，即平方和

	int i;
	for (i=0;i<8;i++)
	{
		VSum += V[i]*V[i];
	}
	m0=sqrt(VSum / (2*N - 6));//中误差m0

	double M[6] = {0.0};//保存六个值的中误差
	for (i = 0; i != 6; i ++)
	{
		M[i] = m0 * sqrt(Q[i]);
		if (i >= 3)
		{
			M[i] = M[i]*180*3600/PI;
		}
	}

	OutputResult(&eoeo, R, M, m0);

	cout<<endl<<"解算全部完成"<<endl<<endl;
}


//////////////////////////////////////////////////////////////////////////
// 函数功能：初始化程序界面
//////////////////////////////////////////////////////////////////////////
void InitInterface()
{
	system("title 单像空间后方交会 by: 武汉大学 遥感信息工程学院 卢昊");
}


//////////////////////////////////////////////////////////////////////////
// 函数功能：输出解算结果
// 参数说明：eoeo：指向最终解算结果的结构体数组，RotationMatrix：旋转矩阵
//			 Precision：保存计算精度的数组，m0：单位权中误差
void OutputResult(EOEO* eoeo, double* RotationMatrix, double* Precision, double m0)
{
	cout<<"计算结果:"<<endl;
	printf("六个外方位元素为：\n");
	printf("Xs = %.4f\n", eoeo->Xs);
	printf("Ys = %.4f\n", eoeo->Ys);
	printf("Zs = %.4f\n", eoeo->Zs);
	printf("phi = %.10f\n", eoeo->phi);
	printf("omega = %.10f\n", eoeo->omega);
	printf("kappa = %.10f\n", eoeo->kappa);
	
	cout<<endl<<"旋转矩阵:\n";
	cout<<RotationMatrix[0]<<'\t'<<RotationMatrix[1]<<'\t'<<RotationMatrix[2]<<endl;
	cout<<RotationMatrix[3]<<'\t'<<RotationMatrix[4]<<'\t'<<RotationMatrix[5]<<endl;
	cout<<RotationMatrix[6]<<'\t'<<RotationMatrix[7]<<'\t'<<RotationMatrix[8]<<endl<<endl;

	cout<<"单位权中误差: "<<m0<<endl<<endl;

	cout << "六元素精度:"<<endl;
	cout <<"Xs:    "<<Precision[0]<<"米"<<endl;
	cout <<"Ys:    "<<Precision[1]<<"米"<<endl;
	cout <<"Zs:    "<<Precision[2]<<"米"<<endl;
	cout <<"phi:   "<<Precision[3]<<"秒"<<endl;
	cout <<"omega: "<<Precision[4]<<"秒"<<endl;
	cout <<"kappa: "<<Precision[5]<<"秒"<<endl<<endl;
	system("pause");
}


//////////////////////////////////////////////////////////////////////////////////////////

void main()
{
	InitInterface();
	SourceData sd[N];
	double m(50000);
	double f(153.24);
	char filename[200] = {"sourcedata.txt"};

	InitData(sd,filename);
	Iterator(sd,m,f);
}