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

    //Download by http://www.NewXing.com
#include <string>
#include <fstream>
#include <iostream.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <stdio.h>
#include <iterator>
#include "MatrixOperation.h"
#include "CResection.h"

#define MAXITERATION 10
#define  PI 3.1415926
using namespace std;

CResection::~CResection()
{

}

CResection::CResection()
{
	bDone = false;
}

CResection::CResection(double PhotographicScale, double focus, char* filePathName)
{
	bDone = false;

	m = PhotographicScale;
	f = focus;
	dotNum = 0;
	sd.empty();
	memcpy(filename,filePathName,MAX_PATH);
	InitData();
	Iterator();
}

void CResection::InitData()
{
	//打开文件以备读取数据
//	cout <<"开始读取数据..."<<endl;
	ifstream datafile1(filename,ios::in | ios::nocreate);
	if( !datafile1)
	{
//		cout<<"打开文件失败, 文件可能不存在"<<endl;
//		system("pause");
		_exit(1);
	}

	int i;	//文本文件行数，即已知点个数
	string temp;

	while(getline(datafile1, temp))
		dotNum ++;	//统计行数
	datafile1.close();

	ifstream datafile2(filename,ios::in | ios::nocreate);
	SourceData sdtemp;
	for (i = 0; i != dotNum; i ++)
	{
		datafile2 >> sdtemp.x >> sdtemp.y >> sdtemp.X >> sdtemp.Y >> sdtemp.Z;
		sd.push_back(sdtemp);
	}

	datafile2.close();

//	cout <<"读取数据完毕..."<<endl;
}

bool CResection::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;
}

void CResection::Iterator()
{
	double phi, omega, kappa, Xs, Ys, Zs;
	phi = omega = kappa =0.0;
	Xs = Ys = Zs = 0.0;

	for (int k=0;k<dotNum;k++)
	{
		sd[k].x /= 1000.0;
		sd[k].y /= 1000.0;
		Xs += sd[k].X;
		Ys += sd[k].Y;
	}

	Xs /= dotNum;
	Ys /= dotNum;
	
	f = f / 1000.0;
	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, *Y0, *Z0;
	X0 = new double[dotNum];
	Y0 = new double[dotNum];
	Z0 = new double[dotNum];
	memset(X0,0,dotNum*sizeof(double));
	memset(Y0,0,dotNum*sizeof(double));
	memset(Z0,0,dotNum*sizeof(double));
	
	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 != dotNum; 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.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 VSum(0);//[vv]，即平方和

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

	for (i = 0; i != 6; i ++)
	{
		Precision[i] = m0 * sqrt(Q[i]);
		if (i >= 3)
		{
			Precision[i] = Precision[i]*180*3600/PI;
		}
	}

	//全部完成，可以输出或保存结果
	bDone = true;

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



//////////////////////////////////////////////////////////////////////////
// 函数功能：输出解算结果
// 参数说明：eoeo：指向最终解算结果的结构体数组，RotationMatrix：旋转矩阵
//			 Precision：保存计算精度的数组，m0：单位权中误差
void CResection::OutputResult()
{
	if (!bDone)
	{
		cout<<"计算尚未进行或未正常完成。"<<endl;
		return;
	}
	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<<R[0]<<'\t'<<R[1]<<'\t'<<R[2]<<endl;
	cout<<R[3]<<'\t'<<R[4]<<'\t'<<R[5]<<endl;
	cout<<R[6]<<'\t'<<R[7]<<'\t'<<R[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 CResection::SaveResult(char* filePathName)
{
	ofstream resultfile(filePathName);
	if( !resultfile)
	{
		_exit(1);
	}

	if (!bDone)
	{
		resultfile << "计算尚未进行或未正常完成。";
		return;
	}

	resultfile.flush();

	resultfile << "计算结果:"<<endl;
	resultfile << "六个外方位元素为：\n";
	resultfile << "Xs = " << eoeo.Xs << endl;
	resultfile << "Ys = " << eoeo.Ys << endl;
	resultfile << "Zs = " << eoeo.Zs << endl;
	resultfile << "phi = " << eoeo.phi << endl;
	resultfile << "omega = " << eoeo.omega << endl;
	resultfile << "kappa = " << eoeo.kappa << endl;
	
	resultfile << endl<< "旋转矩阵:\n";
	resultfile << R[0]<<'\t'<<R[1]<<'\t'<<R[2]<<endl;
	resultfile << R[3]<<'\t'<<R[4]<<'\t'<<R[5]<<endl;
	resultfile << R[6]<<'\t'<<R[7]<<'\t'<<R[8]<<endl<<endl;
	
	resultfile << "单位权中误差: "<<m0<<endl<<endl;
	
	resultfile << "六元素精度:"<<endl;
	resultfile << "Xs:    "<<Precision[0]<< "米"<<endl;
	resultfile << "Ys:    "<<Precision[1]<< "米"<<endl;
	resultfile << "Zs:    "<<Precision[2]<< "米"<<endl;
	resultfile << "phi:   "<<Precision[3]<< "秒"<<endl;
	resultfile << "omega: "<<Precision[4]<< "秒"<<endl;
	resultfile << "kappa: "<<Precision[5]<< "秒"<<endl;

	resultfile.close();
}