www.gusucode.com > VC++空间后方交会的3种形式实例源码-源码程序 > VC++空间后方交会的3种形式实例源码-源码程序/code/CResection/CResection.cpp
#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" //Download by http://www.NewXing.com #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(); }