?? 后方交會.cpp
字號:
#include "stdio.h"
#include "stdlib.h"
#include "malloc.h"
#include "math.h"
#include "iostream.h"
#include "string.h"
#include "brinv.h"
//求取轉置矩陣的函數
void zhuanzhi(double **A,double **AT,int w,int n)
{
int i,j;
for(i=0;i<w;i++)
for(j=0;j<n;j++)
{
AT[j][i]=A[i][j];//**求系數矩陣的逆AT******
}
}
int inverse(double **bb, int n) //求逆函數
{
int *is,*js,i,j,k,l,u,v;
double *a=(double*)malloc(sizeof(double)*n*n);
for(i=0;i<n;i++)
{
for(j=0;j<n;j++)
a[n*i+j]=bb[i][j];
}
double d,p;
is=(int*)malloc(n*sizeof(int));//代表矩陣的行
js=(int*)malloc(n*sizeof(int)); //代表矩陣的列
for (k=0; k<=n-1; k++)
d=0.0;
for (i=k; i<=n-1; i++)
for (j=k; j<=n-1; j++)
{
l=i*n+j;
p=fabs(a[l]);
if (p>d)
{
d=p;
is[k]=i;
js[k]=j;
}
}
if (d+1.0==1.0)//主要是防止計算機的舍入誤差造成的錯誤。
{
free(is);
free(js);
printf("err**not inv\n");
return(0);
}
if (is[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j;
v=is[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if (js[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+js[k];
p=a[u]; a[u]=a[v]; a[v]=p;
}
l=k*n+k;
a[l]=1.0/a[l];
for (j=0; j<=n-1; j++)
if (j!=k)
{
u=k*n+j;
a[u]=a[u]*a[l];
}
for (i=0; i<=n-1; i++)
if (i!=k)
for (j=0; j<=n-1; j++)
if (j!=k)
{
u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
for (i=0; i<=n-1; i++)
if (i!=k)
{
u=i*n+k;
a[u]=-a[u]*a[l];
}
}//結束for (k=0; k<=n-1; k++)
for (k=n-1; k>=0; k--)
{
if (js[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j;
v=js[k]*n+j;
p=a[u]; a[u]=a[v]; a[v]=p;
}
if (is[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+is[k];
p=a[u]; a[u]=a[v]; a[v]=p;
}
}
for(i=0;i<n;i++)
{
for(j=0;j<n;j++)
bb[i][j]=a[n*i+j];
}
free(is);
free(js);
return(1);
} //結束矩陣轉置函數inverse()*/
//矩陣相乘的函數
void xiangc(double **A,double **B,double **c,int u,int w,int l)
{
int i,j,n;
for(i=0;i<u;i++)
for(j=0;j<l;j++)
for(n=0;n<w;n++)
{
c[i][j]+=A[i][n]*B[n][j];
}
}
void main()
{
//****************第一步*******************//
//***********讀出數據，從文本文檔里讀出數據**********//
//分別將象點數據讀給X數組，地面點數據讀給D數組***********//
float **X,**D,f;//X數組代表輸入的象點坐標，D代表輸入的地面點坐標
double H;//f表示攝影中心s到像片的垂距,H表示航高
double temp;//中間變量
int w,j,i;
FILE *fp;//文件名
char filename[20],out[20];
printf("輸入要處理的數據所在文件名:");
gets(filename);
strcpy(out,filename);
if((fp=fopen(strcat(filename,".txt"),"r"))==NULL) //打開文件
{
printf("*********讀文件不成功！*********\n");
exit(0);
}
else
printf("**********讀文件成功！************\n\n");
w=0;
while(!feof(fp))
{
for(j=0;j<5;j++)
{
fscanf(fp,"%f ",&temp);
}
w++; //先確認讀入數據的行數
}
w=w-1;
printf("%d\n",w);
rewind(fp);
//分配存儲數據的空間
X=(float**)malloc(sizeof(float)*(w));
for(i=0;i<w;i++)
{
X[i]=(float*)malloc(sizeof(float)*(2));//為二維數組開辟空間
}
D=(float**)malloc(sizeof(float)*(w));
for(i=0;i<w;i++)
{
D[i]=(float*)malloc(sizeof(float)*(3));//為二維數組開辟空間
}
while(!feof(fp))
{
fscanf(fp,"%f",&f);
for(i=0;i<w;i++)
{
fscanf(fp,"%f",&X[i][0]);
fscanf(fp,"%f",&X[i][1]);
fscanf(fp,"%f",&D[i][0]);
fscanf(fp,"%f",&D[i][1]);
fscanf(fp,"%f",&D[i][2]);//每一行的前兩位讀給 象點數組，后三位讀給 地面點數組
}
}
fclose(fp);
//驗證數據讀入后準確與否
for(i=0;i<w;i++)
{
printf("%.2lf ",X[i][0]);printf(" ");
printf("%.2lf ",X[i][1]);printf(" ");
printf("%.2lf ",D[i][0]);printf(" ");
printf("%.2lf ",D[i][1]);printf(" ");
printf("%.2lf ",D[i][2]);printf(" ");
printf("\n");
}
//******************第二步***********//
//**確定航高H，攝影中心在地面坐標系中的坐標小Xs，Ys，Zs，其中Zs以H為初值，Xs Ys分別以四個角上的控制
//**點平均值代替初始值//
double Xs,Ys,Zs;
Xs=0;
Ys=0;
Zs=0;
for(i=0;i<4;i++)
{
Xs+=D[i][0];
}
for(i=0;i<4;i++)
{
Ys+=D[i][1];
}
Xs=Xs/4;
Ys=Ys/4;printf("Xs的初始值是：%f ",Xs);printf("Ys的初始值是：%f ",Ys);
//求Z0時我們用公式Z0=H=mf，因此我們必須求出比例尺m
//求近似m的方法是 同時求出地面兩點的距離和相片中對應點的距離 兩者之比就是m
double m,M[7];
M[1]=sqrt((X[1][1]-X[2][1])*(X[1][1]-X[2][1])+(X[1][0]-X[2][0])*(X[1][0]-X[2][0]));
M[2]=sqrt((X[1][1]-X[3][1])*(X[1][1]-X[3][1])+(X[1][0]-X[3][0])*(X[1][0]-X[3][0]));
M[3]=sqrt((X[2][1]-X[3][1])*(X[2][1]-X[3][1])+(X[2][0]-X[3][0])*(X[2][0]-X[3][0]));
M[4]=sqrt((X[0][1]-X[3][1])*(X[0][1]-X[3][1])+(X[0][0]-X[3][0])*(X[0][0]-X[3][0]));
M[5]=sqrt((X[0][1]-X[2][1])*(X[0][1]-X[2][1])+(X[0][0]-X[2][0])*(X[0][0]-X[2][0]));
M[6]=sqrt((X[0][1]-X[1][1])*(X[0][1]-X[1][1])+(X[0][0]-X[1][0])*(X[0][0]-X[1][0]));
M[1]=sqrt((D[1][1]-D[2][1])*(D[1][1]-D[2][1])+(D[1][0]-D[2][0])*(D[1][0]-D[2][0]))/M[1];
M[2]=sqrt((D[1][1]-D[3][1])*(D[1][1]-D[3][1])+(D[1][0]-D[3][0])*(D[1][0]-D[3][0]))/M[2];
M[3]=sqrt((D[2][1]-D[3][1])*(D[2][1]-D[3][1])+(D[2][0]-D[3][0])*(D[2][0]-D[3][0]))/M[3];
M[4]=sqrt((D[0][1]-D[3][1])*(D[0][1]-D[3][1])+(D[0][0]-D[3][0])*(D[0][0]-D[3][0]))/M[4];
M[5]=sqrt((D[0][1]-D[2][1])*(D[0][1]-D[2][1])+(D[0][0]-D[2][0])*(D[0][0]-D[2][0]))/M[5];
M[6]=sqrt((D[0][1]-D[1][1])*(D[0][1]-D[1][1])+(D[0][0]-D[1][0])*(D[0][0]-D[1][0]))/M[6];
m=(M[1]+M[2]+M[3]+M[4]+M[5]+M[6])/6;
H=m*f;
Zs=m*f;printf("h=%f \n",f);
printf("h=%f \n",H);
double a,b,c;//三個角元素
//誤差方程的系數矩陣
double **A;
A=(double**)malloc(sizeof(double)*(2*w));
for(i=0;i<2*w;i++)
{
A[i]=(double*)malloc(sizeof(double)*(6));//為二維數組開辟空間
}
//定義一個AT用來存儲A的轉置結果
double **AT;
AT=(double**)malloc(sizeof(double)*(6));
for(i=0;i<7;i++)
{
AT[i]=(double*)malloc(sizeof(double)*(2*w));//為二維數組開辟空間
}
//系數矩陣與其轉置矩陣乘積
double **B;
B=(double**)malloc(sizeof(double)*(6));
for(i=0;i<7;i++)
{
B[i]=(double*)malloc(sizeof(double)*(6));//為二維數組開辟空間
}
//B矩陣求逆矩陣是借用的一個矩陣
double **BJ;
BJ=(double**)malloc(sizeof(double)*(6));
for(i=0;i<6;i++)
{
BJ[i]=(double*)malloc(sizeof(double)*(6));//為二維數組開辟空間
}
double **B1;
B1=(double**)malloc(sizeof(double)*(6));
for(i=0;i<6;i++)
{
B1[i]=(double*)malloc(sizeof(double)*(6));//為二維數組開辟空間
}
//用矩陣X1來表示計算的近似值
double **X1;
X1=(double**)malloc(sizeof(double)*(w));
for(i=0;i<w;i++)
{
X1[i]=(double*)malloc(sizeof(double)*(2));//為二維數組開辟空間
}
//用矩陣L來表示計算的近似值與測量值的差//其用二維數組表示但只要他的一列
double **L;
L=(double**)malloc(sizeof(double)*(2*w));
for(i=0;i<2*w;i++)
{
L[i]=(double*)malloc(sizeof(double)*(1));//為二維數組開辟空間
}
//我們還要用另外一個矩陣 來存AT與L的乘積 C AT有7行2*w+1列 L有2*w+1行2列 故C應該有7行2列
double **C;
C=(double**)malloc(sizeof(double)*(6));
for(i=0;i<6;i++)
{
C[i]=(double*)malloc(sizeof(double)*(1));//為二維數組開辟空間
}
//定義一個結果矩陣D 用來存儲計算得到改正數
double **d;
d=(double**)malloc(sizeof(double)*(6));
for(i=0;i<6;i++)
{
d[i]=(double*)malloc(sizeof(double)*(1));//為二維數組開辟空間
}
//給d數組賦以初值以便使他可以進行下面的迭代
for(i=0;i<6;i++)
for(j=0;j<1;j++)
d[i][j]=2;
//**********第三步*********//
//求取三個角元素a，b，c
//在我們這 為簡單起見 我們將 三者全初始值看為0 故我們可以求誤差方程的系數矩陣A
for(i=0;i<w;i++)
{
A[2*i][0]=-1*f/H;
A[2*i][1]=0;
A[2*i][2]=-1*X[i][0]/H;
A[2*i][3]=-1*f*(1+X[i][0]*X[i][0]/f/f);
A[2*i][4]=X[i][0]*X[i][1]*(-1)/f;
A[2*i][5]=X[i][1];
A[2*i+1][0]=0;
A[2*i+1][1]=-1*f/H;
A[2*i+1][2]=-1*X[i][1]/H;
A[2*i+1][3]=X[i][0]*X[i][1]*(-1)/f;
A[2*i+1][4]=-1*f*(1+X[i][1]*X[i][1]/f/f);
A[2*i+1][5]=-X[i][0];
}
printf("\n\n系數矩陣：\n ");
for(i=0;i<2*w;i++)
{
for(j=0;j<6;j++)
{
printf("%f ",A[i][j]);
}printf("\n");
}
printf("\n");
printf("\n");
printf("\n");
printf("\n");
//下面用迭代來進行各個改正數的計算
do
{
//求系數矩陣轉置矩陣與系數矩陣的乘積 的矩陣B
//現求系數矩陣的轉置，調用函數zhuanzhi
zhuanzhi(A,AT,2*w,6);
printf("A的轉置矩陣：\n");
for(i=0;i<6;i++)
{
for(j=0;j<2*w;j++)
{
printf("%.3f ",AT[i][j]);
}printf("\n");
}
printf("\n");
printf("\n");
printf("\n");
//因為B要參加運算故先必須附以初值
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
B[i][j]=0;
}
}
xiangc(AT,A,B,6,w,6);
printf("A的轉置矩陣與其自身的乘積的矩陣B：\n");
for(i=0;i<6;i++)
{
for(j=0;j<6;j++)
{
printf("%.3f ",B[i][j]);
}printf("\n");
}
printf("\n");
printf("\n");
printf("\n");
/***************第四步求取誤差系數L；************/
//由于在這里我們把三個角元素都很小，故可將共線方程簡化成(x)=-f(X-Xs)/(Z-Zs) (y)=-f(Y-Ys)/(Z-Zs)
for(i=0;i<w;i++)
{
X1[i][0]=-1*f*(D[i][0]-Xs)/(D[i][2]-Zs);
X1[i][1]=-1*f*(D[i][1]-Ys)/(D[i][2]-Zs);
}
//計算L
for(i=0;i<w;i++)
{
L[2*i][0]=X[i][0]-X1[i][0];
L[2*i+1][0]=X[i][1]-X1[i][1];
}
//最后我們可以計算d
//調用函數相乘
//因為C要參加運算故先必須附以初值
for(i=0;i<6;i++)
for(j=0;j<1;j++)
{
C[i][j]=0;
}
xiangc(AT,L,C,6,2*w,1);
printf("A的轉置矩陣與L的乘積的矩陣C：\n");
for(i=0;i<6;i++)
{ for(j=0;j<1;j++)
{
printf("%f ",C[i][j]);
}cout<<"\n";
}
printf("\n");
printf("\n");
printf("\n");
//我們在前面求出了AT和A的乘積B，還有AT與L的乘積C
//下面應該求B的逆矩陣
inverse(B,6) ;
printf("得到的逆矩陣B\n");
int m,n;
for(m=0;m<6;m++)
{
for(n=0;n<6;n++)
{
printf("%f ",B[m][n]);
}printf("\n");
}
printf("\n");
printf("\n");
printf("\n");
//然后我們應該求B與C 乘積了
printf("B與C 乘積的到最后的d矩陣:\n");
xiangc(B,C,d,6,6,1);
for(i=0;i<6;i++)
{ for(j=0;j<1;j++)
{
printf("%.3f ",d[i][j]);
}
printf("\n");
}
printf("\n");
printf("\n");
/*從上面函數調用我們可以得到 最后的改正數，這樣我們可以將改正數加入原先的外方位元素
以便為下次迭代 做準備 或者符合條件 作為輸出 */
Xs=Xs+d[1][0];
Ys=Ys+d[2][0];
Zs=Zs+d[3][0];
a=a+d[4][0];
b=b+d[5][0];
c=c+d[6][0];
}
while(d[1][0]>0.01||d[2][0]>0.01||d[3][0]>0.01||d[4][0]>0.01||d[5][0]>0.01||d[6][0]>0.01);
FILE* resul;
if((resul=fopen(strcat(out,"res.txt"),"w"))==NULL) //打開文件
{
cout<<"Can not open the file!"<<cout;
return;
}
fprintf(resul,"Xs最后結果是：Xs=%f",Xs);
}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -