?? rearmeet.cpp
字號:
//RearMeet.cpp: implementation of the CRearMeet class.
////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "RearMeet.h"
#include "math.h"
#define PI 3.14159265358979
////////////////////////////////////////////////////////////////////
//Construction/Destruction
////////////////////////////////////////////////////////////////////
CRearMeet::CRearMeet()
{
}
CRearMeet::~CRearMeet()
{
}
/*
功能：構造函數
參數：df：主距； dm：攝影比例尺分母； in：控制點個數； x0 y0：像主點在框標坐標系的坐標；
maPointXYZ：相片地面點坐標值； dk：航片旋轉角； dw：旁向偏角； da：航向偏角
返回值：
說明：
*/
CRearMeet::CRearMeet(double df, double dm, int in, double dx0, double dy0, CMatrix maPointxyXYZ)
{
m_pDataFIlE = fopen("Data.txt", "w");
m_iCount = 0;
m_df = df;
m_dm = dm;
m_iPointNum = in;
m_dx0 = dx0;
m_dy0 = dy0;
m_maPointxyXYZ = maPointxyXYZ;
m_maR.Initate(3,3);
m_maB.Initate(2*m_iPointNum,6);
m_maL.Initate(2*m_iPointNum,1);
m_maX0.Initate(6,1);
m_madX.Initate(6,1);
m_maX.Initate(6,1);
m_maV.Initate(2*m_iPointNum,1);
Initate();
UpdateRMatrix();
UpdateBMatrix();
UpdateLMatrix();
}
/*
功能：初始化函數
參數：df：主距； dm：攝影比例尺分母； in：控制點個數； x0 y0：像主點在框標坐標系的坐標；
maPointXYZ：相片地面點坐標值； dk：航片旋轉角； dw：旁向偏角； da：航向偏角
返回值：成功返回 1
說明：
*/
int CRearMeet::Initate()
{
m_da = 0;
m_dw = 0;
m_dk = 0;
m_dXSYSZS[2] = m_df * m_dm;
m_dXSYSZS[0] = m_dXSYSZS[1] = 0.0;
double* pdPoint = m_maPointxyXYZ.GetData();
int iLine = m_maPointxyXYZ.GetLine();
for(int i=0; i<m_iPointNum; i++)
{
m_dXSYSZS[0] += pdPoint[i*iLine+2];
m_dXSYSZS[1] += pdPoint[i*iLine+3];
}
m_dXSYSZS[0] /= m_iPointNum;
m_dXSYSZS[1] /= m_iPointNum;
double* pdX0 = m_maX0.GetData();
pdX0[0] = m_dXSYSZS[0];
pdX0[1] = m_dXSYSZS[1];
pdX0[2] = m_dXSYSZS[2];
pdX0[3] = m_da;
pdX0[4] = m_dw;
pdX0[5] = m_dk;
return 1;
}
/*
功能：主要的計算函數
參數：無
返回值：計算成功返回 1， 失敗返回0
說明：
*/
int CRearMeet::Calculate()
{
m_iCount = 0;
int iResult = 0;
do
{
m_iCount++;
// double* pdmaV = m_maV.GetData();
// double* pdmaPoint = m_maPointxyXYZ.GetData();
// int imaPointLine = m_maPointxyXYZ.GetLine();
// for(int i=0; i<m_iPointNum; i++)
// {
// pdmaPoint[i*imaPointLine] += pdmaV[2*i];
// pdmaPoint[i*imaPointLine+1] += pdmaV[2*i+1];
// }
if(m_iCount != 1)
{
double* pdX0 = m_maX0.GetData();
m_dXSYSZS[0] = pdX0[0];
m_dXSYSZS[1] = pdX0[1];
m_dXSYSZS[2] = pdX0[2];
m_da = pdX0[3];
m_dw = pdX0[4];
m_dk = pdX0[5];
UpdateRMatrix();
UpdateBMatrix();
UpdateLMatrix();
}
CMatrix maBT = m_maB;
fprintf(m_pDataFIlE, "第%d次迭代\n", m_iCount);
fprintf(m_pDataFIlE, "-----------m_maB------------\n");
FileOperate(m_maB);
fprintf(m_pDataFIlE, "-----------m_maL------------\n");
FileOperate(m_maL);
maBT.transpose();
CMatrix maBTB = maBT*m_maB;
CMatrix maConveBTB = maBTB.GetConvertMatrix();
m_maDx = maConveBTB;
fprintf(m_pDataFIlE, "-----------m_maDx------------\n");
FileOperate(m_maDx);
m_madX = maConveBTB*maBT*m_maL;
m_maV = m_maB*m_madX - m_maL;
m_maX0 = m_maX0 + m_madX;
iResult = Judge();
fprintf(m_pDataFIlE, "-----------m_maV------------\n");
FileOperate(m_maV);
}
while(iResult == -1&& m_iCount <= 5);
if(m_iCount == 6) //計算不成功
return 0;
printf("%d\n", m_iCount);
m_maV = m_maB*m_madX - m_maL;
CMatrix maVT = m_maV;
maVT.transpose();
double* pdSigma;
CMatrix maVTPV = maVT*m_maV;
pdSigma = (maVTPV).GetData();
m_dSigma = sqrt(pdSigma[0]/(2*m_iPointNum-6));
fprintf(m_pDataFIlE, "---------------Sigma-----------\n");
fprintf(m_pDataFIlE, "%.18lf", m_dSigma);
m_maX = m_maX0;
double* pdX = m_maX.GetData();
m_dXSYSZS[0] = pdX[0];
m_dXSYSZS[1] = pdX[1];
m_dXSYSZS[2] = pdX[2];
m_da = pdX[3];
m_dw = pdX[4];
m_dk = pdX[5];
UpdateRMatrix();
double* pdmaDx = m_maDx.GetData();
printf("%.8lf, %.8lf, %.8lf, %.8lf, %.8lf, %.8lf\n",
sqrt(pdmaDx[0])*m_dSigma,sqrt(pdmaDx[7])*m_dSigma,sqrt(pdmaDx[14])*m_dSigma,
sqrt(pdmaDx[21])*m_dSigma,sqrt(pdmaDx[28])*m_dSigma,sqrt(pdmaDx[35])*m_dSigma);
m_maR.DisplayMatrix();
// printf("%d\n",iCount);
ResultOutPut();
return 1;
}
int CRearMeet::UpdateBMatrix()
{
double* pdB = m_maB.GetData();
int ipdBLine = m_maB.GetLine();
double* pdR = m_maR.GetData();
int ipdRLine = m_maR.GetLine();
double* pdXYZ = m_maPointxyXYZ.GetData();
int ipdXYZLine = m_maPointxyXYZ.GetLine();
double a_cos = cos(m_da);
double a_sin = sin(m_da);
double w_cos = cos(m_dw);
double w_sin = sin(m_dw);
double k_cos = cos(m_dk);
double k_sin = sin(m_dk);
double dZ = 0.0;
double dx = 0.0;
double dy = 0.0;
for(int i=0; i<m_iPointNum; i++)
{
// dx = pdXYZ[i*ipdXYZLine+0];
// dy = pdXYZ[i*ipdXYZLine+1];
dZ = pdR[0*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) +
pdR[1*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
pdR[2*ipdRLine+2]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);
dx = pdR[0*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) +
pdR[1*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
pdR[2*ipdRLine+0]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);
dy = pdR[0*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+2]-m_dXSYSZS[0]) +
pdR[1*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+3]-m_dXSYSZS[1]) +
pdR[2*ipdRLine+1]*(pdXYZ[i*ipdXYZLine+4]-m_dXSYSZS[2]);
dx = m_dx0 - m_df*dx/dZ;
dy = m_dy0 - m_df*dy/dZ;
int i1 = i*2;
pdB[i1*ipdBLine+0] = (pdR[0*ipdRLine+0]*m_df + pdR[0*ipdRLine+2]*(dx-m_dx0))/dZ;
pdB[i1*ipdBLine+1] = (pdR[1*ipdRLine+0]*m_df + pdR[1*ipdRLine+2]*(dx-m_dx0))/dZ;
pdB[i1*ipdBLine+2] = (pdR[2*ipdRLine+0]*m_df + pdR[2*ipdRLine+2]*(dx-m_dx0))/dZ;
pdB[i1*ipdBLine+3] = (dy-m_dy0)*w_sin -
w_cos*( m_df*k_cos+(dx-m_dx0)*((dx-m_dx0)
*k_cos-(dy-m_dy0)*k_sin)/m_df );
pdB[i1*ipdBLine+4] = -m_df*k_sin -
(dx-m_dx0)*((dx-m_dx0)*k_sin+(dy-m_dy0)*k_cos)/m_df;
pdB[i1*ipdBLine+5] = dy - m_dy0;
int i2 = i1 + 1;
pdB[i2*ipdBLine+0] = (pdR[0*ipdRLine+1]*m_df + pdR[0*ipdRLine+2]*(dy-m_dy0))/dZ;
pdB[i2*ipdBLine+1] = (pdR[1*ipdRLine+1]*m_df + pdR[1*ipdRLine+2]*(dy-m_dy0))/dZ;
pdB[i2*ipdBLine+2] = (pdR[2*ipdRLine+1]*m_df + pdR[2*ipdRLine+2]*(dy-m_dy0))/dZ;
pdB[i2*ipdBLine+3] = -(dx-m_dx0)*w_sin -
w_cos*( -m_df*k_sin+(dy-m_dy0)*((dx-m_dx0)
*k_cos-(dy-m_dy0)*k_sin)/m_df );
pdB[i2*ipdBLine+4] = -m_df*k_cos -
(dy-m_dy0)*((dx-m_dx0)*k_sin+(dy-m_dy0)*k_cos)/m_df;
pdB[i2*ipdBLine+5] = m_dx0 - dx;
}
return 1;
}
int CRearMeet::UpdateLMatrix()
{
double* pdL = m_maL.GetData();
double* pdxyXYZ = m_maPointxyXYZ.GetData();
int ipdXYZLine = m_maPointxyXYZ.GetLine();
double dX = 0.0;
double dY = 0.0;
double dZ = 0.0;
double dXYZXYZS[3] = {0.0};
CMatrix maXYZXYZS;
for(int i=0; i<m_iPointNum; i++)
{
dXYZXYZS[0] = pdxyXYZ[i*ipdXYZLine+2] - m_dXSYSZS[0];
dXYZXYZS[1] = pdxyXYZ[i*ipdXYZLine+3] - m_dXSYSZS[1];
dXYZXYZS[2] = pdxyXYZ[i*ipdXYZLine+4] - m_dXSYSZS[2];
maXYZXYZS.CreateData(dXYZXYZS, 3, 1);
CMatrix maRTra = m_maR;
maRTra.transpose();
CMatrix maXYZ = maRTra*maXYZXYZS;
double* pdXYZ = maXYZ.GetData();
int i1 = i*2;
pdL[i1] = pdxyXYZ[i*ipdXYZLine+0] - (-m_df*pdXYZ[0]/pdXYZ[2] + m_dx0) ;
pdL[i1+1] = pdxyXYZ[i*ipdXYZLine+1] - (-m_df*pdXYZ[1]/pdXYZ[2] + m_dy0) ;
}
return 1;
}
int CRearMeet::UpdateRMatrix()
{
double* pdR = m_maR.GetData();
int iLine = m_maR.GetLine();
double a_cos = cos(m_da);
double a_sin = sin(m_da);
double w_cos = cos(m_dw);
double w_sin = sin(m_dw);
double k_cos = cos(m_dk);
double k_sin = sin(m_dk);
pdR[0] = a_cos*k_cos - a_sin*w_sin*k_sin;
pdR[1] = - a_cos*k_sin - a_sin*w_sin*k_cos;
pdR[2] = - a_sin * w_cos;
pdR[3] = w_cos*k_sin;
pdR[4] = w_cos*k_cos;
pdR[5] = - w_sin;
pdR[6] = a_sin*k_cos + a_cos*w_sin*k_sin;
pdR[7] = - a_sin*k_sin + a_cos*w_sin*k_cos;
pdR[8] = a_cos * w_cos;
return 1;
}
int CRearMeet::Judge()
{
double* pdDx = m_madX.GetData();
int iLine = m_madX.GetLine();
double dTriError = 3e-5;
if(fabs(pdDx[0])<=0.01 && fabs(pdDx[1])<=0.01 && fabs(pdDx[2])<=0.01 &&
fabs(pdDx[3])<=dTriError && fabs(pdDx[4])<=dTriError && fabs(pdDx[5])<=dTriError)
return 1;
else
return -1;
}
void CRearMeet::Display()
{
double* pdX = m_maX.GetData();
for(int i=0; i<6; i++)
printf("%10.5lf ",pdX[i]);
printf("\n");
}
void CRearMeet::FileOperate(CMatrix maOutput)
{
int iRow = maOutput.GetRow();
int iLine = maOutput.GetLine();
double* pdma = maOutput.GetData();
for(int i=0; i<iRow; i++)
{
for(int j=0; j<iLine; j++)
fprintf(m_pDataFIlE, "%20.7lf ", pdma[i*iLine+j]);
fprintf(m_pDataFIlE, "\n");
}
fprintf(m_pDataFIlE, "\n");
}
void CRearMeet::ResultOutPut()
{
fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
fprintf(m_pDataFIlE, " 空間后方交會計算\n");
fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
fprintf(m_pDataFIlE, " 影像坐標 地面坐標\n");
fprintf(m_pDataFIlE, "-----------------------------------------------------\n");
fprintf(m_pDataFIlE, "-------x(mm)-----y(mm)---|----X(m)------Y(m)------Z(m)-------\n");
double* pfmaxyXYZ = m_maPointxyXYZ.GetData();
for(int i=0; i<4; i++)
{
fprintf(m_pDataFIlE, " %8.2lf %8.2lf | %8.2lf %8.2lf %8.2lf\n",
pfmaxyXYZ[i*5]*1000,pfmaxyXYZ[i*5+1]*1000,pfmaxyXYZ[i*5+2],pfmaxyXYZ[i*5+3],pfmaxyXYZ[i*5+4]);
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -