📄 bundleadjustmentdlg.cpp
字号:
// BundleAdjustmentDlg.cpp : implementation file
//
#include "stdafx.h"
#include <iostream.h>
#include <math.h>
#include <stdio.h>
#include "BundleAdjust.h"
#include "BundleAdjustmentDlg.h"
#include "Matrix.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CBundleAdjustmentDlg dialog
CBundleAdjustmentDlg::CBundleAdjustmentDlg(CWnd* pParent /*=NULL*/)
: CDialog(CBundleAdjustmentDlg::IDD, pParent)
{
//{{AFX_DATA_INIT(CBundleAdjustmentDlg)
m_x0 = 0.0;
m_y0 = 0.0;
m_scale = 3000;
m_GcpFileIn = _T("");
m_ResultFileOut = _T("");
m_f = 303.64;
style=-1;
//}}AFX_DATA_INIT
}
void CBundleAdjustmentDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CBundleAdjustmentDlg)
DDX_Text(pDX, IDC_EDIT_x0, m_x0);
DDX_Text(pDX, IDC_EDIT_y0, m_y0);
DDX_Text(pDX, IDC_EDIT_SCALE, m_scale);
DDX_Text(pDX, IDC_EDIT_GcpFileIn, m_GcpFileIn);
DDX_Text(pDX, IDC_EDIT_ResultFileOut, m_ResultFileOut);
DDX_Text(pDX, IDC_EDIT_focal, m_f);
//}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CBundleAdjustmentDlg, CDialog)
//{{AFX_MSG_MAP(CBundleAdjustmentDlg)
ON_BN_CLICKED(IDC_BUTTON_IN, OnButtonIn)
ON_BN_CLICKED(IDC_BUTTON_Out, OnBUTTONOut)
ON_BN_CLICKED(IDC_RADIO_Hudu, OnRADIOHudu)
ON_BN_CLICKED(IDC_RADIO_Jiaodu, OnRADIOJiaodu)
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CBundleAdjustmentDlg message handlers
void CBundleAdjustmentDlg::OnButtonIn()
{
// TODO: Add your control notification handler code here
static char BASED_CODE file[]="TXT Files(*.TXT)|*.txt|所有文件 (*.*)|*.*|doc文件(*.doc)|*.doc||";
CFileDialog SelectFile(TRUE,NULL,NULL,OFN_HIDEREADONLY| OFN_OVERWRITEPROMPT,file,NULL);
SelectFile.DoModal();
CString FileName;
FileName=SelectFile.GetPathName();
m_GcpFileIn=FileName;
UpdateData(FALSE);
}
void CBundleAdjustmentDlg::OnBUTTONOut()
{
// TODO: Add your control notification handler code here
static char BASED_CODE file[]="TXT Files(*.TXT)|*.txt|所有文件 (*.*)|*.*||";
CFileDialog SelectFile(TRUE,NULL,NULL,OFN_HIDEREADONLY| OFN_OVERWRITEPROMPT,file,NULL);
SelectFile.DoModal();
CString FileName;
FileName=SelectFile.GetPathName();
m_ResultFileOut=FileName;
UpdateData(FALSE);
}
void CBundleAdjustmentDlg::OnOK()
{
// TODO: Add extra validation here
double lx[4],ly[4],lX[4],lY[4],lZ[4]; //左片四个控制点的像点坐标和物方空间坐标
double rx[4],ry[4],rX[4],rY[4],rZ[4]; //右片四个控制点的像点坐标和物方空间坐标
double lXs,lYs,lZs,lp,lw,lk; //左片外方位元素
double rXs,rYs,rZs,rp,rw,rk; //右片外方位元素
double lR[3][3],rR[3][3]; //左右片旋转矩阵
double V[16],A[16][12],At[12][16],X[12],L[16]; //改正数V，误差方程式未知数系数A和其转置At,未知数X，常数项L
double N[12][12],W[12]; //法方程系数矩阵N，常数项W
double lxt[4],lyt[4],lXt[4],lYt[4],lZt[4]; //左片过度值
double rxt[4],ryt[4],rXt[4],rYt[4],rZt[4]; //右片过度值
int i,j,k,s; //循环变量
int lflag,mflag,flag[12],wflag = -1; //标志量
int num=-1; //迭代次数
double m0,M[12],sum; //中误差和各个未知量的中误差
CString units; //角度的单位
//if语句，当不输入原始数据文件时，程序报错；输入原始数据文件时程序从文件读入数据
if((m_GcpFileIn == _T(""))||(m_ResultFileOut == _T("")))
{
lflag=0;
}
else lflag=1;
switch(lflag)
{
case 0:MessageBox("Error: Please input the data file !",NULL,MB_OK);break;
case 1:
{
FILE *fp;
fp=fopen(m_GcpFileIn,"r");
for(i=0;i<4;i++)
{
fscanf(fp,"%lf%lf%lf%lf%lf",&lx[i],&ly[i],&lX[i],&lY[i],&lZ[i]);
}
for(i=0;i<4;i++)
{
fscanf(fp,"%lf%lf%lf%lf%lf",&rx[i],&ry[i],&rX[i],&rY[i],&rZ[i]);
}
fclose(fp);
}
}
//像点坐标和像片内方位元素单位转化为米
for(i=0;i<4;i++)
{
lx[i]=lx[i]/1000;
ly[i]=ly[i]/1000;
rx[i]=rx[i]/1000;
ry[i]=ry[i]/1000;
}
m_x0=m_x0/1000;
m_y0=m_y0/1000;
m_f=m_f/1000;
//初始化未知量
lXs=(lX[0]+lX[1]+lX[2]+lX[3])/4;
lYs=(lY[0]+lY[1]+lY[2]+lY[3])/4;
lZs=m_f*m_scale;
rXs=(rX[0]+rX[1]+rX[2]+rX[3])/4;
rYs=(rY[0]+rY[1]+rY[2]+rY[3])/4;
rZs=m_f*m_scale;
lp=lw=lk=rp=rw=rk=0;
//do,while语句，当改正数在限值之内时结束迭代
do
{
num+=1;
//计算旋转矩阵
lR[0][0]=cos(lp)*cos(lk)-sin(lp)*sin(lw)*sin(lk);
lR[0][1]=-cos(lp)*sin(lk)-sin(lp)*sin(lw)*cos(lk);
lR[0][2]=-sin(lp)*cos(lw);
lR[1][0]=cos(lw)*sin(lk);
lR[1][1]=cos(lw)*cos(lk);
lR[1][2]=-sin(lw);
lR[2][0]=sin(lp)*cos(lk)+cos(lp)*sin(lw)*sin(lk);
lR[2][1]=-sin(lp)*sin(lk)+cos(lp)*sin(lw)*cos(lk);
lR[2][2]=cos(lp)*cos(lw);
rR[0][0]=cos(rp)*cos(rk)-sin(rp)*sin(rw)*sin(rk);
rR[0][1]=-cos(rp)*sin(rk)-sin(rp)*sin(rw)*cos(rk);
rR[0][2]=-sin(rp)*cos(rw);
rR[1][0]=cos(rw)*sin(rk);
rR[1][1]=cos(rw)*cos(rk);
rR[1][2]=-sin(rw);
rR[2][0]=sin(rp)*cos(rk)+cos(rp)*sin(rw)*sin(rk);
rR[2][1]=-sin(rp)*sin(rk)+cos(rp)*sin(rw)*cos(rk);
rR[2][2]=cos(rp)*cos(rw);
//计算共线方成过渡量
for(i=0;i<4;i++)
{
lXt[i]=lR[0][0]*(lX[i]-lXs)+lR[1][0]*(lY[i]-lYs)+lR[2][0]*(lZ[i]-lZs);
lYt[i]=lR[0][1]*(lX[i]-lXs)+lR[1][1]*(lY[i]-lYs)+lR[2][1]*(lZ[i]-lZs);
lZt[i]=lR[0][2]*(lX[i]-lXs)+lR[1][2]*(lY[i]-lYs)+lR[2][2]*(lZ[i]-lZs);
rXt[i]=rR[0][0]*(rX[i]-rXs)+rR[1][0]*(rY[i]-rYs)+rR[2][0]*(rZ[i]-rZs);
rYt[i]=rR[0][1]*(rX[i]-rXs)+rR[1][1]*(rY[i]-rYs)+rR[2][1]*(rZ[i]-rZs);
rZt[i]=rR[0][2]*(rX[i]-rXs)+rR[1][2]*(rY[i]-rYs)+rR[2][2]*(rZ[i]-rZs);
}
//计算像点坐标初值(x),(y)
for(i=0;i<4;i++)
{
lxt[i]=m_x0-m_f*lXt[i]/lZt[i];
lyt[i]=m_y0-m_f*lYt[i]/lZt[i];
rxt[i]=m_x0-m_f*rXt[i]/rZt[i];
ryt[i]=m_y0-m_f*rYt[i]/rZt[i];
}
//计算误差方程式中未知量的系数矩阵A和常数量L
//左片
for(i=0;i<8;i+=2)
{
s=i/2;
A[i][0]=(lR[0][0]*m_f+lR[0][2]*(lx[s]-m_x0))/lZt[s];
A[i][1]=(lR[1][0]*m_f+lR[1][2]*(lx[s]-m_x0))/lZt[s];
A[i][2]=(lR[2][0]*m_f+lR[2][2]*(lx[s]-m_x0))/lZt[s];
A[i][3]=(ly[s]-m_y0)*sin(lw)-((lx[s]-m_x0)*((lx[s]-m_x0)*cos(lk)-(ly[s]-m_y0)*sin(lk))/m_f+m_f*cos(lk))*cos(lw);
A[i][4]=-m_f*sin(lk)-(lx[s]-m_x0)*((lx[s]-m_x0)*sin(lk)+(ly[s]-m_y0)*cos(lk))/m_f;
A[i][5]=ly[s]-m_y0;
L[i]=lx[s]-lxt[s];
}
for(i=1;i<8;i+=2)
{
s=(i-1)/2;
A[i][0]=(lR[0][1]*m_f+lR[0][2]*(ly[s]-m_y0))/lZt[s];
A[i][1]=(lR[1][1]*m_f+lR[1][2]*(ly[s]-m_y0))/lZt[s];
A[i][2]=(lR[2][1]*m_f+lR[2][2]*(ly[s]-m_y0))/lZt[s];
A[i][3]=-(lx[s]-m_x0)*sin(lw)-((ly[s]-m_y0)*((lx[s]-m_x0)*cos(lk)-(ly[s]-m_y0)*sin(lk))/m_f-m_f*sin(lk))*cos(lw);
A[i][4]=-m_f*cos(lk)-(ly[s]-m_y0)*((lx[s]-m_x0)*sin(lk)+(ly[s]-m_y0)*cos(lk))/m_f;
A[i][5]=-(lx[s]-m_x0);
L[i]=ly[s]-lyt[s];
}
for(i=0;i<8;i++)
{
for(j=6;j<12;j++)
{
A[i][j]=0;
}
}
//右片
for(i=8;i<16;i++)
{
for(j=0;j<6;j++)
{
A[i][j]=0;
}
}
for(i=8;i<16;i+=2)
{
s=i/2-4;
A[i][6]=(rR[0][0]*m_f+rR[0][2]*(rx[s]-m_x0))/rZt[s];
A[i][7]=(rR[1][0]*m_f+rR[1][2]*(rx[s]-m_x0))/rZt[s];
A[i][8]=(rR[2][0]*m_f+rR[2][2]*(rx[s]-m_x0))/rZt[s];
A[i][9]=(ry[s]-m_y0)*sin(rw)-((rx[s]-m_x0)*((rx[s]-m_x0)*cos(rk)-(ry[s]-m_y0)*sin(rk))/m_f+m_f*cos(rk))*cos(rw);
A[i][10]=-m_f*sin(rk)-(rx[s]-m_x0)*((rx[s]-m_x0)*sin(rk)+(ry[s]-m_y0)*cos(rk))/m_f;
A[i][11]=ry[s]-m_y0;
L[i]=rx[s]-rxt[s];
}
for(i=9;i<16;i+=2)
{
s=(i-1)/2-4;
A[i][6]=(rR[0][1]*m_f+rR[0][2]*(ry[s]-m_y0))/rZt[s];
A[i][7]=(rR[1][1]*m_f+rR[1][2]*(ry[s]-m_y0))/rZt[s];
A[i][8]=(rR[2][1]*m_f+rR[2][2]*(ry[s]-m_y0))/rZt[s];
A[i][9]=-(rx[s]-m_x0)*sin(rw)-((ry[s]-m_y0)*((rx[s]-m_x0)*cos(rk)-(ry[s]-m_y0)*sin(rk))/m_f-m_f*sin(rk))*cos(rw);
A[i][10]=-m_f*cos(rk)-(ry[s]-m_y0)*((rx[s]-m_x0)*sin(rk)+(ry[s]-m_y0)*cos(rk))/m_f;
A[i][11]=-(rx[s]-m_x0);
L[i]=ry[s]-ryt[s];
}
//计算A的转置矩阵At
for(i=0;i<12;i++)
{
for(j=0;j<16;j++)
{
At[i][j]=A[j][i];
}
}
//计算法方程系数矩阵N和常数量W
for(i=0;i<12;i++)
{
for(j=0;j<12;j++)
{
N[i][j]=0;
for(k=0;k<16;k++)
{
N[i][j]+=At[i][k]*A[k][j];
}
}
}
for(i=0;i<12;i++)
{
W[i]=0;
for(j=0;j<16;j++)
{
W[i]+=At[i][j]*L[j];
}
}
//矩阵N的求逆
InverMatrix(N[0],12);
//求解未知量X
for(i=0;i<12;i++)
{
X[i]=0;
for(j=0;j<12;j++)
{
X[i]+=N[i][j]*W[j];
}
}
//求解各个未知量的改正数
for(i=0;i<16;i++)
{
for(j=0;j<12;j++)
V[i]=A[i][j]*X[j]-L[i];
}
lXs+=X[0];lYs+=X[1];lZs+=X[2];lp+=X[3];lw+=X[4];lk+=X[5];
rXs+=X[6];rYs+=X[7];rZs+=X[8];rp+=X[9];rw+=X[10];rk+=X[11];
//迭代条件
for(i=0;i<3;i++)
{
// if(fabs(X[i])>0.0001)flag[i]=0;
// else flag[i]=1;
flag[i]=1;
}
for(i=3;i<6;i++)
{
if(fabs(X[i])>3e-5)flag[i]=0;
else flag[i]=1;
}
for(i=6;i<9;i++)
{
// if(fabs(X[i])>0.0001)flag[i]=0;
// else flag[i]=1;
flag[i]=1;
}
for(i=9;i<12;i++)
{
if(fabs(X[i])>3e-5)flag[i]=0;
else flag[i]=1;
}
mflag=1;
for(i=0;i<12;i++)
{
mflag*=flag[i];
}
}while((mflag==0)&(num<5));
//计算中误差和各个未知量解算值的中误差
sum=0;
for(i=0;i<12;i++)
{
sum+=V[i]*V[i];
}
m0=sqrt(sum/4);
for(i=0;i<12;i++)
{
M[i]=m0*sqrt(N[i][i]);
}
//输出旋转角phi,omega,kappa的格式（角度制或弧度制）
switch(style)
{
case 0:
{
wflag=0;
units="rads";
}break;
case 1:
{
//弧度转化为角度
lp=Trans(lp);
lw=Trans(lw);
lk=Trans(lk);
rp=Trans(rp);
rw=Trans(rw);
rk=Trans(rk);
for(i=3;i<6;i++)
{
M[i]=Trans(M[i]);
}
for(i=9;i<12;i++)
{
M[i]=Trans(M[i]);
}
wflag=0;
units="degree minute second";
} break;
default:
{
MessageBox("Error: Haven't select the file style !",NULL,MB_OK);
wflag=1;
}
}
//输出程序解算结果
switch(wflag)
{
case 0:
{
FILE *fpt;
fpt=fopen(m_ResultFileOut,"w");
fprintf(fpt,"\n\t**********The Result Table**********\n\n\n");
fprintf(fpt,"Explanation:Units of linear elements(Xs,Ys,Zs) are meter,");
fprintf(fpt,"\n\t units of angler elements(phi,omega,kappa) are\n\t %s.",units);
fprintf(fpt,"\n\nNumber Of Iteration:%d\n",num);
fprintf(fpt,"RMS:%lf\n\n",m0);
fprintf(fpt,"\t**The Result Of The Left Image**\n\n");
fprintf(fpt,"1.Exterior Orientation Elements:\n");
fprintf(fpt,"\tXs=%.3lf\t",lXs);
fprintf(fpt,"Ys=%.3lf\t",lYs);
fprintf(fpt,"Zs=%.3lf\n",lZs);
fprintf(fpt,"\tphi=%.6lf\t",lp);
fprintf(fpt,"omega=%.6lf\t",lw);
fprintf(fpt,"kappa=%.6lf\n\n",lk);
fprintf(fpt,"2.RMS of Xs Ys Zs phi omega kappa:\n\t");
for(i=0;i<3;i++)
{
fprintf(fpt,"%lf\t",M[i]);
}
fprintf(fpt,"\n\t");
for(i=3;i<6;i++)
{
fprintf(fpt,"%lf\t",M[i]);
}
fprintf(fpt,"\n\n3.Rotation Matrix is:lR= %.5lf %.5lf %.5lf\n",lR[0][0],lR[0][1],lR[0][2]);
fprintf(fpt," %.5lf %.5lf %.5lf\n",lR[1][0],lR[1][1],lR[1][2]);
fprintf(fpt," %.5lf %.5lf %.5lf\n",lR[2][0],lR[2][1],lR[2][2]);
fprintf(fpt,"\n\t**The Result Of The Right Image**\n\n");
fprintf(fpt,"1.Exterior Orientation Elements:\n");
fprintf(fpt,"\tXs=%.3lf\t",rXs);
fprintf(fpt,"Ys=%.3lf\t",rYs);
fprintf(fpt,"Zs=%.3lf\n",rZs);
fprintf(fpt,"\tphi=%.6lf\t",rp);
fprintf(fpt,"omega=%.6lf\t",rw);
fprintf(fpt,"kappa=%.6lf\n\n",rk);
fprintf(fpt,"2.RMS of Xs Ys Zs phi omega kappa:\n\t");
for(i=6;i<9;i++)
{
fprintf(fpt,"%lf\t",M[i]);
}
fprintf(fpt,"\n\t");
for(i=9;i<12;i++)
{
fprintf(fpt,"%lf\t",M[i]);
}
fprintf(fpt,"\n\n3.Rotation Matrix is:rR= %.5lf %.5lf %.5lf\n",rR[0][0],rR[0][1],rR[0][2]);
fprintf(fpt," %.5lf %.5lf %.5lf\n",rR[1][0],rR[1][1],rR[1][2]);
fprintf(fpt," %.5lf %.5lf %.5lf\n",rR[2][0],rR[2][1],rR[2][2]);
fprintf(fpt,"\n\n\t********** The End **********\n");
fclose(fpt);
} break;
case 1:break;
default:break;
}
CDialog::OnOK();
}
void CBundleAdjustmentDlg::OnRADIOHudu()
{
// TODO: Add your control notification handler code here
style=0;
}
void CBundleAdjustmentDlg::OnRADIOJiaodu()
{
// TODO: Add your control notification handler code here
style=1;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -