【攝影測量學空間後方交會作業】求解程序
#include <stdio.h>
#include<cmath>
#include<iostream.h>
#include<fstream.h>
int main()
{
double NJZ(double sum[100][100],double l[10]);
double x[10],y[10],X[10],Y[10],Z[10],d,D,m,f,T,R,S,r=0,s=0,t=0;
ifstream infile; //定義輸入文件類
infile.open("測試_1.txt"); //打開一個輸入文件“file1.txt”
for(int i=1;i<=4;i++)
{
for(int j=1;j<=5;j++)
{
switch(j)
{
case 1:infile>>x[i];break;
case 2:infile>>y[i];break;
case 3:infile>>X[i];break;
case 4:infile>>Y[i];break;
case 5:infile>>Z[i];break;
}
//infile>>a[i][j];//將“file1.txt”中的十個整型數輸入到a[i]中
}
}
//for(i=1;i<=4;i++)
//{
//cout<<x[i]<<","<<y[i]<<","<<X[i]<<","<<Y[i]<<","<<Z[i]<<endl;
//}
//cout<<endl;
infile.close();//關閉輸入文件
d=sqrt(pow(x[1]-x[2],2)+pow(y[1]-y[2],2));
//cout<<"d="<<d<<endl;
D=sqrt(pow(X[1]-X[2],2)+pow(Y[1]-Y[2],2)+pow(Z[1]-Z[2],2));
//cout<<"D="<<D<<endl;
m=D/d;
//cout<<"m="<<m<<endl;
//cout<<"請輸入像片主距f(以mm為單位):";
//cin>>f;
f=153.24;
T=m*f;
//cout<<"T="<<T<<endl;
R=0.25*(X[1]+X[2]+X[3]+X[4]);
//cout<<"R="<<R<<endl;
S=0.25*(Y[1]+Y[2]+Y[3]+Y[4]);
//cout<<"S="<<S<<endl;
cout<<"第一步:初始化的6個外方位元素分別為:"<<endl;
cout<<"r=0"<<endl;
cout<<"s=0"<<endl;
cout<<"t=0"<<endl;
cout<<"R="<<R<<endl;
cout<<"S="<<S<<endl;
cout<<"T="<<T<<endl;
double a1,a2,a3,b1,b2,b3,c1,c2,c3;
a1=cos(r)*cos(t)-sin(r)*sin(s)*sin(t);
a2=-cos(r)*sin(t)-sin(r)*sin(s)*cos(t);
a3=-sin(r)*cos(s);
b1=cos(s)*sin(t);
b2=cos(s)*cos(t);
b3=-sin(s);
c1=sin(r)*cos(t)+cos(r)*sin(s)*sin(t);
c2=-sin(r)*sin(t)+cos(r)*sin(s)*cos(t);
c3=cos(r)*cos(s);
cout<<"第二步:旋轉矩陣計算成功,如下:"<<endl;
cout<<"a1="<<a1<<" "<<"a2="<<a2<<" "<<"a3="<<a3<<endl;
cout<<"b1="<<b1<<" "<<"b2="<<b2<<" "<<"b3="<<b3<<endl;
cout<<"c1="<<c1<<" "<<"c2="<<c2<<" "<<"c3="<<c3<<endl;
//逐點計算像點坐標的近似值
double u[5],v[5],U[5],V[5],W[5];
for(i=1;i<=4;i++)
{
U[i]=a1*(X[i]-R)+b1*(Y[i]-S)+c1*(Z[i]-T);
V[i]=a2*(X[i]-R)+b2*(Y[i]-S)+c2*(Z[i]-T);
W[i]=a3*(X[i]-R)+b3*(Y[i]-S)+c3*(Z[i]-T);
u[i]=-f*U[i]/W[i];
v[i]=-f*V[i]/W[i];
}
cout<<"第三步:像點坐標近似值計算成功,如下:"<<endl;
for(i=1;i<=4;i++)
{
cout<<"("<<u[i]<<","<<v[i]<<")"<<endl;
}
//計算系數矩陣
double a[10][10];
for(i=1;i<=4;i++)
{
a[2*i-1][1]=(a1*f+a3*x[i])/W[i];
a[2*i-1][2]=(b1*f+b3*x[i])/W[i];
a[2*i-1][3]=(c1*f+c3*x[i])/W[i];
a[2*i-1][4]=y[i]*sin(s)-cos(s)*(x[i]*(x[i]*cos(t)-y[i]*sin(t))/f+f*cos(t));
a[2*i-1][5]=-f*sin(t)-x[i]*(x[i]*sin(t)+y[i]*cos(t))/f;
a[2*i-1][6]=y[i];
a[2*i][1]=(a2*f+a3*y[i])/W[i];
a[2*i][2]=(b2*f+b3*y[i])/W[i];
a[2*i][3]=(c2*f+c3*y[i])/W[i];
a[2*i][4]=-x[i]*sin(s)-cos(s)*(y[i]*(x[i]*cos(t)-y[i]*sin(t))/f-f*sin(t));
a[2*i][5]=-f*cos(t)-y[i]*(x[i]*sin(t)+y[i]*cos(t))/f;
a[2*i][6]=-x[i];
}
cout<<"第四步:系數矩陣計算結束,如下:"<<endl;
for(i=1;i<=4;i++)
{
cout<<a[2*i-1][1]<<" "<<a[2*i-1][2]<<" "<<a[2*i-1][3]<<" "<<a[2*i-1][4]<<" "<<a[2*i-1][5]<<" "<<a[2*i-1][6]<<endl;
cout<<a[2*i][1]<<" "<<a[2*i][2]<<" "<<a[2*i][3]<<" "<<a[2*i][4]<<" "<<a[2*i][5]<<" "<<a[2*i][6]<<endl;
}
//計算常數項
double l[10];
for(i=1;i<=8;i++)
{
l[2*i-1]=x[i]-u[i];
l[2*i]=y[i]-v[i];
}
cout<<"第四步:常數項矩陣計算結束,如下:"<<endl;
for(i=1;i<=8;i++)
{
cout<<l[i]<<endl;
}
//求系數矩陣的轉置
int q=0;
double sum[100][100],A[100][100];
for(i=1;i<=8;i++)
{
for(int j=1;j<=6;j++)
{
A[j][i]=a[i][j];
}
}
cout<<"第五步：系數矩陣轉置結果如下:"<<endl;
for(i=1;i<=6;i++)
{
for(int j=1;j<=8;j++)
{
cout<<A[i][j]<<" ";
}
cout<<endl;
}
//求系數矩陣與其轉置矩陣的乘積
for(i=1;i<=6;i++)
{
for(int k=1;k<=6;k++)
{
sum[i][k]=0;
for(int j=1;j<=8;j++)
{
sum[i][k]=sum[i][k]+A[i][j]*a[j][k];
}
}
}
cout<<"第六步：系數矩陣與其轉置矩陣的乘積結果如下:"<<endl;
for(i=1;i<=6;i++)
{
for(int j=1;j<=6;j++)
{
cout<<sum[i][j]<<" ";
}
cout<<endl<<endl<<endl;
}
//求系數矩陣與常數項矩陣的乘積
double mun[10];
for(i=1;i<=6;i++)
{
mun[i]=0;
for(int j=1;j<=8;j++)
{
mun[i]=mun[i]+A[i][j]*l[j];
}
}
cout<<"第七步：系數矩陣與常數項矩陣的乘積結果如下:"<<endl;
for(i=1;i<=6;i++)
{
cout<<mun[i]<<endl;
}
NJZ(sum,l);
return 0;
}
double NJZ(double sum[100][100],double l[10])
{
int i,j,n=6,q,k,c=0;
double A[100],C[100][100],B[100][100],G[100][100],K[10];
for(i=1;i<=n;i++)
{
for(j=n+1;j<=2*n;j++)
{
//判斷
if(i==j-n)
{
sum[i][j]=1;
}
else
{
sum[i][j]=0;
}
}
}
// cout<<"第一步：為原矩陣配單位矩陣結果如下:"<<endl;
// for(i=1;i<=n;i++)
// {
// for(j=1;j<=2*n;j++)
// {
// cout<<sum[i][j]<<" ";
// }
// cout<<endl;
// }
for(j=1;j<=n;j++)//列
{
for(i=1;i<=n;i++)
{
if(i!=j)
{
if(sum[i][j]!=0)
{
for(k=1;k<=2*n;k++)
{
B[i][k]=sum[i][k]-sum[j][k]*sum[i][j]/sum[j][j];
}
for(k=1;k<=2*n;k++)
{
sum[i][k]=B[i][k];
}
}
}
else
{
if(sum[i][j]==0)//若對角線上的=0
{
for(q=1;q<=n;q++)//從第1行找起，找到不為0的行與其交換
{
if(sum[q][j]!=0)
{
for(k=1;k<=2*n;k++)
{
c++;
A[k]=sum[i][k];
sum[i][k]=sum[q][k];
sum[q][k]=A[k];
}
break;
}
}
}
}
//if(j==3&&i==3)
//{
// cout<<a[1][1]<<" "<<a[1][2]<<" "<<a[1][3]<<" "<<a[1][4]<<" "<<a[1][5]<<" "<<a[1][6]<<endl;
// cout<<a[2][1]<<" "<<a[2][2]<<" "<<a[2][3]<<" "<<a[2][4]<<" "<<a[2][5]<<" "<<a[2][6]<<endl;
// cout<<a[3][1]<<" "<<a[3][2]<<" "<<a[3][3]<<" "<<a[3][4]<<" "<<a[3][5]<<" "<<a[3][6]<<endl;
//}
}
}
for(j=1;j<=n;j++)
{
if(sum[j][j]!=1)
{
for(k=1;k<=2*n;k++)
{
sum[j][k]=sum[j][k]/sum[j][j];
}
}
}
cout<<"第八步：求系數矩陣與其逆矩陣的乘積的逆矩陣結果如下:"<<endl;
for(i=1;i<=n;i++)
{
for(j=1;j<=2*n;j++)
{
if(sum[i][j]<0.00001)
{
sum[i][j]=0;
}
}
}
for(i=1;i<=n;i++)
{
for(j=1;j<=n;j++)
{
G[i][j]=sum[i][j+n];
}
}
for(i=1;i<=n;i++)
{
for(j=1;j<=n;j++)
{
cout<<G[i][j]<<" ";
}
cout<<endl<<endl;
}
//求解6個外方位元素的改正數
for(i=1;i<=6;i++)
{
K[i]=0;
for(j=1;j<=6;j++)
{
K[i]=K[i]+G[i][j]*l[j];
}
}
cout<<"第九步：6個外方位元素的改正數結果如下:"<<endl;
for(i=1;i<=6;i++)
{
cout<<K[i]<<endl;
}
return 0;
}
