Category

Development Platform
Home > SourceCode/Document > Graph program > View Code
DLTDlg.cpp
Package: DLT.rar　[view]
Upload User: szacenet
Upload Date: 2022-06-20
Package Size: 317k
Code Size: 22k
Category:

Graph program

Development Platform:

Visual C++

DLTDlg.cpp：Code Content
  1. // DLTDlg.cpp : implementation file
  2. //
  4. #include "stdafx.h"
  5. #include "DLT.h"
  6. #include "DLTDlg.h"
  8. #ifdef _DEBUG
  9. #define new DEBUG_NEW
  10. #undef THIS_FILE
  11. static char THIS_FILE[] = __FILE__;
  12. #endif
  14. /////////////////////////////////////////////////////////////////////////////
  15. // CAboutDlg dialog used for App About
  17. class CAboutDlg : public CDialog
  18. {
  19. public:
  20. CAboutDlg();
  22. // Dialog Data
  23. //{{AFX_DATA(CAboutDlg)
  24. enum { IDD = IDD_ABOUTBOX };
  25. //}}AFX_DATA
  27. // ClassWizard generated virtual function overrides
  28. //{{AFX_VIRTUAL(CAboutDlg)
  29. protected:
  30. virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
  31. //}}AFX_VIRTUAL
  33. // Implementation
  34. protected:
  35. //{{AFX_MSG(CAboutDlg)
  36. //}}AFX_MSG
  37. DECLARE_MESSAGE_MAP()
  38. };
  40. CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
  41. {
  42. //{{AFX_DATA_INIT(CAboutDlg)
  43. //}}AFX_DATA_INIT
  44. }
  46. void CAboutDlg::DoDataExchange(CDataExchange* pDX)
  47. {
  48. CDialog::DoDataExchange(pDX);
  49. //{{AFX_DATA_MAP(CAboutDlg)
  50. //}}AFX_DATA_MAP
  51. }
  53. BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
  54. //{{AFX_MSG_MAP(CAboutDlg)
  55. // No message handlers
  56. //}}AFX_MSG_MAP
  57. END_MESSAGE_MAP()
  59. /////////////////////////////////////////////////////////////////////////////
  60. // CDLTDlg dialog
  62. CDLTDlg::CDLTDlg(CWnd* pParent /*=NULL*/)
  63. : CDialog(CDLTDlg::IDD, pParent)
  64. {
  65. //{{AFX_DATA_INIT(CDLTDlg)
  66. //}}AFX_DATA_INIT
  67. // Note that LoadIcon does not require a subsequent DestroyIcon in Win32
  68. m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
  70. pLeftLi = new double[12];//li系数精确值,k
  71. pRightLi = new double[12];//li系数精确值,k
  72. pLOrient = new double [11];
  73. pROrient = new double [11];
  74. }
  76. void CDLTDlg::DoDataExchange(CDataExchange* pDX)
  77. {
  78. CDialog::DoDataExchange(pDX);
  79. //{{AFX_DATA_MAP(CDLTDlg)
  80. //}}AFX_DATA_MAP
  81. }
  83. BEGIN_MESSAGE_MAP(CDLTDlg, CDialog)
  84. //{{AFX_MSG_MAP(CDLTDlg)
  85. ON_WM_SYSCOMMAND()
  86. ON_WM_PAINT()
  87. ON_WM_QUERYDRAGICON()
  88. ON_BN_CLICKED(IDC_BUTN_CTRL, OnButnCtrl)
  89. ON_BN_CLICKED(IDC_BUTN_LEFT, OnButnLeft)
  90. ON_BN_CLICKED(IDC_BUTN_RIGHT, OnButnRight)
  91. //}}AFX_MSG_MAP
  92. END_MESSAGE_MAP()
  94. /////////////////////////////////////////////////////////////////////////////
  95. // CDLTDlg message handlers
  97. BOOL CDLTDlg::OnInitDialog()
  98. {
  99. CDialog::OnInitDialog();
  101. // Add "About..." menu item to system menu.
  103. // IDM_ABOUTBOX must be in the system command range.
  104. ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
  105. ASSERT(IDM_ABOUTBOX < 0xF000);
  107. CMenu* pSysMenu = GetSystemMenu(FALSE);
  108. if (pSysMenu != NULL)
  109. {
  110. CString strAboutMenu;
  111. strAboutMenu.LoadString(IDS_ABOUTBOX);
  112. if (!strAboutMenu.IsEmpty())
  113. {
  114. pSysMenu->AppendMenu(MF_SEPARATOR);
  115. pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
  116. }
  117. }
  119. // Set the icon for this dialog.  The framework does this automatically
  120. //  when the application's main window is not a dialog
  121. SetIcon(m_hIcon, TRUE); // Set big icon
  122. SetIcon(m_hIcon, FALSE); // Set small icon
  124. // TODO: Add extra initialization here
  126. return TRUE;  // return TRUE  unless you set the focus to a control
  127. }
  129. void CDLTDlg::OnSysCommand(UINT nID, LPARAM lParam)
  130. {
  131. if ((nID & 0xFFF0) == IDM_ABOUTBOX)
  132. {
  133. CAboutDlg dlgAbout;
  134. dlgAbout.DoModal();
  135. }
  136. else
  137. {
  138. CDialog::OnSysCommand(nID, lParam);
  139. }
  140. }
  142. // If you add a minimize button to your dialog, you will need the code below
  143. //  to draw the icon.  For MFC applications using the document/view model,
  144. //  this is automatically done for you by the framework.
  146. void CDLTDlg::OnPaint() 
  147. {
  148. if (IsIconic())
  149. {
  150. CPaintDC dc(this); // device context for painting
  152. SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);
  154. // Center icon in client rectangle
  155. int cxIcon = GetSystemMetrics(SM_CXICON);
  156. int cyIcon = GetSystemMetrics(SM_CYICON);
  157. CRect rect;
  158. GetClientRect(&rect);
  159. int x = (rect.Width() - cxIcon + 1) / 2;
  160. int y = (rect.Height() - cyIcon + 1) / 2;
  162. // Draw the icon
  163. dc.DrawIcon(x, y, m_hIcon);
  164. }
  165. else
  166. {
  167. CDialog::OnPaint();
  168. }
  169. }
  171. // The system calls this to obtain the cursor to display while the user drags
  172. //  the minimized window.
  173. HCURSOR CDLTDlg::OnQueryDragIcon()
  174. {
  175. return (HCURSOR) m_hIcon;
  176. }
  178. void CDLTDlg::OnOK() 
  179. {
  180. // TODO: Add extra validation here
  181. if (m_CtrlPoints.size()==0 ||m_LeftPoints.size() == 0 || m_RightPoints.size()==0)
  182. {
  183. MessageBox("请先输入数据");
  184. return ;
  185. }
  186. CalXYZAndPrecision(m_LeftPoints,m_RightPoints,pLOrient,pROrient,pLeftLi,pRightLi);
  187. if(ShellExecute(NULL,"open",strResultFileName,NULL,NULL,SW_SHOWNORMAL) < (HANDLE)32)
  188. MessageBox("结果保存在："+strResultFileName,"TipS",MB_OK|MB_ICONINFORMATION);
  189. //CDialog::OnOK();
  190. }
  192. void CDLTDlg::OnButnLeft() 
  193. {
  194. // TODO: Add your control notification handler code here
  195. if(m_CtrlPoints.size()==0)
  196. {
  197. MessageBox("无控制点数据","ERROR",MB_OK|MB_ICONWARNING);
  198. return;
  199. }
  200. //读取数据
  201. m_LeftPoints.clear();
  202.     CString filename = OpenFileStore(&m_LeftPoints);
  203. if (m_LeftPoints.size()<6)
  204. {
  205. MessageBox("量测像点数目太少，不能计算！","Tips",MB_OK|MB_ICONINFORMATION);
  206. m_LeftPoints.clear();
  207. SetDlgItemText(IDC_BUTN_LEFT,"");
  208. return;
  209. }
  210. SetDlgItemText(IDC_EDIT_LEFT,filename);
  211. //////////////////////////////////////////////////
  212. //计算li系数近似值
  213. FILE *fp;
  214. fp = fopen(strResultFileName,"a+");
  215. double *pLi = new double [11];
  216. if(!CalApproximateLiFactors(m_LeftPoints,m_CtrlPoints,pLi))
  217. {
  218. MessageBox("像点对应的控制点不足6个，无法计算!");
  219. m_LeftPoints.clear();
  220. SetDlgItemText(IDC_BUTN_LEFT,"");
  221. return ;
  222. }
  223. //计算li系数精确值,k和方位元素
  224. int nCnt = CalOrtAndLiFactors(m_LeftPoints,m_CtrlPoints,pLi,pLOrient,pLeftLi);
  225. fprintf(fp,"左片"+filename+"的li系数如下：n     近似值         精确值n");
  226.     for (int i = 0;i<11;i++)
  227.     {
  228. fprintf(fp,"l%d  %.6f   %.6fn",i,pLi[i],pLeftLi[i]);
  229.     }
  230. fprintf(fp,"对称径向畸变系数k:%.6fn迭代次数：%dn",pLeftLi[i],nCnt);
  231. //输出方位元素
  232. CString strFWs[11] ={"x0","y0","fx","ds","dB","Xs","Ys","Zs","t(phi)","w","k"};
  233. for ( i = 0;i<11;i++)
  234.     {
  235. fprintf(fp,"%s  %.6fn",strFWs[i],pLOrient[i]);
  236.     }
  237. fclose(fp);
  238. delete [] pLi;
  239. /////////////////////////
  240. }
  242. void CDLTDlg::OnButnRight() 
  243. {
  244. // TODO: Add your control notification handler code here
  245. if(m_CtrlPoints.size()==0)
  246. {
  247. MessageBox("无控制点数据","ERROR",MB_OK|MB_ICONWARNING);
  248. return;
  249. }
  250. m_RightPoints.clear();
  251.     CString filename = OpenFileStore(&m_RightPoints);
  252. if (m_RightPoints.size()<6)
  253. {
  254. MessageBox("量测像点数目太少，不能计算！","Tips",MB_OK|MB_ICONINFORMATION);
  255. m_RightPoints.clear();
  256. SetDlgItemText(IDC_BUTN_RIGHT,"");
  257. return;
  258. }
  259. SetDlgItemText(IDC_EDIT_RIGHT,filename);
  260. //////////////////////////
  261. FILE *fp;
  262. fp = fopen(strResultFileName,"a+");
  263. double *pLi = new double [11];
  264. if(!CalApproximateLiFactors(m_RightPoints,m_CtrlPoints,pLi))
  265. {
  266. MessageBox("像点对应的控制点不足6个，无法计算!");
  267. m_RightPoints.clear();
  268. SetDlgItemText(IDC_BUTN_LEFT,"");
  269. return ;
  270. }
  271. //计算li系数精确值,k和方位元素
  272. int nCnt = CalOrtAndLiFactors(m_RightPoints,m_CtrlPoints,pLi,pROrient,pRightLi);
  273. fprintf(fp,"右片"+filename+"的li系数如下：n     近似值         精确值n");
  274.     for (int i = 0;i<11;i++)
  275.     {
  276. fprintf(fp,"l%d  %.6f   %.6fn",i,pLi[i],pRightLi[i]);
  277.     }
  278. fprintf(fp,"对称径向畸变系数k:%.6fn迭代次数：%dn",pRightLi[i],nCnt);
  279. //输出方位元素
  280. CString strFWs[11] ={"x0","y0","fx","ds","dB","Xs","Ys","Zs","t(phi)","w","k"};
  281. for ( i = 0;i<11;i++)
  282.     {
  283. fprintf(fp,"%s  %.6fn",strFWs[i],pROrient[i]);
  284.     }
  285. fclose(fp);
  287. delete [] pLi;
  288. /////////////////////////
  289. }
  291. void CDLTDlg::OnButnCtrl() 
  292. {
  293. // TODO: Add your control notification handler code here
  294. m_CtrlPoints.clear();
  295.     CString filename = OpenFileStore(&m_CtrlPoints);
  296. if(filename=="") return ;
  297. if (m_CtrlPoints.size()<6)
  298. {
  299. MessageBox("控制点数目太少，不能计算！","Tips",MB_OK|MB_ICONINFORMATION);
  300. m_CtrlPoints.clear();
  301. SetDlgItemText(IDC_BUTN_CTRL,"");
  302. return;
  303. }
  304. SetDlgItemText(IDC_EDIT_CTRL,filename);
  305. strResultFileName = filename.Left(filename.ReverseFind('\')+1)+"\DLT_Results.txt";
  306. }
  308. CString CDLTDlg::OpenFileStore(vector<PixPoint> * Container )
  309. {
  310.     CString strFilename = "";
  311. Container->clear();
  312. CFileDialog openFile(TRUE,NULL,NULL,OFN_HIDEREADONLY|OFN_OVERWRITEPROMPT,"文本文件(*.txt)|*.txt||",NULL);
  313. if (openFile.DoModal()==IDCANCEL)
  314. return "";
  315. strFilename = openFile.GetPathName();
  316. //read data
  317. CStdioFile pInFile;
  318. if (!pInFile.Open(strFilename, CFile::modeRead)) 
  319. return "";
  320. int nNum =0 ;//point order number
  321. double x = 0.0;//  
  322. double y = 0.0;
  324. CString strLine;
  325. while (pInFile.ReadString(strLine))
  326. {
  327. //strLine是一行文本
  328. PixPoint point;
  329. sscanf(strLine,"%d %lf %lf",&nNum,&x,&y);
  330. point.ID = nNum;
  331. point.x = x;//PixPoint(nNum,x,y);
  332. point.y = y;
  333. Container->push_back(point); 
  334. }
  335. pInFile.Close();
  336.     return strFilename;
  337. }
  339. CString CDLTDlg::OpenFileStore(vector<GeodeticPoint> * Container )
  340. {
  341. CString strFilename = "";
  342. Container->clear();
  343. CFileDialog openFile(TRUE,NULL,NULL,OFN_HIDEREADONLY|OFN_OVERWRITEPROMPT,"文本文件(*.txt)|*.txt||",NULL);
  344. if (openFile.DoModal()==IDCANCEL)
  345. return "";
  346. strFilename = openFile.GetPathName();
  347. //read data
  348. CStdioFile pInFile;
  349. if (!pInFile.Open(strFilename, CFile::modeRead)) 
  350. return "";
  351. int nNum =0 ;//point order number
  352. double x = 0.0;//  
  353. double y = 0.0;
  354. double z =0.0;
  356. CString strLine;
  357. while (pInFile.ReadString(strLine))
  358. {
  359. //strLine是一行文本
  360. GeodeticPoint point;
  361. sscanf(strLine,"%d %lf %lf %lf",&nNum,&x,&y,&z);
  362. point.ID = nNum;
  363. point.x = x;//GeodeticPoint(nNum,x,y,z);
  364. point.y = y;
  365. point.z = z;
  366. Container->push_back(point); 
  367. }
  368. pInFile.Close();
  369.     return strFilename; 
  370. }
  372. bool CDLTDlg::CalApproximateLiFactors(vector<PixPoint> xPixContner,vector<GeodeticPoint> CtrlContner,double *pPara)
  373. {
  374. //搜索像点对应的控制点
  375.     vector<GeodeticPoint> CalCtrlPts;
  376. vector<PixPoint>PixContner;
  377. for (int i =0;i<xPixContner.size();i++)
  378. {
  379. for(int j = 0;j<CtrlContner.size();j++)
  380. {
  381. if (xPixContner.at(i).ID == CtrlContner.at(j).ID)
  382. {
  383. CalCtrlPts.push_back(CtrlContner.at(j));
  384. PixContner.push_back(xPixContner.at(i));
  385. break;
  386. }
  387. }
  388. }
  390. int nCalPtsNum = CalCtrlPts.size();
  391. if (nCalPtsNum<6)
  392. return false;
  393. //计算li系数的近似解approximate calculation 
  394. double * LiApproA = new double[nCalPtsNum*2*11];
  395. double * LiApproC = new double[nCalPtsNum * 2];
  396. for (i = 0;i<nCalPtsNum;i++)//逐点计算系数
  397. {
  398. LiApproA[22*i + 0] = CalCtrlPts.at(i).x;
  399. LiApproA[22*i + 1] = CalCtrlPts.at(i).y;
  400. LiApproA[22*i + 2] = CalCtrlPts.at(i).z;
  401. LiApproA[22*i + 3] = 1;
  402. LiApproA[22*i + 4] = 0;
  403. LiApproA[22*i + 5] = 0;
  404. LiApproA[22*i + 6] = 0;
  405. LiApproA[22*i + 7] = 0;
  406. LiApproA[22*i + 8] = PixContner.at(i).x * CalCtrlPts.at(i).x;
  407. LiApproA[22*i + 9] = PixContner.at(i).x * CalCtrlPts.at(i).y;
  408. LiApproA[22*i + 10] = PixContner.at(i).x * CalCtrlPts.at(i).z;
  410. LiApproA[22*i + 11] = 0;
  411. LiApproA[22*i + 12] = 0;
  412. LiApproA[22*i + 13] = 0;
  413. LiApproA[22*i + 14] = 0;
  414. LiApproA[22*i + 15] = CalCtrlPts.at(i).x;
  415. LiApproA[22*i + 16] = CalCtrlPts.at(i).y;
  416. LiApproA[22*i + 17] = CalCtrlPts.at(i).z;
  417. LiApproA[22*i + 18] = 1;
  418. LiApproA[22*i + 19] = PixContner.at(i).y * CalCtrlPts.at(i).x;
  419. LiApproA[22*i + 20] = PixContner.at(i).y * CalCtrlPts.at(i).y;
  420. LiApproA[22*i + 21] = PixContner.at(i).y * CalCtrlPts.at(i).z;
  422. LiApproC[2*i + 0] = -1*PixContner.at(i).x;
  423. LiApproC[2*i + 1] = -1*PixContner.at(i).y;
  424. }
  426. double * LiApproAt = new double[nCalPtsNum*2*11];//A的转置
  427. double * LiApproAtA = new double[11*11];//A的转置 乘以 A
  428. double * LiApproAtC = new double[11];
  429. transpose(LiApproA,LiApproAt,nCalPtsNum*2,11);
  430. mult(LiApproAt,LiApproA,LiApproAtA,11,nCalPtsNum*2,11);
  431. inv(LiApproAtA,11);//LiApproAtA的逆仍为LiApproAtA
  432. mult(LiApproAt,LiApproC,LiApproAtC,11,nCalPtsNum*2,1);
  433. mult(LiApproAtA,LiApproAtC,pPara,11,11,1);
  434. //释放临时内存
  435. delete [] LiApproA;
  436. delete [] LiApproAt;
  437. delete [] LiApproAtA;
  438. delete [] LiApproAtC;
  439. delete [] LiApproC;
  440. CalCtrlPts.clear();
  441. PixContner.clear();
  442. return true; 
  443. }
  445. //返回pElement和pTrueLi，pElems中的顺序依次为x0,y0,fx,ds,dB,Xs,Ys,Zs,t(phi),w,k ; pTrueLi中为L1-L11和k
  446. int CDLTDlg::CalOrtAndLiFactors(vector<PixPoint> xPixContner,vector<GeodeticPoint> CtrlContner,double *li,double *pElems,double *pTrueLi)
  447. {
  448. //搜索像点对应的控制点
  449.     vector<GeodeticPoint> CalCtrlPts;
  450. vector<PixPoint>PixContner;
  451. for (int i =0;i<xPixContner.size();i++)
  452. {
  453. for(int j = 0;j<CtrlContner.size();j++)
  454. {
  455. if (xPixContner.at(i).ID == CtrlContner.at(j).ID)
  456. {
  457. CalCtrlPts.push_back(CtrlContner.at(j));
  458. PixContner.push_back(xPixContner.at(i));
  459. break;
  460. }
  461. }
  462. }
  463. ///////////////////////////////////////////////////////////
  464. int nCalPtsNum = CalCtrlPts.size();
  465. double x0,y0,r2;//r2为像点到像主点的距离平方
  466. BOOL bFlag = true;//迭代标志
  467. double * pL = new double [12];//前11个为li
  468. for (i =0;i<11;i++)
  469.  pL[i] = li[i];
  470. pL[i] = 0.0;
  471.     //分配临时存储空间
  472. double * M = new double[nCalPtsNum*2*12];
  473. double * W = new double[nCalPtsNum * 2];
  474. double * Mt = new double[nCalPtsNum*2*12];//M的转置
  475. double * MtM = new double[12*12];//M的转置 乘以 M
  476. double * MtW = new double[12];
  477. double xoyofenmu,A,B,C,fx,r32;//r3的平方
  478. int nIterationCnt = 0;
  479. while(bFlag)
  480. {
  481. nIterationCnt++;
  482. if (nIterationCnt == 101)
  483. {
  484. //MessageBox("迭代次数达到100次","Error",MB_OK|MB_ICONWARNING);
  485. return -1;
  486. }
  487. xoyofenmu = pL[8]*pL[8] + pL[9]*pL[9] + pL[10]*pL[10];
  488. x0 = -(pL[0]*pL[8] + pL[1]*pL[9] + pL[2]*pL[10])/xoyofenmu;
  489. y0 = -(pL[4]*pL[8] + pL[5]*pL[9] + pL[6]*pL[10])/xoyofenmu;
  490. //逐点计算系数
  491. for (i = 0;i<nCalPtsNum;i++)
  492. {
  493. A = pL[8]*CalCtrlPts.at(i).x + pL[9]*CalCtrlPts.at(i).y + pL[10]*CalCtrlPts.at(i).z + 1;
  494. r2 = pow(PixContner.at(i).x - x0,2) + pow(PixContner.at(i).y - y0,2);
  495.             M[24*i + 0] = -CalCtrlPts.at(i).x/A;
  496. M[24*i + 1] = -CalCtrlPts.at(i).y/A;
  497. M[24*i + 2] = -CalCtrlPts.at(i).z/A;
  498. M[24*i + 3] = -1/A;
  499. M[24*i + 4] = 0;
  500. M[24*i + 5] = 0;
  501. M[24*i + 6] = 0;
  502. M[24*i + 7] = 0;
  503. M[24*i + 8] = -PixContner.at(i).x * CalCtrlPts.at(i).x/A;
  504. M[24*i + 9] = -PixContner.at(i).x * CalCtrlPts.at(i).y/A;
  505. M[24*i + 10] = -PixContner.at(i).x * CalCtrlPts.at(i).z/A;
  506. M[24*i + 11] = -(PixContner.at(i).x - x0)*r2;
  508. M[24*i + 12] = 0;
  509. M[24*i + 13] = 0;
  510. M[24*i + 14] = 0;
  511. M[24*i + 15] = 0;
  512. M[24*i + 16] = -CalCtrlPts.at(i).x/A;
  513. M[24*i + 17] = -CalCtrlPts.at(i).y/A;
  514. M[24*i + 18] = -CalCtrlPts.at(i).z/A;
  515. M[24*i + 19] = -1/A;
  516. M[24*i + 20] = -PixContner.at(i).y * CalCtrlPts.at(i).x/A;
  517. M[24*i + 21] = -PixContner.at(i).y * CalCtrlPts.at(i).y/A;
  518. M[24*i + 22] = -PixContner.at(i).y * CalCtrlPts.at(i).z/A;
  519. M[24*i + 23] = -(PixContner.at(i).y - y0)*r2;
  521. W[2*i + 0] = PixContner.at(i).x/A;
  522.     W[2*i + 1] = PixContner.at(i).y/A;
  523. }
  524. //解求pL
  525.         transpose(M,Mt,nCalPtsNum*2,12);
  526. mult(Mt,M,MtM,12,nCalPtsNum*2,12);
  527. inv(MtM,12);
  528. mult(Mt,W,MtW,12,nCalPtsNum*2,1);
  529.     mult(MtM,MtW,pL,12,12,1);
  530. //求fx--pElems【2】作为迭代结束的标志
  531.         xoyofenmu = pL[8]*pL[8] + pL[9]*pL[9] + pL[10]*pL[10];
  532. x0 = -(pL[0]*pL[8] + pL[1]*pL[9] + pL[2]*pL[10])/xoyofenmu;
  533. y0 = -(pL[4]*pL[8] + pL[5]*pL[9] + pL[6]*pL[10])/xoyofenmu;
  534. r32 = 1/xoyofenmu;
  535. A = r32*(pL[0]*pL[0] + pL[1]*pL[1] + pL[2]*pL[2]) - x0*x0;
  536. B = r32*(pL[4]*pL[4] + pL[5]*pL[5] + pL[6]*pL[6]) - y0*y0;
  537. C = r32*(pL[0]*pL[4] + pL[1]*pL[5] + pL[2]*pL[6]) - x0*y0;
  538. pElems[4] = asin( sqrt( C*C/(A*B) ) );//dB
  539. fx = sqrt(A)*cos(pElems[4]) ;
  540.         if(nIterationCnt == 1)//第一次运算时pElems[2]无值
  541. {
  542. pElems[2] = fx;
  543. continue;
  544. }
  545.         if (fabs(fx - pElems[2])<0.001)
  546.         {
  547. bFlag = false;
  548. break;
  549.         }
  550. pElems[2] = fx;
  551. }
  552. //计算结果
  553. //返回Li系数
  554. for (i=0;i<12;i++)
  555. {
  556. pTrueLi[i] = pL[i];
  557. }
  558. //返回11个内外方位元素参数和变形参数
  559. //x0,y0,fx,ds,dB,Xs,Ys,Zs,t(phi),w,k 
  560.     pElems[0] = x0;//
  561. pElems[1] = y0;//
  562. pElems[2] = fx;//
  563. pElems[3] = sqrt(A/B) -1;//ds
  564. //pElems[4] = ;//dB迭代中已求
  565.    //先求角元素
  566. double a3,b3,c3,b1,b2;
  567. a3 = pL[8]/sqrt(xoyofenmu);
  568. b3 = pL[9]/sqrt(xoyofenmu);
  569. c3 = pL[10]/sqrt(xoyofenmu);
  570. b2 = (pL[5]*sqrt(r32) + b3*y0)*(1+pElems[3])*cos(pElems[4])/fx;
  571. b1 = (pElems[1]*sqrt(r32) + b2*fx*tan(pElems[4]) + b3*x0 )/fx ;
  572. pElems[8] = atan(-a3/c3);//t(phi)
  573. pElems[9] = asin(-b3);//w
  574. pElems[10] = atan(b1/b2);//k
  575.    //再求线元素
  576.     M[0] = pL [0] ;
  577. M[1] = pL [1] ;
  578. M[2] = pL [2] ;
  579. M[3] = pL [4] ;
  580. M[4] = pL [5] ;
  581. M[5] = pL [6] ;
  582. M[6] = pL [8] ;
  583. M[7] = pL [9] ;
  584. M[8] = pL [10] ;
  586. W[0] = -pL [3];
  587. W[1] = -pL [7];
  588. W[2] = -1;
  589. inv(M,3);
  590. mult(M,W,Mt,3,3,1);
  591. pElems[5] = Mt[0];//Xs
  592. pElems[6] = Mt[1];//
  593. pElems[7] = Mt[2];//
  596. //释放临时内存空间
  597. delete [] pL;
  598. delete [] M;
  599. delete [] Mt;
  600. delete [] MtM;
  601. delete [] MtW;
  602. CalCtrlPts.clear();
  603. PixContner.clear();
  604.     return nIterationCnt;
  605. }
  607. void CDLTDlg::CalXYZAndPrecision(vector<PixPoint> LPixContner,vector<PixPoint> RPixContner,double *pLLinar,double *pRLinar,double *pLi1,double *pLi2)
  608. {
  609. //搜索同名像点并进行非线性误差改正
  610.     vector<PRSPoint> CalCtrlPts;
  611. for (int i =0;i<LPixContner.size();i++)
  612. {
  613. for(int j = 0;j<RPixContner.size();j++)
  614. {
  615. if (LPixContner.at(i).ID == RPixContner.at(j).ID)
  616. {
  617. PRSPoint point;
  618. double r21,r22;
  619. //进行线性误差改正
  620. r21 = pow(LPixContner.at(i).x - pLLinar[0],2) + pow(LPixContner.at(i).y - pLLinar[1],2);
  621. r22 = pow(RPixContner.at(j).x - pRLinar[0],2) + pow(RPixContner.at(j).y - pRLinar[1],2);
  622. point.ID = LPixContner.at(i).ID;
  623. point.x1 = LPixContner.at(i).x + pLi1[11]*(LPixContner.at(i).x - pLLinar[0])*r21;
  624. point.y1 = LPixContner.at(i).y + pLi1[11]*(LPixContner.at(i).y - pLLinar[1])*r21;
  625. point.x2 = RPixContner.at(j).x + pLi2[11]*(RPixContner.at(j).x - pRLinar[0])*r22;
  626. point.y2 = RPixContner.at(j).y + pLi2[11]*(RPixContner.at(j).y - pRLinar[1])*r22;
  627. ///计算X，Y,Z的初值
  628. double * A = new double [12];
  629. double * L = new double [4];
  630. double * X = new double [4];
  631. A[0] = pLi1[0] + LPixContner.at(i).x*pLi1[8];
  632. A[1] = pLi1[1] + LPixContner.at(i).x*pLi1[9];
  633. A[2] = pLi1[2] + LPixContner.at(i).x*pLi1[10];
  635. A[3] = pLi1[4] + LPixContner.at(i).y*pLi1[10];
  636. A[4] = pLi1[5] + LPixContner.at(i).y*pLi1[10];
  637. A[5] = pLi1[6] + LPixContner.at(i).y*pLi1[10];
  639. A[6] = pLi2[0] + RPixContner.at(j).x*pLi2[8];
  640. A[7] = pLi2[1] + RPixContner.at(j).x*pLi2[9];
  641. A[8] = pLi2[2] + RPixContner.at(j).x*pLi2[10];
  643. L[0] = -pLi1[3] - LPixContner.at(i).x;
  644. L[1] = -pLi1[7] - LPixContner.at(i).y;
  645. L[2] = -pLi2[3] - RPixContner.at(j).x;
  646. inv(A,3);
  647. mult(A,L,X,3,3,1);
  648. point.X = X[0];
  649. point.Y = X[1];
  650. point.Z = X[2];
  652. CalCtrlPts.push_back(point);
  653. delete [] A;
  654. delete [] L;
  655. delete [] X;
  656. break;
  657. }
  658. }
  659. }
  660. int nCalPtsNum = CalCtrlPts.size();
  661. if(nCalPtsNum < 1) return;
  662. double * N = new double [4 * 3];
  663. double * Nt = new double [4 * 3];
  664. double * NtN = new double [3*3];
  665. double * Q = new double [4];
  666. double * NtQ = new double [3];
  667.     double * SS = new double [3];
  668. double * NSS = new double [4];
  669. double * V = new double [4];
  670. double * Vt = new double [4];
  671. double VtV,A1 ,A2;
  672. //逐点循环迭代求解并输出结果
  673. FILE * fp;
  674. fp = fopen(strResultFileName,"a+");
  675. fprintf(fp,"以下为同名点的大地坐标和精度n 点号       X              Y              Z            精度n");
  676. for (i=0;i<nCalPtsNum;i++)
  677. {
  678. //初始化参数
  679. int bFlag = 3,nCount=0;
  680. double X[3] = {CalCtrlPts.at(i).X, CalCtrlPts.at(i).Y,CalCtrlPts.at(i).Z};//保存计算结果用于比较
  681. while(bFlag)
  682. {
  683. bFlag = 3;
  684. nCount++;
  685. if (nCount == 11)
  686. {
  687. //CalCtrlPts.at(i).ID = -999;  
  688. break;
  689. }
  690. A1 = pLi1[8]*CalCtrlPts.at(i).X + pLi1[9]*CalCtrlPts.at(i).Y + pLi1[10]*CalCtrlPts.at(i).Z + 1;
  691. A2 = pLi2[8]*CalCtrlPts.at(i).X + pLi2[9]*CalCtrlPts.at(i).Y + pLi2[10]*CalCtrlPts.at(i).Z + 1;
  692. N[0] = -(pLi1[0] + pLi1[8]*CalCtrlPts.at(i).x1)/A1; 
  693. N[1] = -(pLi1[1] + pLi1[9]*CalCtrlPts.at(i).x1)/A1;
  694. N[2] = -(pLi1[2] + pLi1[10]*CalCtrlPts.at(i).x1)/A1;
  695. N[3] = -(pLi1[4] + pLi1[8]*CalCtrlPts.at(i).y1)/A1;
  696. N[4] = -(pLi1[5] + pLi1[9]*CalCtrlPts.at(i).y1)/A1;
  697. N[5] = -(pLi1[6] + pLi1[10]*CalCtrlPts.at(i).y1)/A1;
  699. N[6] = -(pLi2[0] + pLi2[8]*CalCtrlPts.at(i).x2)/A2; 
  700. N[7] = -(pLi2[1] + pLi2[9]*CalCtrlPts.at(i).x2)/A2;
  701. N[8] = -(pLi2[2] + pLi2[10]*CalCtrlPts.at(i).x2)/A2;
  702. N[9] = -(pLi2[4] + pLi2[8]*CalCtrlPts.at(i).y2)/A2;
  703. N[10] = -(pLi2[5] + pLi2[9]*CalCtrlPts.at(i).y2)/A2;
  704. N[11] = -(pLi2[6] + pLi2[10]*CalCtrlPts.at(i).y2)/A2;
  706. Q[0] = (pLi1[3] + CalCtrlPts.at(i).x1)/A1;
  707. Q[1] = (pLi1[7] + CalCtrlPts.at(i).y1)/A1;
  708. Q[2] = (pLi2[3] + CalCtrlPts.at(i).x2)/A2;
  709. Q[3] = (pLi2[7] + CalCtrlPts.at(i).y2)/A2;
  710. //计算结果
  711. transpose(N,Nt,4,3);
  712. mult(Nt,N,NtN,3,4,3);
  713. mult(Nt,Q,NtQ,3,4,1);
  714. inv(NtN,3);
  715. mult(NtN,NtQ,SS,3,3,1);
  716. for (int j=0;j<3;j++)
  717. {
  718. if (fabs(SS[j]-X[j])<0.001)
  719. bFlag --;
  720. X[j] = SS[j];//保存上一次计算结果，用比较
  721. }
  722. //保存结果
  723. CalCtrlPts.at(i).X = SS[0];
  724. CalCtrlPts.at(i).Y = SS[1];
  725. CalCtrlPts.at(i).Z = SS[2];
  726. }
  727. //计算中误差
  728. mult(N,SS,NSS,4,3,1);
  729. for (int j=0;j<4;j++)//计算NS + Q
  730. {
  731. V[j] = NSS[j] - Q[j];
  732. }
  733. transpose(V,Vt,4,1);
  734. mult(Vt,V,&VtV,1,4,1);
  735. CalCtrlPts.at(i).m = VtV;
  736. //output 
  737. if (nCount == 11)
  738.     fprintf(fp,"%d     %.6f     %.6f     %.6f    %.6f 迭代次数达10次，计算终止n",
  739. CalCtrlPts.at(i).ID,CalCtrlPts.at(i).X,CalCtrlPts.at(i).Y,CalCtrlPts.at(i).Z,CalCtrlPts.at(i).m);
  740. else
  741. fprintf(fp,"%d     %.6f     %.6f     %.6f   %.6fn",
  742. CalCtrlPts.at(i).ID,CalCtrlPts.at(i).X,CalCtrlPts.at(i).Y,CalCtrlPts.at(i).Z,CalCtrlPts.at(i).m);
  743. }
  744. fclose(fp);
  745. //释放临时内存空间
  746. delete [] N;
  747. delete [] Nt;
  748. delete [] NtN;
  749. delete [] Q;
  750. delete [] NtQ;
  751. delete [] SS;
  752. delete [] NSS;
  753. delete [] V;
  754. delete [] Vt;
  755. CalCtrlPts.clear();
  756. }