?? back111222.cpp
字號:
#include "iostream.h"
#include "math.h"
#define f 0.15324
#define x0 0
#define y0 0
void plus(double *a,double *b, double*c,int m1,int n1, int m2,int n2)
{
int i,j,k;
if (n1!=m2)
{
cout<<"can not plus!"<<endl;
return ;
}
for(i=0;i<m1;i++)
for(j=0;j<n2;j++)
*(c+j+i*n2)=0;
for(i=0;i<m1;i++)
for(j=0;j<n2;j++)
for(k=0;k<n1;k++)
*(c+j+i*n2)+=*(a+k+i*n1)*(*(b+j+k*n2));
}
/******************************
矩陣轉置
矩陣a 為m*n的矩陣，
返回矩陣a_t n*m
******************************/
void transpose(double *a, double*a_t,int m,int n)
{
int i,j;
for(i=0;i<m;i++)
for(j=0;j<n;j++)
{
*(a_t+i+j*m)=*(a+j+i*n);
}
return;
}
/******************************
矩陣求逆
矩陣a 為m*n的矩陣，
返回矩陣e 為a的逆矩陣
矩陣a可逆的充要條件是a是方陣且a為非奇異矩陣
******************************/
void inverse(double *a,double*e,int m,int n)
{
int i,j;
double temp;
double temp2;
/*若該矩陣不是方陣，則不能求逆*/
if(m!=n)
{
cout<<"a不是方陣，不能求逆！"<<endl;
return;
}
/*判斷該矩陣行列式的值，若值為0，則不能求逆 注釋掉的部分有錯誤*/
/* double value_a=1;
for(i=0;i<m;i++)
{
temp=*(a+i+i*m);
for(j=0;j<m;j++)
{
*(a+j+i*m)/=temp;
}
for(int k=i+1;k<m;k++)
{
temp2=*(a+i+k*m);
for(j=0;j<n;j++)
{
*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
}
}
value_a*=temp;
}
if (value_a==0)
{
cout<<"奇異矩陣不能求逆！"<<endl;
return false;
}
*/
for(i=0;i<m;i++)
for(j=0;j<n;j++)
{
if(i==j) *(e+j+i*m)=1;
else *(e+j+i*m)=0;
}
for(i=0;i<m;i++)
{
temp=*(a+i+i*m);
for(j=0;j<m;j++)
{
*(a+j+i*m)/=temp;
*(e+j+i*m)/=temp;
}
for(int k=i+1;k<m;k++)
{
temp2=*(a+i+k*m);
for(j=0;j<n;j++)
{
*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
*(e+j+k*m)=*(e+j+k*m)-temp2*(*(e+j+i*m));
}
}
}
for(i=m-1;i>0;i--)
{
for(j=0;j<m;j++)
for(int k=i-1;k>=0;k--)
{
temp2=*(a+i+k*m);
for(j=0;j<n;j++)
{
*(a+j+k*m)=*(a+j+k*m)-temp2*(*(a+j+i*m));
*(e+j+k*m)=*(e+j+k*m)-temp2*(*(e+j+i*m));
}
}
}
}
double Ab(double a)
{
double b;
if(a>=0)
b=a;
else b=-a;
return b;
}
/******************************
主函數
******************************/
void main()
{
int i,j; //循環變量
int num=0; //迭代次數
double scale; //比例尺分母
double Zs,Xs,Ys,fai,w,k; //外方位元素
double temp=0; //臨時變量
double x[4]={-0.08615,-0.05340,-0.01478,0.01046}; //相片坐標觀測值
double y[4]={-0.06899,0.08221,-0.07663,0.06443};
double X[4]={36589.41,37631.08,39100.97,40426.54}; //地面坐標觀測值
double Y[4]={25273.32,31324.51,24934.98,30319.81};
double Z[4]={2195.17,728.69,2386.50,757.31};
double a1,a2,a3,b1,b2,b3,c1,c2,c3; //矩陣R系數
double XX[4],YY[4],ZZ[4]; //共線方程分子分母
double x_x[4],y_y[4]; //像點坐標的近似值
double dXs,dYs,dZs,dfai,dw,dk; //外方位元素改正數
double A[8][6],L[8][1]; //總誤差方程式
/*誤差方程式計算中間變量*/
double AT[6][8]; //A的轉置
double ATA[6][6]; //AT*A
double ATA_1[6][6]; //ATA_1
double ATL[6][8]; //AT*A_1*AT
double DX[6][1];
/*精度*/
double v=0,m0,m[6];
//計算得到近似比例尺
double s[3]; //計算出三段距離，得出比例尺，取平均
for(i=0;i<3;i++)
{
s[i]=sqrt((Y[i]-Y[i+1])*(Y[i]-Y[i+1])+(X[i]-X[i+1])*(X[i]-X[i+1]))/sqrt((y[i]-y[i+1])*(y[i]-y[i+1])+(x[i]-x[i+1])*(x[i]-x[i+1]));
temp+=s[i];
}
scale=temp/3;
scale=40000;
//給定外方位元素的初值
fai=0;
w=0;
k=0;
Zs=scale*f+(Z[0]+Z[1]+Z[2]+Z[3])*0.25;
Xs=(X[0]+X[1]+X[2]+X[3])*0.25;
Ys=(Y[0]+Y[1]+Y[2]+Y[3])*0.25;
//進行迭代
while (Ab(dfai)>=0.0017 || Ab(dw)>=0.0017 || Ab(dk)>=0.0017 || num==0)
{
num+=1;
//計算旋轉矩陣R
a1=cos(fai)*cos(k)-sin(fai)*sin(w)*sin(k);
a2=-cos(fai)*sin(k)-sin(fai)*sin(w)*cos(k);
a3=-sin(fai)*cos(w);
b1=cos(w)*sin(k);
b2=cos(w)*cos(k);
b3=-sin(w);
c1=sin(fai)*cos(k)+cos(fai)*sin(w)*sin(k);
c2=-sin(fai)*sin(k)+cos(fai)*sin(w)*cos(k);
c3=cos(fai)*cos(w);
//通過共線方程計算像點坐標的近似值
for(i=0;i<4;i++)
{
XX[i]=a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs);
YY[i]=a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs);
ZZ[i]=a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs);
x_x[i]=-f*(a1*(X[i]-Xs)+b1*(Y[i]-Ys)+c1*(Z[i]-Zs))/(a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs))+x0;
y_y[i]=-f*(a2*(X[i]-Xs)+b2*(Y[i]-Ys)+c2*(Z[i]-Zs))/(a3*(X[i]-Xs)+b3*(Y[i]-Ys)+c3*(Z[i]-Zs))+y0;
}
//計算誤差方程式的系數和常數項，組成誤差方程式
for(i=0;i<4;i++)
{
/* A[2*i][0]=(a1*f+a3*x_x[i])/ZZ[i];
A[2*i][1]=(b1*f+b3*x_x[i])/ZZ[i];
A[2*i][2]=(c1*f+c3*x_x[i])/ZZ[i];
A[2*i+1][0]=(a2*f+a3*y_y[i])/ZZ[i];
A[2*i+1][1]=(b2*f+b3*y_y[i])/ZZ[i];
A[2*i+1][2]=(c2*f+c3*y_y[i])/ZZ[i];
A[2*i][3]=y_y[i]*sin(w)-(x_x[i]*(x_x[i]*cos(k)-y_y[i]*sin(k))/f+f*cos(k))*cos(w);
A[2*i][4]=-f*sin(k)-x_x[i]*(x_x[i]*sin(k)+y_y[i]*cos(k))/f;
A[2*i][5]=y_y[i];
A[2*i+1][3]=-x_x[i]*sin(w)-((x_x[i]*cos(k)-y_y[i]*sin(k))*y_y[i]/f-f*sin(k))*cos(w);
A[2*i+1][4]=-f*cos(k)-(x_x[i]*sin(k)+y_y[i]*cos(k))*y_y[i]/f;
A[2*i+1][5]=-x_x[i];
L[2*i][0]=x[i]-x_x[i];
L[2*i+1][0]=y[i]-y_y[i];
*/
A[2*i][0]=(a1*f+a3*(x[i]-x0))/ZZ[i];
A[2*i][1]=(b1*f+b3*(x[i]-x0))/ZZ[i];
A[2*i][2]=(c1*f+c3*(x[i]-x0))/ZZ[i];
A[2*i+1][0]=(a2*f+a3*(y[i]-y0))/ZZ[i];
A[2*i+1][1]=(b2*f+b3*(y[i]-y0))/ZZ[i];
A[2*i+1][2]=(c2*f+c3*(y[i]-y0))/ZZ[i];
A[2*i][3]=(y[i]-y0)*sin(w)-((x[i]-x0)*((x[i]-x0)*cos(k)-y[i]*sin(k))/f+f*cos(k))*cos(w);
A[2*i][4]=-f*sin(k)-(x[i]-x0)*((x[i]-x0)*sin(k)+(y[i]-y0)*cos(k))/f;
A[2*i][5]=y[i]-y0;
A[2*i+1][3]=-(x[i]-x0)*sin(w)-(((x[i]-x0)*cos(k)-(y[i]-y0)*sin(k))*(y[i]-y0)/f-f*sin(k))*cos(w);
A[2*i+1][4]=-f*cos(k)-((x[i]-x0)*sin(k)+y[i]*cos(k))*(y[i]-y0)/f;
A[2*i+1][5]=-(x[i]-x0);
L[2*i][0]=x[i]-x_x[i];
L[2*i+1][0]=y[i]-y_y[i];
}
for(i=0;i<4;i++)
cout<<x_x[i]<<endl;
cout<<endl;
for(i=0;i<4;i++)
cout<<y_y[i]<<endl;
cout<<endl;
for(i=0;i<8;i++)
cout<<L[i][0]<<endl;
cout<<endl;
//計算法方程的系數矩陣
transpose(*A,*AT,8,6);
plus(*AT,*A,*ATA,6,8,8,6);
inverse(*ATA,*ATA_1,6,6);
plus(*ATA_1,*AT,*ATL,6,6,6,8);
plus(*ATL,*L,*DX,6,8,8,1);
dXs=DX[0][0];
dYs=DX[1][0];
dZs=DX[2][0];
dfai=DX[3][0];
dw=DX[4][0];
dk=DX[5][0];
Xs+=dXs;
Ys+=dYs;
Zs+=dZs;
fai+=dfai;
w+=dw;
k+=dk;
if(num>=100) //迭代次數超過100次，退出
break;
}
cout<<"外方位元素為："<<endl;
cout<<"Xs:"<<Xs<<" Ys:"<<Ys<<" Zs:"<<Zs<<" fai:"<<fai<<" w:"<<w<<" k:"<<k<<endl<<endl;
double R[3][3]={a1,a2,a3,b1,b2,b3,c1,c2,c3};
cout<<"旋轉矩陣為："<<endl;
for(i=0;i<3;i++)
{
for(j=0;j<3;j++)
cout<<R[i][j]<<" ";
cout<<endl;
}
for(i=0;i<8;i++)
v+=L[i][0]*L[i][0];
//cout<<v<<endl;
m0=sqrt(v/(2*4-6));
cout<<"dXs,dYs,dZs,dfai,dw,dk的精度分別是："<<endl;
for(i=0;i<6;i++)
{
m[i]=sqrt(ATA_1[i][i])*m0;
cout<<m[i]<<endl;
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -