📄 backwardsection_xiayudlg.cpp
字号:
while(nDiedaiNum<600 )//最多迭代600次
{
//计算旋转矩阵
rotation(pdfElement[FAI],pdfElement[OMIGA],pdfElement[KAMA],pdfR);
CString strNum ;
strNum.Format("第%d次迭代：",nDiedaiNum+1);
file.WriteString(strNum);
file.WriteString("\n");
//输出旋转矩阵到文件
file.WriteString("旋转矩阵R:");
file.WriteString("\n");
for( i=0;i<9;i++)
{
if( (i!=0) && ((i%3)==0) )
file.WriteString("\n");
CString strTemp;
strTemp.Format("%lf",pdfR[i]);
strTemp+= " ";
file.WriteString(strTemp);
}
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("\n");
double *pdfMeanX = new double[nCtrlPointNum];
double *pdfMeanY = new double[nCtrlPointNum];
double *pdfMeanZ = new double[nCtrlPointNum];
//逐点计算像点坐标近似值
for(int j=0;j<nCtrlPointNum;j++)
{
pdfMeanX[j] = pdfR[0]*((pGcp+j)->ptGroundCoor.dfX-pdfElement[X])
+pdfR[3]*((pGcp+j)->ptGroundCoor.dfY-pdfElement[Y])
+pdfR[6]*((pGcp+j)->ptGroundCoor.dfZ-pdfElement[Z]);
pdfMeanY[j] = pdfR[1]*((pGcp+j)->ptGroundCoor.dfX-pdfElement[X])
+pdfR[4]*((pGcp+j)->ptGroundCoor.dfY-pdfElement[Y])
+pdfR[7]*((pGcp+j)->ptGroundCoor.dfZ-pdfElement[Z]);
pdfMeanZ[j] = pdfR[2]*((pGcp+j)->ptGroundCoor.dfX-pdfElement[X])
+pdfR[5]*((pGcp+j)->ptGroundCoor.dfY-pdfElement[Y])
+pdfR[8]*((pGcp+j)->ptGroundCoor.dfZ-pdfElement[Z]);
double dfx = dfx0 - dffk*pdfMeanX[j]/pdfMeanZ[j];//像点坐标x近似值
double dfy = dfy0 - dffk*pdfMeanY[j]/pdfMeanZ[j];//像点坐标y近似值
//误差方程式子系数项
*(pdfa+j*12+0) = (pdfR[0]*dffk+pdfR[2]*(dfx-dfx0))/pdfMeanZ[j];
*(pdfa+j*12+1) = (pdfR[3]*dffk+pdfR[5]*(dfx-dfx0))/pdfMeanZ[j];
*(pdfa+j*12+2) = (pdfR[6]*dffk+pdfR[8]*(dfx-dfx0))/pdfMeanZ[j];
*(pdfa+j*12+3) = (dfy-dfy0)*sin(pdfElement[OMIGA])-
((dfx-dfx0)/dffk*((dfx-dfx0)*cos(pdfElement[KAMA])-(dfy-dfy0)*sin(pdfElement[KAMA]))+
dffk*cos(pdfElement[KAMA]))*cos(pdfElement[OMIGA]);
*(pdfa+j*12+4) = -1*dffk*sin(pdfElement[KAMA])-(dfx-dfx0)/dffk*((dfx-dfx0)*sin(pdfElement[KAMA])+(dfy-dfy0)*cos(pdfElement[KAMA]));
*(pdfa+j*12+5) = dfy-dfy0;
*(pdfa+j*12+6) = (pdfR[1]*dffk+pdfR[2]*(dfy-dfy0))/pdfMeanZ[j];
*(pdfa+j*12+7) = (pdfR[4]*dffk+pdfR[5]*(dfy-dfy0))/pdfMeanZ[j];
*(pdfa+j*12+8) = (pdfR[7]*dffk+pdfR[8]*(dfy-dfy0))/pdfMeanZ[j];
*(pdfa+j*12+9) = -1*(dfx-dfx0)*sin(pdfElement[OMIGA])-
((dfy-dfy0)/dffk*((dfx-dfx0)*cos(pdfElement[KAMA])-(dfy-dfy0)*sin(pdfElement[KAMA]))-
dffk*sin(pdfElement[KAMA]))*cos(pdfElement[OMIGA]);
*(pdfa+j*12+10) = -1*dffk*cos(pdfElement[KAMA])-(dfy-dfy0)/dffk*((dfx-dfx0)*sin(pdfElement[KAMA])+(dfy-dfy0)*cos(pdfElement[KAMA]));
*(pdfa+j*12+11) = -1*(dfx-dfx0);
//误差方程式子常数项
*(pdfL+j*2+0) = (pGcp+j)->ptImgCoor.dfX - dfx; //控制点影像坐标x - 影像坐标x的近似值
*(pdfL+j*2+1) = (pGcp+j)->ptImgCoor.dfY - dfy; //控制点影像坐标y - 影像坐标y的近似值
}
//...输出中间过程，供调试用...
file.WriteString("误差方程式系数矩阵A:");
file.WriteString("\n");
for(int i=0;i<2*nCtrlPointNum*6;i++)
{
if( (i!=0) && ((i%6)==0) )
file.WriteString("\n");
CString strTemp;
strTemp.Format("%lf",pdfa[i]);
strTemp+= " ";
file.WriteString(strTemp);
}
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("误差方程式常数项矩阵L:");
file.WriteString("\n");
for( i=0;i<2*nCtrlPointNum;i++)
{
if( (i!=0) && ((i%1)==0) )
file.WriteString("\n");
CString strTemp;
strTemp.Format("%lf",pdfL[i]);
strTemp+= " ";
file.WriteString(strTemp);
}
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("\n");
//求系数矩阵的转置
transpose(pdfa, pdfaT, 2*nCtrlPointNum, 6);
//求系数矩阵A的转置与A的乘积
mult(pdfaT,pdfa, pdfaMutaT, 6, 2*nCtrlPointNum, 6);
//求系数矩阵A的转置与A的乘积的逆
BOOL b = invers_matrix(pdfaMutaT,6);
//求系数矩阵A的转置与A的乘积的逆 与 A的转置的乘积
mult(pdfaMutaT,pdfaT,pdfBridge,6, 6,2*nCtrlPointNum);
//求系数矩阵A的转置与A的乘积的逆 与 A的转置的乘积 与常数项的乘积 得到最后的改正数
mult(pdfBridge,pdfL,Result,6, 2*nCtrlPointNum,1);
//输出每次迭代外方位元素改正数
file.WriteString("外方位元素改正数:");
file.WriteString("\n");
for( i=0;i<6;i++)
{
if( (i!=0) && ((i%1)==0) )
file.WriteString("\n");
CString strTemp;
strTemp.Format("%.15f",Result[i]);
strTemp+= " ";
file.WriteString(strTemp);
}
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("\n");
//迭代记数器
nDiedaiNum++;
//用未知数近似值与上次迭代计算的改正数之和作为新的近似值
pdfElement[X] += Result[0];
pdfElement[Y] += Result[1];
pdfElement[Z] += Result[2];
pdfElement[FAI] += Result[3];
pdfElement[OMIGA] += Result[4];
pdfElement[KAMA] += Result[5];
//限差小于阈值时，迭代结束，求得解
file.WriteString("外方位元素值:");
file.WriteString("\n");
for( i=0;i<6;i++)
{
if( (i!=0) && ((i%1)==0) )
file.WriteString("\n");
CString strTemp;
strTemp.Format("%.15f",pdfElement[i]);
strTemp+= " ";
file.WriteString(strTemp);
}
file.WriteString("\n");
file.WriteString("\n");
file.WriteString("\n");
//迭代结束条件，
if( (fabs(Result[3]*180/PI*60*60)<0.1) && (fabs(Result[4]*180/PI*60*60)<0.1)
&& (fabs(Result[5]*180/PI*60*60)<0.1) &&( fabs(Result[0]*180/PI*60*60)>0.06 ) )//注意fabs(Result[0]*180/PI*60*60)>0.06控制仅靠外方位线元素约束出现的非正常解，这个问题很有意思...夏宇,04.10.
{
CString strTip;
strTip = "解算过程及结果已经输出到文件";
strTip += m_strPathName;
strTip += "中";
AfxMessageBox(strTip);
bRight = TRUE;
break;
}
}
if(bRight == FALSE)
{
CString strTip;
strTip = "没有符合条件的解! 解算过程已经输出到文件";
strTip += m_strPathName;
strTip += "中";
AfxMessageBox(strTip);
}
file.Close();
CDialog::OnOK();
}
//计算旋转矩阵
void CBackWardSection_XiaYuDlg::rotation(double fai,double omiga,double kappa,double *R)
{
R[0]= cos(fai)*cos(kappa)-sin(fai)*sin(omiga)*sin(kappa);
R[1]= -cos(fai)*sin(kappa)-sin(fai)*sin(omiga)*cos(kappa);
R[2]= -sin(fai)*cos(omiga);
R[3]= cos(omiga)*sin(kappa);
R[4]= cos(omiga)*cos(kappa);
R[5]= -sin(omiga);
R[6]= sin(fai)*cos(kappa)+cos(fai)*sin(omiga)*sin(kappa);
R[7]= -sin(fai)*sin(kappa)+cos(fai)*sin(omiga)*cos(kappa);
R[8]= cos(fai)*cos(omiga);
}
//矩阵求逆
BOOL CBackWardSection_XiaYuDlg:: invers_matrix(double *m1,int n)
{
int *is,*js;
int i,j,k,l,u,v;
double temp,max_v;
is=(int *)malloc(n*sizeof(int));
js=(int *)malloc(n*sizeof(int));
if(is==NULL||js==NULL)
{
printf("out of memory!\n");
return(0);
}
for(k=0;k<n;k++)
{
max_v=0.0;
for(i=k;i<n;i++)
for(j=k;j<n;j++)
{
temp=fabs(m1[i*n+j]);
if(temp>max_v)
{
max_v=temp; is[k]=i; js[k]=j;
}
}
if(max_v==0.0)
{
free(is); free(js);
printf("invers is not availble!\n");
return(0);
}
if(is[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=is[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(js[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+js[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
l=k*n+k;
m1[l]=1.0/m1[l];
for(j=0;j<n;j++)
if(j!=k)
{
u=k*n+j;
m1[u]*=m1[l];
}
for(i=0;i<n;i++)
if(i!=k)
for(j=0;j<n;j++)
if(j!=k)
{
u=i*n+j;
m1[u]-=m1[i*n+k]*m1[k*n+j];
}
for(i=0;i<n;i++)
if(i!=k)
{
u=i*n+k;
m1[u]*=-m1[l];
}
}
for(k=n-1;k>=0;k--)
{
if(js[k]!=k)
for(j=0;j<n;j++)
{
u=k*n+j; v=js[k]*n+j;
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
if(is[k]!=k)
for(i=0;i<n;i++)
{
u=i*n+k; v=i*n+is[k];
temp=m1[u]; m1[u]=m1[v]; m1[v]=temp;
}
}
free(is); free(js);
return(1);
}
//矩阵转置
void CBackWardSection_XiaYuDlg::transpose(double *m1, double *m2, int m, int n)
{
int i,j;
for(i=0;i<m;i++)
for(j=0;j<n;j++)
m2[j*m+i]=m1[i*n+j];
return;
}
//矩阵相乘
void CBackWardSection_XiaYuDlg::mult(double *m1, double *m2, double *result, int i_1, int j_12, int j_2)
{
int i,j,k;
for(i=0;i<i_1;i++)
for(j=0;j<j_2;j++)
{
result[i*j_2+j]=0.0;
for(k=0;k<j_12;k++)
result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];
}
return ;
}
void CBackWardSection_XiaYuDlg::OnClickListgcp(NMHDR* pNMHDR, LRESULT* pResult)
{
// TODO: Add your control notification handler code here
CEditDlg dlg;
// dlg.m_dfX = ;
int nItem = 0;
POSITION pos = m_ListGcp.GetFirstSelectedItemPosition();
if (pos == NULL)
{
if(MessageBox("添加控制点?","后方交会",MB_OKCANCEL|MB_ICONQUESTION)==IDOK)
{
int nNum = m_ListGcp.GetItemCount();
m_ListGcp.InsertItem(nNum,"");//插入控制点记录
CString strText;
strText.Format("%d",nNum+1);
m_ListGcp.SetItem(nNum,0,LVIF_TEXT,strText,0,LVIF_STATE,0,0); //设置控制点编号
int nItemTemp = nNum-1;
CString strValue;
strValue = m_ListGcp.GetItemText(nItemTemp,1);
double m_dfX = atof(strValue);
strValue = m_ListGcp.GetItemText(nItemTemp,2);
double m_dfY = atof(strValue);
strValue = m_ListGcp.GetItemText(nItemTemp,3);
double m_dfZ = atof(strValue);
strValue = m_ListGcp.GetItemText(nItemTemp,4);
double m_dfImgx = atof(strValue);
strValue = m_ListGcp.GetItemText(nItemTemp,5);
double m_dfImgy = atof(strValue);
CString strConversion;
strConversion.Format("%lf",m_dfX);
m_ListGcp.SetItemText(nNum,1,strConversion);
strConversion.Format("%lf",m_dfY);
m_ListGcp.SetItemText(nNum,2,strConversion);
strConversion.Format("%lf",m_dfZ);
m_ListGcp.SetItemText(nNum,3,strConversion);
strConversion.Format("%lf",m_dfImgx);
m_ListGcp.SetItemText(nNum,4,strConversion);
strConversion.Format("%lf",m_dfImgy);
m_ListGcp.SetItemText(nNum,5,strConversion);
}
return;
}
else
{
while (pos)
{
nItem = m_ListGcp.GetNextSelectedItem(pos);
}
}
CString strText;
strText.Format("%d",nItem+1);
dlg.m_strVary = strText;
CString strValue;
strValue = m_ListGcp.GetItemText(nItem,1);
dlg.m_dfX = atof(strValue);
strValue = m_ListGcp.GetItemText(nItem,2);
dlg.m_dfY = atof(strValue);
strValue = m_ListGcp.GetItemText(nItem,3);
dlg.m_dfZ = atof(strValue);
strValue = m_ListGcp.GetItemText(nItem,4);
dlg.m_dfImgx = atof(strValue);
strValue = m_ListGcp.GetItemText(nItem,5);
dlg.m_dfImgy = atof(strValue);
// GetDlgItem(IDC_STATICVARY)->SetWindowText(strText);
if(dlg.DoModal()==IDOK)
{
CString strConversion;
strConversion.Format("%lf",dlg.m_dfX);
m_ListGcp.SetItemText(nItem,1,strConversion);
strConversion.Format("%lf",dlg.m_dfY);
m_ListGcp.SetItemText(nItem,2,strConversion);
strConversion.Format("%lf",dlg.m_dfZ);
m_ListGcp.SetItemText(nItem,3,strConversion);
strConversion.Format("%lf",dlg.m_dfImgx);
m_ListGcp.SetItemText(nItem,4,strConversion);
strConversion.Format("%lf",dlg.m_dfImgy);
m_ListGcp.SetItemText(nItem,5,strConversion);
}
*pResult = 0;
}
void CBackWardSection_XiaYuDlg::OnBeginlabeleditListgcp(NMHDR* pNMHDR, LRESULT* pResult)
{
LV_DISPINFO* pDispInfo = (LV_DISPINFO*)pNMHDR;
// TODO: Add your control notification handler code here
*pResult = 0;
}
//函数名：OnAction
//参数：无
//功能：输出结果文件
void CBackWardSection_XiaYuDlg::OnAction()
{
CFileDialog dlg(FALSE,"txt",NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT,"TXT Files (*.txt)|*.txt||",NULL );
if(dlg.DoModal()==IDOK)
{
m_strPathName = dlg.GetPathName();
}
UpdateData(FALSE);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -