工控编程吧

标题: 上位机MFC实现单像空间后方交会源代码 [打印本页]

---

作者: qq263946146    时间: 2019-10-11 10:20
标题: 上位机MFC实现单像空间后方交会源代码

一、程序流程图

(, 下载次数: 0)

上传

点击文件名下载附件

二、部分代码

两个结构体，分别用来保存外方位元素和原始数据。

1. struct EOEO                // elements of exterior orientation
   
2. {
   
3.         double Xs;
   
4.         double Ys;
   
5.         double Zs;
   
6.         double omega;
   
7.         double phi;
   
8.         double kappa;
   
9. };
   

复制代码

1. 几个主要函数
   
2. 
   
3. //////////////////////////////////////////////////////////////////////////
   
4. // 函数功能：初始化坐标数据
   
5. // 参数说明：sd：保存原始数据的结构体数组，PhotographicScale：摄影比例尺，focus：摄影机主距
   
6. //                         filename：保存坐标数据的文件名
   
7. //////////////////////////////////////////////////////////////////////////
   
8. void InitData(SourceData* sd, char* filename)
   
9. {
   
10.         //打开文件以备读取数据
    
11.         cout <<"开始读取数据..."<<endl;
    
12.         ifstream datafile(filename,ios::in | ios::nocreate);
    
13.         if( !datafile)
    
14.         {
    
15.                 cout<<"打开文件失败, 文件可能不存在"<<endl;
    
16.                 system("pause");
    
17.                 _exit(1);
    
18.         }
    
19. 
    
20.         memset(sd, 0, N*sizeof(SourceData));
    
21.        
    
22.         int i;
    
23.         for (i = 0; i != N; i ++)
    
24.         {
    
25.                 datafile >> sd[i].x >> sd[i].y >> sd[i].X >> sd[i].Y >> sd[i].Z;
    
26.         }
    
27.         datafile.close();
    
28.         cout <<"读取数据完毕..."<<endl;
    
29. }
    
30. 
    
31. 
    
32. //////////////////////////////////////////////////////////////////////////
    
33. // 函数功能：检查改正数是否已达精度要求
    
34. // 参数说明：deta：保存改正数的数组
    
35. //////////////////////////////////////////////////////////////////////////
    
36. bool CheckPrecision(double* deta)
    
37. {
    
38.         //2.9088820866572159615394846141477e-5 即 0.1′的弧度表示
    
39.         bool ret;
    
40.         ret = (fabs(deta[0])<0.000001 && fabs(deta[1])<0.000001        && fabs(deta[2])<0.000001 && \
    
41.                 fabs(deta[3])<2.90888208656e-5 && fabs(deta[4])<2.90888208656e-5 && \
    
42.                 fabs(deta[5])<2.90888208656e-5);
    
43.         return ret;
    
44. }
    
45. 
    
46. //////////////////////////////////////////////////////////////////////////
    
47. // 函数功能：迭代器，计算的主体部分
    
48. // 参数说明：sd：保存原始数据的结构体数组，PhotographicScale：摄影比例尺，focus：摄影机主距
    
49. //////////////////////////////////////////////////////////////////////////
    
50. void Iterator(SourceData sd[N], double PhotographicScale, double Focus)
    
51. {
    
52.         double phi, omega, kappa, Xs, Ys, Zs;
    
53.         phi = omega = kappa =0.0;
    
54.         Xs = Ys = Zs = 0.0;
    
55. 
    
56.         for (int k=0;k<N;k++)
    
57.         {
    
58.                 sd[k].x /= 1000.0;
    
59.                 sd[k].y /= 1000.0;
    
60.                 Xs += sd[k].X;
    
61.                 Ys += sd[k].Y;
    
62.         }
    
63.         Xs /= N;
    
64.         Ys /= N;
    
65.        
    
66.         double f = Focus / 1000.0;
    
67.         double m = PhotographicScale;
    
68.         Zs =m*f;
    
69.        
    
70.         cout<<endl<<"六元素初始化值:"<<endl;
    
71.         cout <<"Xs: "<<Xs<<'\t'<<"Ys: "<<Ys<<'\t'<<"Zs: "<<Zs<<endl;
    
72.         cout <<"phi: "<<phi<<"\t\t"<<"omega: "<<omega<<"\t"<<"kappa: "<<kappa<<endl<<endl;
    
73. 
    
74.         //声明并初始化六元素改正数矩阵
    
75.         double deta[6] = {1,1,1,1,1,1};
    
76. 
    
77.         double x0(0);
    
78.         double y0(0);//内方位元素
    
79.         double X0[N] = {0.0};
    
80.         double Y0[N] = {0.0};
    
81.         double Z0[N] = {0.0};
    
82. 
    
83.         //声明旋转矩阵
    
84.         double R[9];
    
85. 
    
86.         double A[2*6] = {0.0};
    
87.         double AT[6*2] = {0.0};
    
88. 
    
89.         double Buf1[36] = {0.0};
    
90.         double Buf2[36] = {0.0};//ATA累加
    
91. 
    
92.         double Buf3[6] = {0.0};
    
93.         double Buf4[6] = {0.0};//ATL累加
    
94.        
    
95.         double Buf5[8*6] = {0.0};//存储8×6的A矩阵，没办法
    
96.         double Buf6[8*1] = {0.0};//存储8×1的L矩阵，同上
    
97.         double V[8*1] = {0.0};
    
98. 
    
99.         double ATA[36] = {0.0};
    
100.         double ATL[6] = {0.0};
     
101.         double L[2] = {0.0};
     
102. 
     
103.         int iCount = 0;//迭代次数
     
104.         cout << "开始迭代计算..."<<endl;
     
105.         while(!CheckPrecision(deta))
     
106.         {
     
107.                 cout <<endl<<"第 "<< ++iCount<<"次迭代:"<<endl;
     
108.                 if (iCount == MAXITERATION)
     
109.                 {
     
110.                         cout << ">>>迭代次数超限，可能不收敛"<<endl;
     
111.                         break;
     
112.                 }
     
113. 
     
114.                 //每次迭代之前必须清空两个保存累加值的矩阵ATA与ATL
     
115.                 for (int i = 0; i != 36; i ++)
     
116.                 {
     
117.                         ATA[i] = 0.0;
     
118.                         if (i < 6)
     
119.                         {
     
120.                                 ATL[i] = 0.0;
     
121.                         }
     
122.                 }
     
123. 
     
124.                 //计算旋转矩阵
     
125.                 R[0] = cos(phi)*cos(kappa) - sin(phi)*sin(omega)*sin(kappa);
     
126.                 R[1] = -cos(phi)*sin(kappa) - sin(phi)*sin(omega)*cos(kappa);
     
127.                 R[2] = -sin(phi)*cos(omega);
     
128.                 R[3] = cos(omega)*sin(kappa);
     
129.                 R[4] = cos(omega)*cos(kappa);
     
130.                 R[5] = -sin(omega);
     
131.                 R[6] = sin(phi)*cos(kappa) + cos(phi)*sin(omega)*sin(kappa);
     
132.                 R[7] = -sin(phi)*sin(kappa) + cos(phi)*sin(omega)*cos(kappa);
     
133.                 R[8] = cos(phi)*cos(omega);
     
134. 
     
135.                 for (i = 0; i != N; i ++)
     
136.                 {
     
137.                         Z0[i] = R[2]*(sd[i].X-Xs) + R[5]*(sd[i].Y-Ys) + R[8]*(sd[i].Z-Zs);
     
138.                         X0[i] = x0 - f*(R[0]*(sd[i].X-Xs) + R[3]*(sd[i].Y-Ys) + R[6]*(sd[i].Z-Zs)) / Z0[i];
     
139.                         Y0[i] = y0 - f*(R[1]*(sd[i].X-Xs) + R[4]*(sd[i].Y-Ys) + R[7]*(sd[i].Z-Zs)) / Z0[i];
     
140. 
     
141.                         A[0] = ((R[0]*f + R[2]*(sd[i].x-x0)) / Z0[i]);
     
142.                         A[1] = ((R[3]*f + R[5]*(sd[i].x-x0)) / Z0[i]);
     
143.                         A[2] = ((R[6]*f + R[8]*(sd[i].x-x0)) / Z0[i]);
     
144.                         A[3] = ((sd[i].y-y0)*sin(omega) - ((sd[i].x-x0)*((sd[i].x-x0)*cos(kappa)\
     
145.                                 - (sd[i].y-y0)*sin(kappa))/f + f*cos(kappa))*cos(omega));
     
146.                         A[4] = (-f*sin(kappa) - (sd[i].x-x0)*((sd[i].x-x0)*sin(kappa) + (sd[i].y-y0)*cos(kappa))/f);
     
147.                         A[5] = (sd[i].y - y0);
     
148.                         A[6] = ((R[1]*f + R[2]*(sd[i].y-y0)) /Z0[i]);
     
149.                         A[7] = ((R[4]*f + R[5]*(sd[i].y-y0)) /Z0[i]);
     
150.                         A[8] = ((R[7]*f + R[8]*(sd[i].y-y0)) /Z0[i]);
     
151.                         A[9] = (-(sd[i].x-x0)*sin(omega) - ((sd[i].y-y0)*((sd[i].x-x0)*cos(kappa)\
     
152.                                 - (sd[i].y-y0)*sin(kappa))/f - f*sin(kappa))*cos(omega));
     
153.                         A[10] = (-f*cos(kappa) - (sd[i].y-y0)*((sd[i].x-x0)*sin(kappa) + (sd[i].y-y0)*cos(kappa))/f);
     
154.                         A[11] = (-(sd[i].x-x0));
     
155. 
     
156.                         //该循环保存A矩阵，最后评定精度用
     
157.                         for (int l=0;l<12;l++)
     
158.                         {
     
159.                                 Buf5[12*i+l] = A[l];
     
160.                         }
     
161.                        
     
162.                         //所谓的逐步法化，即要在循环内部就将ATA计算出来累加，下面的L矩阵类似
     
163.                         MatrixTranspose(A,AT,2,6);
     
164.                         MatrixMultiply(AT,A,Buf1,6,2,6);
     
165.                         MatrixCopy(ATA, Buf2, 36);
     
166.                         MatrixAdd(Buf1,Buf2,ATA,36);        // 为逐步法化后的ATA矩阵累加
     
167. 
     
168.                         L[0] = (sd[i].x - X0[i]);
     
169.                         L[1] = (sd[i].y - Y0[i]);
     
170. 
     
171.                         //保存L矩阵，最后评定精度用
     
172.                         for (l=0;l<2;l++)
     
173.                         {
     
174.                                 Buf6[2*i+l] = L[l];
     
175.                         }
     
176. 
     
177.                         MatrixMultiply(AT,L,Buf3,6,2,1);
     
178.                         MatrixCopy(ATL, Buf4, 6);
     
179.                         MatrixAdd(Buf3,Buf4,ATL,6);                       
     
180.                 }//for
     
181. 
     
182.                 //“逐步法化”的另一处不同，出循环即可直接计算ATA逆乘ATL
     
183.                 MatrixInversion(ATA,6);
     
184.                 MatrixMultiply(ATA,ATL,deta,6,6,1);
     
185. 
     
186.                 //deta即为改正数
     
187.                 Xs += deta[0];
     
188.                 Ys += deta[1];
     
189.                 Zs += deta[2];
     
190.                 phi += deta[3];
     
191.                 omega += deta[4];
     
192.                 kappa += deta[5];
     
193. 
     
194.                 cout<<"改正数值:"<<endl;
     
195.                 for (i=0; i != 6; i ++)
     
196.                 {
     
197.                         cout <<"deta["<<i<<"]: "<<deta[i]<<endl;
     
198.                 }
     
199.                 cout << endl<<"六元素值:"<<endl;
     
200.                 cout <<"Xs: "<<Xs<<endl<<"Ys: "<<Ys<<endl<<"Zs: "<<Zs<<endl;
     
201.                 cout <<"kappa: "<<kappa<<endl<<"omega: "<<omega<<endl<<"phi: "<<phi<<endl<<endl;
     
202.         }//while
     
203. 
     
204.         EOEO eoeo;
     
205.         eoeo.kappa = kappa;
     
206.         eoeo.omega = omega;
     
207.         eoeo.phi = phi;
     
208.         eoeo.Xs = Xs;
     
209.         eoeo.Ys = Ys;
     
210.         eoeo.Zs = Zs;
     
211. 
     
212.         cout << ">>>正常退出迭代"<<endl<<endl<<endl;
     
213. 
     
214.         //精度评定
     
215.         double Q[6] = {0.0};
     
216.         for (int h=0;h<6;h++)
     
217.         {
     
218.                 Q[h] = ATA[h*6+h];
     
219.         }
     
220. 
     
221.         MatrixMultiply(Buf5,deta,V,8,6,1);//V=Ax-L
     
222.         MatrixMinus(V,Buf6,V,8);
     
223. 
     
224.         double m0(0);//单位权中误差
     
225.         double VSum(0);//[vv]，即平方和
     
226. 
     
227.         int i;
     
228.         for (i=0;i<8;i++)
     
229.         {
     
230.                 VSum += V[i]*V[i];
     
231.         }
     
232.         m0=sqrt(VSum / (2*N - 6));//中误差m0
     
233. 
     
234.         double M[6] = {0.0};//保存六个值的中误差
     
235.         for (i = 0; i != 6; i ++)
     
236.         {
     
237.                 M[i] = m0 * sqrt(Q[i]);
     
238.                 if (i >= 3)
     
239.                 {
     
240.                         M[i] = M[i]*180*3600/PI;
     
241.                 }
     
242.         }
     
243. 
     
244.         OutputResult(&eoeo, R, M, m0);
     
245.         cout<<endl<<"解算全部完成"<<endl<<endl;
     
246. }
     
247. 
     

复制代码

三、计算结果

(, 下载次数: 2)

上传

点击文件名下载附件

四、本程序的两种拓展形式

1、  将空间后方交会计算封装成类

为了实现空间后交计算的移植性，这里我将该过程封装成一个类CResection，并定义了相应的接口。

该类接受原始数据的接口为带参构造函数，其原型为：

CResection ();

输出接口为两个类成员函数：

       OutputResult();     //该成员适用于windows console application 程序调用时调用iostream库输出结果。

       SaveResult();        //该成员适用于任何情况下将计算结果保存为文件，用户指定文件路径与文件名。

       以上三个函数类型声明为public以满足从外部环境声明类对象调用的需要。另包含3个私有成员函数Iterator (),

CheckPrecision(),InitData() 进行内部计算，编码与运行时对用户不可见。

       详细请参看工程CResection代码。

用户调用该类时，只需将CResection.h与CResection.cpp添加进目标工程并在文件适当位置包含该类头文件即可。

使用实例：

#include “CResection.h”

//…………………

CResection res (50000,154.23, “SourceData.txt”);

res.SaveResult (“Results.txt”);

res.OutputResult();       //仅在控制台程序可用

2、  将该功能封装成动态链接库

封装成动态链接库，只要将类CResection稍加修改即可，对于代码能实现可靠的隐蔽性。

调用该动态链接库，只需在工程设置中link标签页下添加CResection.lib，

并在需要调用该库的文件中包含CResection.h，而后即可声明该类对象进行空间后交计算。

详细请参看工程CResection_dll代码

(, 下载次数: 0)

上传

点击文件名下载附件

[halcon]1[/halcon]

[MFC408]1[/MFC408]

[weixinlianxi]1[/weixinlianxi]

---

欢迎光临 工控编程吧 (https://www.gkbc8.com/)

Powered by Discuz! X3.4