?? 攝影測量最終程序.txt
字號:
#include<iostream>
#include<iomanip>
#include<fstream>
#include<cmath>
using namespace std;
int main()
{
ofstream table("E:\\121011.txt",ios_base::app);
int i,j,r,rr=0;
long double xy[4][2]={{-86.15,-68.99},
{-53.40,82.21},
{-14.78,-76.63},
{10.46,64.43}};
long double XY[4][3]={{36589.41,25273.32,2195.17},
{37631.08,31324.51,728.69},
{39100.97,24934.98,2386.50},
{40426.54,30319.81,757.31}};
const int a=8,b=6;
long double f=153.24,m=10000,a1,b1,c1,a2,b2,c2,a3,b3,c3,
XAX,YAY,ZAZ,x,y,L[8][1]={0.0};
long double p=0,w=0,k=0;//定義角度元素
long double c[b][b]={0.0},A[a][b]={0.0},AA[b][b]={0.0};//d[b][1]為改正數
long double Xs,Ys,Zs,ip=206265;
Xs=(XY[0][0]+XY[1][0]+XY[2][0]+XY[3][0])/4.0;
Ys=(XY[0][1]+XY[1][1]+XY[2][1]+XY[3][1])/4.0;
Zs=m*f/1000+(XY[0][2]+XY[1][2]+XY[2][2]+XY[3][2])/4.0;
do
{
long double AA[b][b]={0.0};
table<<"cout six number:"<<endl;
table<<"Xs "<<setprecision(8)<<setw(14)<<Xs<<endl;
table<<"Ys "<<setprecision(8)<<setw(14)<<Ys<<endl;
table<<"Zs "<<setprecision(8)<<setw(14)<<Zs<<endl;
table<<"p "<<setprecision(8)<<setw(14)<<p<<endl;
table<<"w "<<setprecision(8)<<setw(14)<<w<<endl;
table<<"k "<<setprecision(8)<<setw(14)<<k<<endl;
table<<"\n\n\n\n\n";
a1=cos(p)*cos(k)-sin(p)*sin(w)*sin(k);
b1=cos(w)*sin(k);
c1=sin(p)*cos(k)+cos(p)*sin(w)*sin(k);
a2=-cos(p)*sin(k)-sin(p)*sin(w)*cos(k);
b2=cos(w)*cos(k);
c2=-sin(p)*sin(k)+cos(p)*sin(w)*cos(k);
a3=-sin(p)*cos(w);
b3=-sin(w);
c3=cos(p)*cos(w);
table<<"輸出九個系數:\n";
table<<"a1 "<<setprecision(8)<<setw(14)<<a1<<endl;
table<<"a2 "<<setprecision(8)<<setw(14)<<a2<<endl;
table<<"a3 "<<setprecision(8)<<setw(14)<<a3<<endl;
table<<"b1 "<<setprecision(8)<<setw(14)<<b1<<endl;
table<<"b2 "<<setprecision(8)<<setw(14)<<b2<<endl;
table<<"b3 "<<setprecision(8)<<setw(14)<<b3<<endl;
table<<"c1 "<<setprecision(8)<<setw(14)<<c1<<endl;
table<<"c2 "<<setprecision(8)<<setw(14)<<c2<<endl;
table<<"c3 "<<setprecision(8)<<setw(14)<<c3<<endl;
table<<"\n\n\n\n\n\n\n";
for(i=0,j=0;(i<8&&j<4);i+=2,j++)
{
XAX=a1*(XY[j][0]-Xs)+b1*(XY[j][1]-Ys)+c1*(XY[j][2]-Zs);
YAY=a2*(XY[j][0]-Xs)+b2*(XY[j][1]-Ys)+c2*(XY[j][2]-Zs);
ZAZ=a3*(XY[j][0]-Xs)+b3*(XY[j][1]-Ys)+c3*(XY[j][2]-Zs);
table<<"XAX: "<<setprecision(8)<<setw(14)<<XAX<<endl;
table<<"YAY: "<<setprecision(8)<<setw(14)<<YAY<<endl;
table<<"ZAZ: "<<setprecision(8)<<setw(14)<<ZAZ<<endl;
x=-f*XAX/ZAZ;//x的新值
y=-f*YAY/ZAZ;//y的新值
//table<<"x: "<<setprecision(8)<<setw(14)<<x<<endl;
//table<<"y: "<<setprecision(8)<<setw(14)<<y<<endl;
L[i][0]=xy[j][0]-x;
L[i+1][0]=xy[j][1]-y;
table<<"L[i][0]: "<<setprecision(8)<<setw(14)<<L[i][0]<<endl;
table<<"L[i+1][0]: "<<setprecision(8)<<setw(14)<<L[i+1][0]<<endl;
A[i][0]=(a1*f+a3*xy[j][0])/ZAZ;
A[i][1]=(b1*f+b3*xy[j][0])/ZAZ;
A[i][2]=(c1*f+c3*xy[j][0])/ZAZ;
A[i][3]=xy[j][1]*sin(w)-(xy[j][0]*(xy[j][0]*cos(k)-xy[j][1]*sin(k))/f+f*cos(k))*cos(w);
A[i][4]=-f*sin(k)-xy[j][0]*(xy[j][0]*sin(k)+xy[j][1]*cos(k))/f;
A[i][5]=xy[j][1];
A[i+1][0]=(a2*f+a3*xy[j][1])/ZAZ;
A[i+1][1]=(b2*f+b3*xy[j][1])/ZAZ;
A[i+1][2]=(c2*f+c3*xy[j][1])/ZAZ;
A[i+1][3]=-xy[j][0]*sin(w)-(xy[j][1]*(xy[j][0]*cos(k)-xy[j][1]*sin(k))/f-f*sin(k))*cos(w);
A[i+1][4]=-f*cos(k)-xy[j][1]*(xy[j][0]*sin(k)+xy[j][1]*cos(k))/f;
A[i+1][5]=-xy[j][0];
}
table<<"\nA[i][j]:\n";
for(i=0;i<a;i++)
for(j=0;j<b;j++)
{
table<<setprecision(8)<<setw(18)<<A[i][j];
if(j==b-1) table<<endl;
}
table<<"\n\n\n";
for(i=0;i<b;i++)
for(j=0;j<b;j++)
for(r=0;r<a;r++)
AA[i][j]+=A[r][i]*A[r][j];//AA[i][j]=A^TA
//for(i=0;i<b;i++)
// for(j=0;j<b;j++)
//AA[i][j]=xs*AA[i][j];
table<<"\nAA[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<b;j++)
{
table<<setprecision(8)<<setw(18)<<AA[i][j];
if(j==b-1) table<<endl;
}
table<<"\n\n\n\n";
//A^TA結束
//第一次矩陣相乘結束,下面對AA求逆
//矩陣求逆程序片斷開始
int h,g;
long double pp;
long double q[b][2*b]={0.0};
for(i=0;i<b;i++)//構造高斯矩陣
for(j=0;j<b;j++)
{
q[i][j]=AA[i][j];
}
table<<"\n1:q[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<2*b;j++)
{
table<<" "<<setprecision(8)<<setw(18)<<q[i][j];
if(j==2*b-1) table<<endl;
}
table<<"\n\n\n\n\n";
for(i=0;i<b;i++)
for(j=b;j<2*b;j++)
{
if(i+b==j) q[i][j]=1;
else q[i][j]=0;
}
table<<"\n2:q[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<2*b;j++)
{
table<<" "<<setprecision(8)<<setw(18)<<q[i][j];
if(j==2*b-1) table<<endl;
}
cout<<"\n\n\n\n\n\n";
for(h=g=0;h<b-1;h++,g++)//消去對角線以下的數據
for(i=h+1;i<b;i++)
{
for(j=i;j<b;j++)
{
if((q[h][h]==0)&&(q[j][h]!=0))
{
for(r=h;r<2*b;r++)
{
long double RR;
RR=q[h][r];
q[h][r]=q[j][r];
q[j][r]=RR;
}
break;
}
}
if(q[i][g]==0)
continue;
pp=q[h][g]/q[i][g];
for(j=g;j<2*b;j++)
{
q[i][j]*=pp;
q[i][j]-=q[h][j];
}
}
table<<"\n3:q[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<2*b;j++)
{
table<<" "<<setprecision(8)<<setw(18)<<q[i][j];
if(j==2*b-1) table<<endl;
}
table<<"\n\n\n\n\n\n";
for(h=g=b-1;g>0;g--,h--) // 消去對角線以上的數據
for(i=g-1;i>=0;i--)
{
if(q[i][h]==0)
continue;
pp=q[h][g]/q[i][g];
for(j=0;j<2*b;j++)
{
q[i][j]*=pp;
q[i][j]-=q[g][j];
}
}
table<<"\n3---4:q[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<2*b;j++)
{
table<<setprecision(8)<<setw(18)<<q[i][j];
if(j==2*b-1) table<<endl;
}
table<<"\n\n\n\n\n\n";
for(i=0;i<b;i++) //將對角線上數據化為1
{
pp=1.0/q[i][i];
for(j=0;j<2*b;j++)
q[i][j]*=pp;
}
table<<"\n4:q[i][j]:\n";
for(i=0;i<b;i++)
for(j=0;j<2*b;j++)
{
table<<setprecision(8)<<setw(18)<<q[i][j];
if(j==(2*b-1)) table<<endl;
}
table<<"\n\n\n\n\n\n";
for(i=0;i<b;i++) //提取逆矩陣并除xs
for(j=0;j<b;j++)
c[i][j]=q[i][j+b];
//矩陣求逆程序片斷結束
//(A^TA)-1結束,此時C=(A^A)-1
table<<"\nc[i][j]:\n\n";
for(i=0;i<b;i++)
for(j=0;j<b;j++)
{
table<<setprecision(8)<<setw(18)<<c[i][j];
if(j==(b-1)) table<<endl;
}
table<<"\n\n\n\n\n\n";
//第二次矩陣相乘開始
//CA^T開始
long double x[b][a]={0.0};
for(i=0;i<b;i++)
for(j=0;j<a;j++)
for(r=0;r<b;r++)
x[i][j]+=c[i][r]*A[j][r];//A^T[i][j]=A[j][i]
table<<"\nx[i][j]:\n\n";
for(i=0;i<b;i++)
for(j=0;j<a;j++)
{
table<<setprecision(8)<<setw(18)<<x[i][j];
if(j==(a-1)) table<<endl;
}
table<<"\n\n\n\n\n\n";
//CA^T結束
//(CA^T)L開始
long double d[b][1]={0};
for(i=0;i<b;i++)
for(j=0;j<1;j++)
for(r=0;r<a;r++)
d[i][j]+=x[i][r]*L[r][j];
table<<"\nd[i][j]:\n\n";
for(i=0;i<b;i++)
for(j=0;j<1;j++)
{
table<<setprecision(8)<<setw(18)<<d[i][j]<<endl;
}
table<<"\n\n\n\n\n\n";
//(CA^T)L結束
//第二次矩陣相乘結束
Xs+=d[0][0];
Ys+=d[1][0];
Zs+=d[2][0];
p+=d[3][0];
w+=d[4][0];
k+=d[5][0];
rr++;//循環次數增1
cout<<"rr="<<rr<<endl;
if((abs(d[3][0]*ip)<6)&&(abs(d[4][0]*ip)<6)&&(abs(d[5][0]*ip)<6))
{
table<<"外方位元素的最終結果為：\n"<<endl;
table<<"Xs "<<setprecision(8)<<setw(14)<<Xs<<endl;
table<<"Ys "<<setprecision(8)<<setw(14)<<Ys<<endl;
table<<"Zs "<<setprecision(8)<<setw(14)<<Zs<<endl;
table<<"p "<<setprecision(8)<<setw(14)<<p<<endl;
table<<"w "<<setprecision(8)<<setw(14)<<w<<endl;
table<<"k "<<setprecision(8)<<setw(14)<<k<<endl;
table<<"\n";
break;
}
}while(rr<20);
}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -