📄 备份.cpp
字号:
photo.time(LXY,lxy,coordinate_space,3,3,1);
photo.qiufu(coordinate_space,3,1);
// outfile<<" 近似坐标是："<<endl;
photo.display(coordinate_space,3,1);//输出物方空间坐标的近似值
// outfile<<coordinate_space[0][0]<<" "<<coordinate_space[1][0]<<" "<<coordinate_space[2][0]<<endl;
cout<<"*********************************"<<endl;
////////////////////////////// 3 ////////////////////////////////////////
double **M,**L,**W,**r,**A,**L1,**L2,**V;
double **ML;
double photo1_x0,photo1_y0,photo2_x0,photo2_y0,x0,y0,photo1_k1,photo2_k1;
double _A,_B,_C,old_fx,new_fx;
M=new double* [12];
for( i=0;i<12;i++)
M[i]=new double [12];
L=new double* [12];
for( i=0;i<12;i++)
L[i]=new double [1];
L1=new double* [11];
for( i=0;i<11;i++)
L1[i]=new double [1];
L2=new double* [11];
for( i=0;i<11;i++)
L2[i]=new double [1];
W=new double* [12];
for( i=0;i<12;i++)
W[i]=new double [1];
r=new double* [6];
for( i=0;i<6;i++)
r[i]=new double [1];
A=new double* [6];
for( i=0;i<6;i++)
A[i]=new double [1];
ML=new double* [12];
for( i=0;i<12;i++)
ML[i]=new double [1];
V=new double* [12];
for( i=0;i<12;i++)
V[i]=new double [1];
int ii=0;
new_fx=10000;//任取初始值以便循环
x0=-1*(photo1.L[0][0]*photo1.L[8][0]+photo1.L[1][0]*photo1.L[9][0]+photo1.L[2][0]*photo1.L[10][0])/(pow(photo1.L[8][0],2)+pow(photo1.L[9][0],2)+pow(photo1.L[10][0],2));
y0=-1*(photo1.L[4][0]*photo1.L[8][0]+photo1.L[5][0]*photo1.L[9][0]+photo1.L[6][0]*photo1.L[10][0])/(pow(photo1.L[8][0],2)+pow(photo1.L[9][0],2)+pow(photo1.L[10][0],2));
//cout<<x0<<" "<<y0<<endl;
do
{
ii++;
old_fx=new_fx; // cout<<old_fx<<endl;
for(i=0;i<=5;i++)
A[i][0]=photo1.L[8][0]*photo1.space[i][0]+photo1.L[9][0]*photo1.space[i][1]+photo1.L[10][0]*photo1.space[i][2]+1;
// cout<<"A"<<A[0][0]<<endl; //测试
for(i=0;i<=5;i++)
r[i][0]=sqrt( pow((photo1.zuobiaoyi[i][0]-x0),2) + pow((photo1.zuobiaoyi[i][1]-y0),2));
for(i=0;i<=5;i++) // 确定M
{
for(int j=0;j<=2;j++) //1 2 3列
{
M[2*i][j]=-1*photo1.space[i][j]/A[i][0];
M[2*i+1][j]=0;
}
M[2*i][3]=-1*1/A[i][0]; // 4列
M[2*i+1][3]=0;
for(j=4;j<=7;j++)// 5 6 7 8列
{
M[2*i][j]=0;
M[2*i+1][j]=M[2*i][j-4];
}
for(j=8;j<=10;j++)// 9 10 11列
{
M[2*i][j] =-1*photo1.zuobiaoyi[i][0]*photo1.space[i][j-8]/A[i][0];
M[2*i+1][j]=-1*photo1.zuobiaoyi[i][1]*photo1.space[i][j-8]/A[i][0];
}
M[2*i][11] =-1*(photo1.zuobiaoyi[i][0]-x0)*r[i][0]*r[i][0];// 12 列
M[2*i+1][11]=-1*(photo1.zuobiaoyi[i][1]-y0)*r[i][0]*r[i][0];
}
for(i=0;i<=5;i++) // 确定W
{
W[2*i][0] =-1*photo1.zuobiaoyi[i][0]/A[i][0];
W[2*i+1][0]=-1*photo1.zuobiaoyi[i][1]/A[i][0];
}
photo1.qiuni(M,M,12);
photo1.time(M,W,L,12,12,1);
photo1.qiufu(L,12,1);/////////////////////////解出新的L
photo1.qiuni(M,M,12);
photo1.time(M,L,ML,12,12,1);
photo1.plus(ML,W,V,12,1);
for(i=0;i<6;i++)//更新像平面坐标
for(int j=0;j<2;j++)
photo1.zuobiaoyi[i][j]+=V[2*i+j][0];
// cout<<"输出坐标仪坐标："<<endl;
// photo1.display(photo1.zuobiaoyi,6,2);
x0=-1*(L[0][0]*L[8][0]+L[1][0]*L[9][0]+L[2][0]*L[10][0])/(pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2));
y0=-1*(L[4][0]*L[8][0]+L[5][0]*L[9][0]+L[6][0]*L[10][0])/(pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2));
_A=( pow(L[0][0],2)+pow(L[1][0],2)+pow(L[2][0],2)) / ( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-pow(x0,2);
_B=( pow(L[4][0],2)+pow(L[5][0],2)+pow(L[6][0],2)) / ( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-pow(y0,2);
_C=( L[4][0]*L[0][0]+L[5][0]*L[1][0]+L[6][0]*L[2][0]) / ( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-y0*x0;
new_fx=sqrt((_A*_B-_C*_C)/_B);
for(i=0;i<11;i++)
photo1.L[i][0]=L[i][0];// 存放了最新的L
// cout<<"L1的迭代："<<ii<<" 次"<<"new_fx="<<new_fx<<" x0="<<x0<<" y0="<<y0<<endl;
// photo1.display(L,12,1);
}
while(fabs(new_fx-old_fx)>0.01);//毫米
for(i=0;i<11;i++)
L1[i][0]=L[i][0];// 存放了精确的L 第一张照片
photo1_x0=x0;
photo1_y0=y0;
photo1_k1=L[11][0];
// cout<<"L1精确值"<<endl;
// photo1.display(L1,11,1); //测试
// cout<<"************************************"<<endl;
ii=0;
new_fx=100000;//任取初始值以便循环
x0=-1*(photo2.L[0][0]*photo2.L[8][0]+photo2.L[1][0]*photo2.L[9][0]+photo2.L[2][0]*photo2.L[10][0])/(pow(photo2.L[8][0],2)+pow(photo2.L[9][0],2)+pow(photo2.L[10][0],2));
y0=-1*(photo2.L[4][0]*photo2.L[8][0]+photo2.L[5][0]*photo2.L[9][0]+photo2.L[6][0]*photo2.L[10][0])/(pow(photo2.L[8][0],2)+pow(photo2.L[9][0],2)+pow(photo2.L[10][0],2));
do
{
ii++;
old_fx=new_fx;
for(i=0;i<=5;i++)
A[i][0]=photo2.L[8][0]*photo2.space[i][0]+photo2.L[9][0]*photo2.space[i][1]+photo2.L[10][0]*photo2.space[i][2]+1;
for(i=0;i<=5;i++)
r[i][0]=sqrt( (photo2.zuobiaoyi[i][0]-x0)*(photo2.zuobiaoyi[i][0]-x0) + (photo2.zuobiaoyi[i][1]-y0)*(photo2.zuobiaoyi[i][1]-y0) );
for(i=0;i<=5;i++) // 确定M
{
for(int j=0;j<=2;j++) //1 2 3列
{
M[2*i][j]=-1*photo2.space[i][j]/A[i][0];
M[2*i+1][j]=0;
}
M[2*i][3]=-1*1/A[i][0]; // 4列
M[2*i+1][j]=0;
for(j=4;j<=7;j++)// 5 6 7 8列
{
M[2*i][j]=0;
M[2*i+1][j]=M[2*i][j-4];
}
for(j=8;j<=10;j++)// 9 10 11列
{
M[2*i][j] =-1*photo2.zuobiaoyi[i][0]*photo2.space[i][j-8]/A[i][0];
M[2*i+1][j]=-1*photo2.zuobiaoyi[i][1]*photo2.space[i][j-8]/A[i][0];
}
M[2*i][11] =-1*(photo2.zuobiaoyi[i][0]-x0)*r[i][0]*r[i][0];// 12 列
M[2*i+1][11]=-1*(photo2.zuobiaoyi[i][1]-y0)*r[i][0]*r[i][0];
}
for(i=0;i<=5;i++) // 确定W
{
W[2*i][0] =-1*photo2.zuobiaoyi[i][0]/A[i][0];
W[2*i+1][0]=-1*photo2.zuobiaoyi[i][1]/A[i][0];
}
photo2.qiuni(M,M,12);
photo2.time(M,W,L,12,12,1);/////////////////////////解出新的L
photo2.qiufu(L,12,1);
photo2.qiuni(M,M,12);
photo2.time(M,L,ML,12,12,1);
photo2.plus(ML,W,V,12,1);
for(i=0;i<6;i++)//更新像平面坐标
for(int j=0;j<2;j++)
photo2.zuobiaoyi[i][j]+=V[2*i+j][0];
x0=-1*(L[0][0]*L[8][0]+L[1][0]*L[9][0]+L[2][0]*L[10][0])/(pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2));
y0=-1*(L[4][0]*L[8][0]+L[5][0]*L[9][0]+L[6][0]*L[10][0])/(pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2));
_A=( pow(L[0][0],2)+pow(L[1][0],2)+pow(L[2][0],2)) / ( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-pow(x0,2);
_B=( pow(L[4][0],2)+pow(L[5][0],2)+pow(L[6][0],2)) / ( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-pow(y0,2);
_C=( L[4][0]*L[0][0]+L[5][0]*L[1][0]+L[6][0]*L[2][0]) /( pow(L[8][0],2)+pow(L[9][0],2)+pow(L[10][0],2))-y0*x0;
new_fx=sqrt((_A*_B-_C*_C)/_B);
for(i=0;i<11;i++)
photo2.L[i][0]=L[i][0];// 存放了最新的L
// cout<<"L2的迭代："<<ii<<" 次"<<"new_fx="<<new_fx<<" x0="<<x0<<" y0="<<y0<<endl;
// photo2.display(L,12,1);
}
while(fabs(new_fx-old_fx)>0.01);//毫米
for(i=0;i<11;i++)
L2[i][0]=L[i][0];// 存放了精确的L 第二张照片
photo2_x0=x0;
photo2_y0=y0;
photo2_k1=L[11][0];
// cout<<"L2精确值"<<endl;
// photo2.display(L2,11,1);
// cout<<"************************************"<<endl; //测试
////////////////////////////// 4 ////////////////////////////////////////
double **N,**Q,**S,**v;
double A1,A2;//两张相片各取一点，各自对应的A
double **Nt,**Nt_N,**NNN,**NS;//中间矩阵
N=new double* [4];
for( i=0;i<4;i++)
N[i]=new double [3];
Q=new double* [4];
for( i=0;i<4;i++)
Q[i]=new double [1];
NS=new double* [4];
for( i=0;i<4;i++)
NS[i]=new double [1];
v=new double* [4];
for( i=0;i<4;i++)
v[i]=new double [1];
S=new double* [3];
for( i=0;i<3;i++)
S[i]=new double [1];
Nt=new double* [3];
for( i=0;i<3;i++)
Nt[i]=new double [4];
Nt_N=new double* [3];
for( i=0;i<3;i++)
Nt_N[i]=new double [3];
NNN=new double* [3];
for( i=0;i<3;i++)
NNN[i]=new double [4];
/////////////////////////////////////////////////////////以下循环
coordinate_zuobiaoyi[0][0]+=photo1_k1*(coordinate_zuobiaoyi[0][0]-photo1_x0)*(pow((coordinate_zuobiaoyi[0][0]-photo1_x0),2)+pow((coordinate_zuobiaoyi[0][1]-photo1_y0),2));
coordinate_zuobiaoyi[0][1]+=photo1_k1*(coordinate_zuobiaoyi[0][1]-photo1_y0)*(pow((coordinate_zuobiaoyi[0][0]-photo1_x0),2)+pow((coordinate_zuobiaoyi[0][1]-photo1_y0),2));
coordinate_zuobiaoyi[1][0]+=photo2_k1*(coordinate_zuobiaoyi[1][0]-photo2_x0)*(pow((coordinate_zuobiaoyi[1][0]-photo2_x0),2)+pow((coordinate_zuobiaoyi[1][1]-photo2_y0),2));
coordinate_zuobiaoyi[1][1]+=photo2_k1*(coordinate_zuobiaoyi[1][1]-photo2_y0)*(pow((coordinate_zuobiaoyi[1][0]-photo2_x0),2)+pow((coordinate_zuobiaoyi[1][1]-photo2_y0),2));
// cout<<coordinate_zuobiaoyi[0][0]<<" "<<coordinate_zuobiaoyi[0][1]<<endl;
// cout<<coordinate_zuobiaoyi[1][0]<<" "<<coordinate_zuobiaoyi[1][1]<<endl;
S[0][0]=coordinate_space[0][0];
S[1][0]=coordinate_space[1][0];
S[2][0]=coordinate_space[2][0];
do
{
coordinate_space[0][0]=S[0][0]; //cout<<coordinate_space[0][0]<<endl;
coordinate_space[1][0]=S[1][0]; // cout<<coordinate_space[1][0]<<endl;
coordinate_space[2][0]=S[2][0]; // cout<<coordinate_space[2][0]<<endl;
A1=L1[8][0]*coordinate_space[0][0]+L1[9][0]*coordinate_space[1][0]+L1[10][0]*coordinate_space[2][0]+1;
A2=L2[8][0]*coordinate_space[0][0]+L2[9][0]*coordinate_space[1][0]+L2[10][0]*coordinate_space[2][0]+1;
N[0][0]=-1*(L1[0][0]+L1[8][0]*coordinate_zuobiaoyi[0][0])/A1; ///确定N
N[0][1]=-1*(L1[1][0]+L1[9][0]*coordinate_zuobiaoyi[0][0])/A1;
N[0][2]=-1*(L1[2][0]+L1[10][0]*coordinate_zuobiaoyi[0][0])/A1;
N[1][0]=-1*(L1[4][0]+L1[8][0]*coordinate_zuobiaoyi[0][1])/A1;
N[1][1]=-1*(L1[5][0]+L1[9][0]*coordinate_zuobiaoyi[0][1])/A1;
N[1][2]=-1*(L1[6][0]+L1[10][0]*coordinate_zuobiaoyi[0][1])/A1;
N[2][0]=-1*(L2[0][0]+L2[8][0]*coordinate_zuobiaoyi[1][0])/A2;
N[2][1]=-1*(L2[1][0]+L2[9][0]*coordinate_zuobiaoyi[1][0])/A2;
N[2][2]=-1*(L2[2][0]+L2[10][0]*coordinate_zuobiaoyi[1][0])/A2;
N[3][0]=-1*(L2[4][0]+L2[8][0]*coordinate_zuobiaoyi[1][1])/A2;
N[3][1]=-1*(L2[5][0]+L2[9][0]*coordinate_zuobiaoyi[1][1])/A2;
N[3][2]=-1*(L2[6][0]+L2[10][0]*coordinate_zuobiaoyi[1][1])/A2;
Q[0][0]=-1*(L1[3][0]+coordinate_zuobiaoyi[0][0])/A1;// 确定Q
Q[1][0]=-1*(L1[7][0]+coordinate_zuobiaoyi[0][1])/A1;
Q[2][0]=-1*(L2[3][0]+coordinate_zuobiaoyi[1][0])/A2;
Q[3][0]=-1*(L2[7][0]+coordinate_zuobiaoyi[1][1])/A2;
photo.zhuanzhi(N,Nt,4,3);
photo.time(Nt,N,Nt_N,3,4,3);
photo.qiuni(Nt_N,Nt_N,3);
photo.time(Nt_N,Nt,NNN,3,3,4);
photo.time(NNN,Q,S,3,4,1);
photo.qiufu(S,3,1);
photo.time(N,S,NS,4,3,1);
photo.plus(NS,Q,v,4,1);//更新待定点的像片坐标
coordinate_zuobiaoyi[0][0]+=v[0][0];
coordinate_zuobiaoyi[0][1]+=v[1][0];
coordinate_zuobiaoyi[1][0]+=v[2][0];
coordinate_zuobiaoyi[1][1]+=v[3][0];
}
while (fabs(S[0][0]-coordinate_space[0][0])>0.0001||fabs(S[1][0]-coordinate_space[1][0])>0.0001||fabs(S[2][0]-coordinate_space[2][0])>0.0001);
cout<<"同名相点对应的物方精确坐标是："<<endl;
cout<<"X: "<<S[0][0]<<" "<<"Y "<<S[1][0]<<" "<<"Z: "<<S[2][0]<<endl;
outfile<<" 精确坐标是："<<endl;
outfile<<" "<<S[0][0]<<" "<<S[1][0]<<" "<<S[2][0]<<endl;
outfile<<" 较差是：(毫米)"<<endl;
double x,y,z;
x=fabs(S[0][0]-kongjian[k][0])*1000;
y=fabs(S[1][0]-kongjian[k][1])*1000;
z=fabs(S[2][0]-kongjian[k][2])*1000;
if(x<0.1) x=0;
if(y<0.1) y=0;
if(z<0.1) z=0;
outfile<<" "<<x<<" "<<y<<" "<<z<<endl;
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -