# susqbnd.rar

• Ulaqf
了解作者
• Others
开发工具
• 2KB
文件大小
• rar
文件格式
• 0
收藏次数
• 1 积分
下载积分
• 0
下载次数
• 2018-05-28 22:07
上传日期

susqbnd.rar
• r3r摄影测量最终程序.txt
8.2KB

#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 rel='nofollow' onclick='return false;'>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<<"外方位元素�

相关推荐