一、 目的要求
1、 通过程序的设计深入理解解析空中三角测量的整个过程
2、 提高应用程序设计语言解决问题的能力。
二、 资料及用具
一台微机,一套调试程序所用的数据。
三、 实习内容及作业步骤
用VB(或者C语言)程序设计语言编写单航带区域网概算的程序,具有包含一些三方面的内容:
(一) 连续法像对的相对定向
目标:求解各模型点在各模型像空间辅助坐标系中的坐标(Xi,Yi,Zi)
步骤:
用连续法相对定向求解相对定向元素(bx,by,bz,φ ,ω,κ)。
R1=E,R2由相对定向元素φ ,ω,κ求
带公式求出投影系数N1,N2。
计算像空间坐标系的模型点坐标(N1,N2)。
有模型点坐标转换成摄影测量坐标系坐标Xp。
(二) 航带网模型的建立
目标:求出航带内各模型点在航带统一坐标系(第一个像片的像空间坐标系)中的坐标(Xi‘,Yi‘,Zi‘)。
步骤:
求比例尺变换系数K。
1、 根据模型间的三个公共点求解个模型的缩放系数ki
2、 通过各模型缩放系数ki将模型内各模型点转换到航带统一坐标系。
(三) 模型的绝对定向
目标:求出航带内各模型点在大地坐标系中的坐标(Xti,Yti,Zti)。
步骤:
大地坐标已知,摄影测量坐标系坐标由模型点坐标可知,他们的差值可以根据第一个点计算。
1、 根据两个已知控制点确定大地坐标系与地面摄测坐标系之间的转换参数(a、b)。
2、 将所有控制点的大地坐标(Xt,Yt,Zt)通过转换参数(a、b)转换为地面摄测坐标系(Xtp,Ytp,Ztp)。P100
3、 根据已知控制点的地面摄测坐标(Xtp,Ytp,Ztp)求解绝对定向元素(ΔX,ΔY,ΔZ,Φ,Ω,Κ,λ)。重心化
4、 根据绝对定向元素(ΔX,ΔY,ΔZ,Φ,Ω,Κ,λ)将各模型点在航带统一坐标系中的坐标(Xi‘,Yi‘,Zi‘)转换为地面摄测坐标系(Xtp,Ytp,Ztp)。P87
5、 将各模型点的地面摄测坐标(Xtp,Ytp,Ztp)通过转换参数(a、b)转换为所有控制点的大地坐标(Xt,Yt,Zt)。P103
四、 上交成果
调试完成的程序一份及计算结果。
附:调试数据:
1、 基本数据
摄影机主距:f=153.033mm bx=200mm
2、 像对坐标数据(单位:微米)
像对1: 701 702
1201 648790 735260 -230980 788550
1818 113860 595800 -771380 679190
1202 241050 586260 -644640 661950
1204 578030 223960 -327700 277590
1203 256140 187820 -648360 260160
1205 606230 -104550 -315660 -51440
1206 278340 -565020 -660020 -487200
1052 179220 -757800 -765430 -670400
410 478510 -637940 -466930 -569840
399 975930 -700850 16690 -658940
192 917380 -16160 -4600 18540
370 803150 758570 -76440 802960
1118 94870 709670 -785850 795830
194 35030 -34550 -877140 50930
-398 19790 -843710 -925660 -745370
像对2: 703 702
400 -568240 -631500 426790 -634450
1207 -601170 642970 323920 637060
399 -980300 -630600 16690 -658940
192 -963100 51030 -4600 18540
370 -989450 836700 -76410 802880
1209 -8790 641530 921460 679700
401 -29060 -915900 981740 -884160
-190 1460 5490 965780 38900
像对3: 703 704
1826 931930 729560 -26020 680090
402 907100 -785750 20330 -836630
1055 753660 -838360 -134160 -896670
411 397770 -725220 -500070 -791380
1211 49010 50160 -875490 -17320
188 918060 48330 -10150 11950
1225 890340 544420 -58540 499270
1210 624000 444520 -317940 392160
1209 -8690 641540 -945780 560530
401 -29020 -915940 -930070 -1004070
-190 1460 5460 -921980 -63410
3、控制点数据
点号 X坐标 Y坐标 Z坐标
==================================================
1201 24204.689 46604.652 46.251
1205 24343.683 45111.263 48.198
1206 24965.047 44309.253 49.340
1209 22167.904 46432.515 46.457
4、检查点数据
点号 X坐标 Y坐标 Z坐标
==================================================
1118 25192.533 46608.059 48.936
1202 24941.046 46375.998 46.539
1203 24945.705 45662.638 49.052
1204 24369.486 45700.421 49.020
1207 23218.292 46347.142 48.993
输入文件:
输出文件:
相对定向最终结果:
-0.03699813 0.000954 0.000356 -0.002035 0.058404
0.09298690 0.001239 0.001651 -0.000996 0.126354
0.16606539 0.001761 0.000771 0.002682 0.090635
循环次数:
4
4
6
最后一次的循环改正数:
U V φ0 ω0 k0
0.000000044434206837 -0.000000446379736953 -0.000000120335980734 0.000000040374781333 0.000000625192215553
0.000000108774010818 0.000006981381330488 -0.000000236458701720 -0.000000185925191181 0.000004183349296592
-0.000000018515048819 -0.000001275900029040 -0.000000146075451673 -0.000000037988660966 -0.000001720368636429
计算出的比例尺
h0 = 8236.864011
计算出的 k1 k2
k1 k2
1.039223 0.960157
计算出的 a b T
a b T
-0.044919 -0.999528 1.000537
计算出的绝对定向元素
T0 X0 Y0 Z0 tra1 tra2 tra3
1.000362 -1158.742958 -1313.633061 -984.027640 -0.001149 -0.002029 0.000028
最后的大地坐标
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
24343.691188 45111.345990 48.244403
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
22167.768685 46432.312849 46.462352
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
20424.449748 45514.657037 48.033118
与待定点的差值
-0.330895 0.432428 2.423066
-0.305223 0.249968 -0.016612
-0.128118 0.080508 1.390518
-0.176774 0.011668 1.405143
-0.042888 -0.203221 2.236999
// 5.cpp : 定义控制台应用程序的入口点。 // #include "stdafx.h" #include "iostream" using namespace std; #include "string" #include "stdio.h" #include "math.h" //矩阵求逆开始 void InverseMatrix(double a[],int n) { int *is,*js,i,j,k,l,u,v; 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 ; } 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 = 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; } } free(is); free(js); } //矩阵求逆结束 typedef struct //原始数据 像点坐标 { double index; double x1; double y1; double x2; double y2; }DATA1; typedef struct //求得的XYZ { double index; double X1; double Y1; double X2; double Y2; double Z1; double Z2; }DATA2; typedef struct //输入的控制点坐标 { double index; double X; double Y; double Z; }DATA3; typedef struct { double index; double x; //左片的相同点 double y; double z; int a; //计算比例尺时控制点的点号 }DATA4; typedef struct { double index; double X1; double Y1; double Z1; }DATA5; typedef struct { double index; double x1; double y1; double z1; }DATA6; int _tmain(int argc, _TCHAR* argv[]) { //给定的初值 double f=0.153033; double bx=0.2; DATA1 data1[15]; DATA1 data2[8]; DATA1 data3[11]; DATA6 data5[5]; DATA5 mddata1[15]; DATA5 mddata2[8]; DATA5 mddata3[11]; //*************************************************读文件 { FILE *fp1; fp1=fopen("data1.txt","r"); if(fp1==0) { printf("Error!Can't open it!\n"); return 0; } for(int i=0;!feof(fp1);i++) { fscanf(fp1,"%lf %lf %lf %lf %lf ",&data1[i].index,&data1[i].x1,&data1[i].y1,&data1[i].x2,&data1[i].y2); data1[i].x1=data1[i].x1/1000000; data1[i].y1=data1[i].y1/1000000; data1[i].x2=data1[i].x2/1000000; data1[i].y2=data1[i].y2/1000000; } fclose(fp1); } { FILE *fp1,*fp2; fp1=fopen("data2.txt","r"); if(fp1==0) { printf("Error!Can't open it!\n"); return 0; } for(int i=0;!feof(fp1);i++) { fscanf(fp1,"%lf %lf %lf %lf %lf ",&data2[i].index,&data2[i].x1,&data2[i].y1,&data2[i].x2,&data2[i].y2); data2[i].x1=data2[i].x1/1000000; data2[i].y1=data2[i].y1/1000000; data2[i].x2=data2[i].x2/1000000; data2[i].y2=data2[i].y2/1000000; } fclose(fp1); } { FILE *fp1,*fp2; fp1=fopen("data3.txt","r"); if(fp1==0) { printf("Error!Can't open it!\n"); return 0; } for(int i=0;!feof(fp1);i++) { fscanf(fp1,"%lf %lf %lf %lf %lf ",&data3[i].index,&data3[i].x1,&data3[i].y1,&data3[i].x2,&data3[i].y2); data3[i].x1=data3[i].x1/1000000; data3[i].y1=data3[i].y1/1000000; data3[i].x2=data3[i].x2/1000000; data3[i].y2=data3[i].y2/1000000; } fclose(fp1); } DATA3 data4[4]; { FILE *fp1; fp1=fopen("data4.txt","r"); if(fp1==0) { printf("Error!Can't open it!\n"); return 0; } for(int i=0;!feof(fp1);i++) { fscanf(fp1,"%lf %lf %lf %lf ",&data4[i].index,&data4[i].X,&data4[i].Y,&data4[i].Z); } fclose(fp1); } { FILE *fp1; fp1=fopen("data5.txt","r"); if(fp1==0) { printf("Error!Can't open it!\n"); return 0; } for(int i=0;!feof(fp1);i++) { fscanf(fp1,"%lf %lf %lf %lf ",&data5[i].index,&data5[i].x1,&data5[i].y1,&data5[i].z1); } fclose(fp1); } //**********************************************读文件结束 //********************************************相对定向 double result1[5]={0}; double result2[5]={0}; double result3[5]={0}; int i1,i2,i3; double U1=0,V1=0,tra11=0,tra12=0,tra13=0, U2=0,V2=0,tra21=0,tra22=0,tra23=0, U3=0,V3=0,tra31=0,tra32=0,tra33=0; double N11[15],N21[15], N12[8],N22[8], N13[11],N23[11]; double a4,a5,a6,b4,b5,b6,c4,c5,c6; DATA2 data12[15]; double data13[15][5]; double data14[5][5]; double R1[5][15]; double A1[5][15]; double L1[15]; double by1; double bz1; for(i1=1;;i1++) { //R1=E R2由初值迭代计算 //计算初始值 U1=U1+result1[0], V1=V1+result1[1], tra11=tra11+result1[2], tra12=tra12+result1[3], tra13=tra13+result1[4]; by1=bx*U1; bz1=bx*V1; //矩阵九项各值 double a1=cos(tra11)*cos(tra13)-sin(tra11)*sin(tra12)*sin(tra13), a2=-cos(tra11)*sin(tra13)-sin(tra11)*sin(tra12)*cos(tra13), a3=-sin(tra11)*cos(tra12), b1=cos(tra12)*sin(tra13), b2=cos(tra12)*cos(tra13), b3=-sin(tra12), c1=sin(tra11)*cos(tra13)+cos(tra11)*sin(tra12)*sin(tra13), c2=-sin(tra11)*sin(tra13)+cos(tra11)*sin(tra12)*cos(tra13), c3=cos(tra11)*cos(tra12); //计算XYZ像空间坐标系 for(int i=0;i<15;i++) { //计算由像点与矩阵R相乘后的坐标XYZ data12[i].index=data1[i].index; data12[i].X1=data1[i].x1; data12[i].Y1=data1[i].y1; data12[i].Z1=-f; data12[i].X2=a1*data1[i].x2+a2*data1[i].y2+a3*(-f); data12[i].Y2=b1*data1[i].x2+b2*data1[i].y2+b3*(-f); data12[i].Z2=c1*data1[i].x2+c2*data1[i].y2+c3*(-f); //计算投影系数 N11[i]=(bx*data12[i].Z2-bz1*data12[i].X2)/(data12[i].X1*data12[i].Z2-data12[i].X2*data12[i].Z1); N21[i]=(bx*data12[i].Z1-bz1*data12[i].X1)/(data12[i].X1*data12[i].Z2-data12[i].X2*data12[i].Z1); //计算矩阵A data13[i][0]=bx; data13[i][1]=-(data12[i].Y2/data12[i].Z2)*bx; data13[i][2]=-(data12[i].X2*data12[i].Y2*N21[i])/data12[i].Z2; data13[i][3]=-(data12[i].Z2+(data12[i].Y2*data12[i].Y2)/data12[i].Z2)*N21[i]; data13[i][4]=data12[i].X2*N21[i]; //计算矩阵L L1[i]=N11[i]*data12[i].Y1-N21[i]*data12[i].Y2-by1; } //计算转置矩阵 for (int i=0;i<15;i++) { for (int j=0;j<5;j++) { R1[j][i]=data13[i][j]; } } double t; for(int i=0;i<5;i++) { for (int m=0;m<5;m++) { t=0; for (int j=0;j<15;j++) { t=R1[i][j]*data13[j][m]+t; } data14[i][m]=t; } } InverseMatrix(*data14,5); double tt; for(int i=0;i<5;i++) { for (int m=0;m<15;m++) { tt=0; for (int j=0;j<5;j++) { tt=data14[i][j]*R1[j][m]+tt; } A1[i][m]=tt; } } for(int i=0;i<5;i++) { double o=0; for( int j=0;j<15;j++) { o=A1[i][j]*L1[j]+o; } result1[i]=o; } if(fabs(result1[0])<0.00003 && fabs(result1[1])<0.00003 && fabs(result1[2])<0.00003 && fabs(result1[3])<0.00003 && fabs(result1[4])<0.00003 ) { tra11=tra11+result1[2], tra12=tra12+result1[3], tra13=tra13+result1[4], a1=cos(tra11)*cos(tra13)-sin(tra11)*sin(tra12)*sin(tra13), a2=-cos(tra11)*sin(tra13)-sin(tra11)*sin(tra12)*cos(tra13), a3=-sin(tra11)*cos(tra12), b1=cos(tra12)*sin(tra13), b2=cos(tra12)*cos(tra13), b3=-sin(tra12), c1=sin(tra11)*cos(tra13)+cos(tra11)*sin(tra12)*sin(tra13), c2=-sin(tra11)*sin(tra13)+cos(tra11)*sin(tra12)*cos(tra13), c3=cos(tra11)*cos(tra12), a4=a1; a5=a2; a6=a3; b4=b1; b5=b2; b6=b3; c4=c1; c5=c2; c6=c3; break; } } /****************************************///计算第二个文件 DATA2 data22[8]; double data23[8][5]; double data24[5][5]; double R2[5][8]; double A2[5][8]; double L2[8]; double by2 ; double bz2 ; for(i2=1;;i2++) { //计算初始值 U2=U2+result2[0], V2=V2+result2[1], tra21=tra21+result2[2], tra22=tra22+result2[3], tra23=tra23+result2[4]; by2=bx*U2; bz2=bx*V2; //矩阵九项各值 double a1=cos(tra21)*cos(tra23)-sin(tra21)*sin(tra22)*sin(tra23), a2=-cos(tra21)*sin(tra23)-sin(tra21)*sin(tra22)*cos(tra23), a3=-sin(tra21)*cos(tra22), b1=cos(tra22)*sin(tra23), b2=cos(tra22)*cos(tra23), b3=-sin(tra22), c1=sin(tra21)*cos(tra23)+cos(tra21)*sin(tra22)*sin(tra23), c2=-sin(tra21)*sin(tra23)+cos(tra21)*sin(tra22)*cos(tra23), c3=cos(tra21)*cos(tra22); //计算XYZ像空间坐标系 for(int i=0;i<8;i++) { //计算由像点与矩阵R相乘后的坐标XYZ data22[i].index=data2[i].index; data22[i].X1=a4*data2[i].x1+a5*data2[i].y1+a6*(-f); data22[i].X2=a1*data2[i].x2+a2*data2[i].y2+a3*(-f); data22[i].Y1=b4*data2[i].x1+b5*data2[i].y1+b6*(-f); data22[i].Y2=b1*data2[i].x2+b2*data2[i].y2+b3*(-f); data22[i].Z1=c4*data2[i].x1+c5*data2[i].y1+c6*(-f); data22[i].Z2=c1*data2[i].x2+c2*data2[i].y2+c3*(-f); //计算投影系数 N12[i]=(bx*data22[i].Z2-bz2*data22[i].X2)/(data22[i].X1*data22[i].Z2-data22[i].X2*data22[i].Z1); N22[i]=(bx*data22[i].Z1-bz2*data22[i].X1)/(data22[i].X1*data22[i].Z2-data22[i].X2*data22[i].Z1); //计算矩阵A data23[i][0]=bx; data23[i][1]=-(data22[i].Y2/data22[i].Z2)*bx; data23[i][2]=-(data22[i].X2*data22[i].Y2*N22[i])/data22[i].Z2; data23[i][3]=-(data22[i].Z2+(data22[i].Y2*data22[i].Y2)/data22[i].Z2)*N22[i]; data23[i][4]=data22[i].X2*N22[i]; //计算矩阵L L2[i]=N12[i]*data22[i].Y1-N22[i]*data22[i].Y2-by2; } for (int i=0;i<8;i++) { for (int j=0;j<5;j++) { R2[j][i]=data23[i][j]; } } double t; for(int i=0;i<5;i++) { for (int m=0;m<5;m++) { t=0; for (int j=0;j<8;j++) { t=R2[i][j]*data23[j][m]+t; } data24[i][m]=t; } } InverseMatrix(*data24,5); //计算(ATA)-1AT double tt; for(int i=0;i<5;i++) { for (int m=0;m<8;m++) { tt=0; for (int j=0;j<5;j++) { tt=data24[i][j]*R2[j][m]+tt; } A2[i][m]=tt; } } for(int i=0;i<5;i++) { double o=0; for( int j=0;j<8;j++) { o=A2[i][j]*L2[j]+o; } result2[i]=o; } if(fabs(result2[0])<0.00003 && fabs(result2[1])<0.00003 && fabs(result2[2])<0.00003 && fabs(result2[3])<0.00003 && fabs(result2[4])<0.00003 ) { tra21=tra21+result2[2], tra22=tra22+result2[3], tra23=tra23+result2[4], a1=cos(tra21)*cos(tra23)-sin(tra21)*sin(tra22)*sin(tra23), a2=-cos(tra21)*sin(tra23)-sin(tra21)*sin(tra22)*cos(tra23), a3=-sin(tra21)*cos(tra22), b1=cos(tra22)*sin(tra23), b2=cos(tra22)*cos(tra23), b3=-sin(tra22), c1=sin(tra21)*cos(tra23)+cos(tra21)*sin(tra22)*sin(tra23), c2=-sin(tra21)*sin(tra23)+cos(tra21)*sin(tra22)*cos(tra23), c3=cos(tra21)*cos(tra22); a4=a1; a5=a2; a6=a3; b4=b1; b5=b2; b6=b3; c4=c1; c5=c2; c6=c3; break; } } ///******************************************///计算第三个文件 DATA2 data32[11]; double data33[11][5]; double data34[5][5]; double R3[5][11]; double A3[5][11]; double L3[11]; double by3; double bz3; for(i3=1;;i3++) { //R1=E R2由初值迭代计算 //计算初始值 U3=U3+result3[0], V3=V3+result3[1], tra31=tra31+result3[2], tra32=tra32+result3[3], tra33=tra33+result3[4]; by3=bx*U3; bz3=bx*V3; //矩阵九项各值 double a1=cos(tra31)*cos(tra33)-sin(tra31)*sin(tra32)*sin(tra33), a2=-cos(tra31)*sin(tra33)-sin(tra31)*sin(tra32)*cos(tra33), a3=-sin(tra31)*cos(tra32), b1=cos(tra32)*sin(tra33), b2=cos(tra32)*cos(tra33), b3=-sin(tra32), c1=sin(tra31)*cos(tra33)+cos(tra31)*sin(tra32)*sin(tra33), c2=-sin(tra31)*sin(tra33)+cos(tra31)*sin(tra32)*cos(tra33), c3=cos(tra31)*cos(tra32); //计算XYZ像空间坐标系 for(int i=0;i<11;i++) { //计算由像点与矩阵R相乘后的坐标XYZ data32[i].index=data3[i].index; data32[i].X1=a4*data3[i].x1+a5*data3[i].y1+a6*(-f); data32[i].X2=a1*data3[i].x2+a2*data3[i].y2+a3*(-f); data32[i].Y1=b4*data3[i].x1+b5*data3[i].y1+b6*(-f); data32[i].Y2=b1*data3[i].x2+b2*data3[i].y2+b3*(-f); data32[i].Z1=c4*data3[i].x1+c5*data3[i].y1+c6*(-f); data32[i].Z2=c1*data3[i].x2+c2*data3[i].y2+c3*(-f); //计算投影系数 N13[i]=(bx*data32[i].Z2-bz3*data32[i].X2)/(data32[i].X1*data32[i].Z2-data32[i].X2*data32[i].Z1); N23[i]=(bx*data32[i].Z1-bz3*data32[i].X1)/(data32[i].X1*data32[i].Z2-data32[i].X2*data32[i].Z1); //计算矩阵A data33[i][0]=bx; data33[i][1]=-(data32[i].Y2/data32[i].Z2)*bx; data33[i][2]=-(data32[i].X2*data32[i].Y2*N23[i])/data32[i].Z2; data33[i][3]=-(data32[i].Z2+(data32[i].Y2*data32[i].Y2)/data32[i].Z2)*N23[i]; data33[i][4]=data32[i].X2*N23[i]; //计算矩阵L L3[i]=N13[i]*data32[i].Y1-N23[i]*data32[i].Y2-by3; } for (int i=0;i<11;i++) { for (int j=0;j<5;j++) { R3[j][i]=data33[i][j]; } } double t; for(int i=0;i<5;i++) { for (int m=0;m<5;m++) { t=0; for (int j=0;j<11;j++) { t=R3[i][j]*data33[j][m]+t; } data34[i][m]=t; } } InverseMatrix(*data34,5); //计算(ATA)-1AT double tt; for(int i=0;i<5;i++) { for (int m=0;m<11;m++) { tt=0; for (int j=0;j<5;j++) { tt=data34[i][j]*R3[j][m]+tt; } A3[i][m]=tt; } } for(int i=0;i<5;i++) { double o=0; for( int j=0;j<11;j++) { o=A3[i][j]*L3[j]+o; } result3[i]=o; } if(fabs(result3[0])<0.00003 && fabs(result3[1])<0.00003 && fabs(result3[2])<0.00003 && fabs(result3[3])<0.00003 & fabs(result3[4])<0.00003 ) { break; } } //********************************************相对定向结束 //计算模型点坐标(没有经过改正的) for(int i=0;i<11;i++) { mddata3[i].index=data3[i].index; mddata3[i].X1=data32[i].X1*N13[i]; mddata3[i].Z1=data32[i].Z1*N13[i]; mddata3[i].Y1=(data32[i].Y1*N13[i]+data32[i].Y2*N23[i]+by3)/2; } for(int i=0;i<8;i++) { mddata2[i].index=data2[i].index; mddata2[i].X1=data22[i].X1*N12[i]; mddata2[i].Z1=data22[i].Z1*N12[i]; mddata2[i].Y1=(data22[i].Y1*N12[i]+data22[i].Y2*N22[i]+by2)/2; } for(int i=0;i<15;i++) { mddata1[i].index=data1[i].index; mddata1[i].X1=data12[i].X1*N11[i]; mddata1[i].Z1=data12[i].Z1*N11[i]; mddata1[i].Y1=(data12[i].Y1*N11[i]+data12[i].Y2*N21[i]+by1)/2; } // 计算三个公共点的比例尺变换系数 double K1[3]={0},k1=0; int count1=0; double K2[3]={0},k2=0; int count2=0; for (int i=0;i<15;i++) { for (int j=0;j<8;j++) { if (data1[i].index==data2[j].index) { K2[count2]=(mddata1[i].Z1-bz1)/(mddata2[j].Z1); k1=k1+K2[count2]; count2++; } } } for (int i=0;i<8;i++) { for (int j=0;j<11;j++) { if (data22[i].index==data32[j].index) { K1[count1]=(mddata2[i].Z1-bz2)/(mddata3[j].Z1); k2=k2+K1[count1]; count1++; } } } k1=k1/3; k2=k2/3; //**************************************************计算比例尺开始 //求控制点对应的像点 DATA4 P1[10]={0}; int point1 =0; for (int i=0;i<15;i++) { for (int j=0;j<4;j++) { if (mddata1[i].index==data4[j].index) { P1[point1].index=mddata1[i].index; P1[point1].x=mddata1[i].X1; P1[point1].y=mddata1[i].Y1; P1[point1].z=mddata1[i].Z1; point1++; } } } DATA4 P2[10]={0}; int point2 =0; for (int i=0;i<11;i++) { for (int j=0;j<4;j++) { if (mddata3[i].index==data4[j].index) { P2[point2].index=mddata3[i].index; P2[point2].x=mddata3[i].X1; P2[point2].y=mddata3[i].Y1; P2[point2].z=mddata3[i].Z1; point2++; } } } double blc[6]; //像点距离 double blc5[6]; //控制点坐标的距离 //求控制点对应的的图上距离 int i=1; blc[0]=sqrt((P1[i].x-P1[i+1].x)*(P1[i].x-P1[i+1].x)+(P1[i].y-P1[i+1].y)*(P1[i].y-P1[i+1].y)); blc[1]=sqrt((P1[i].x-P1[i-1].x)*(P1[i].x-P1[i-1].x)+(P1[i].y-P1[i-1].y)*(P1[i].y-P1[i-1].y)); blc[2]=sqrt((P1[i-1].x-P1[i+1].x)*(P1[i-1].x-P1[i+1].x)+(P1[i-1].y-P1[i+1].y)*(P1[i-1].y-P1[i+1].y)); blc[3]=sqrt((P2[0].x-P1[i+1].x)*(P2[0].x-P1[i+1].x)+(P2[0].y-P1[i+1].y)*(P2[0].y-P1[i+1].y)); blc[4]=sqrt((P2[0].x-P1[i].x)*(P2[0].x-P1[i].x)+(P2[0].y-P1[i].y)*(P2[0].y-P1[i].y)); blc[5]=sqrt((P2[0].x-P1[i-1].x)*(P2[0].x-P1[i-1].x)+(P2[0].y-P1[i-1].y)*(P2[0].y-P1[i-1].y)); //求控制点的实际距离 blc5[0]=sqrt((data4[0].X-data4[1].X)*(data4[0].X-data4[1].X)+(data4[0].Y-data4[1].Y)*(data4[0].Y-data4[1].Y)); blc5[1]=sqrt((data4[2].X-data4[1].X)*(data4[2].X-data4[1].X)+(data4[2].Y-data4[1].Y)*(data4[2].Y-data4[1].Y)); blc5[2]=sqrt((data4[3].X-data4[1].X)*(data4[3].X-data4[1].X)+(data4[3].Y-data4[1].Y)*(data4[3].Y-data4[1].Y)); blc5[3]=sqrt((data4[0].X-data4[2].X)*(data4[0].X-data4[2].X)+(data4[0].Y-data4[2].Y)*(data4[0].Y-data4[2].Y)); blc5[4]=sqrt((data4[0].X-data4[3].X)*(data4[0].X-data4[3].X)+(data4[0].Y-data4[3].Y)*(data4[0].Y-data4[3].Y)); blc5[5]=sqrt((data4[2].X-data4[3].X)*(data4[2].X-data4[3].X)+(data4[2].Y-data4[3].Y)*(data4[2].Y-data4[3].Y)); //开始计算比例尺 double H[6],h0; H[0]=blc5[1]/blc[0]; H[1]=blc5[0]/blc[1]; H[2]=blc5[3]/blc[2]; H[3]=blc5[5]/blc[3]; H[4]=blc5[2]/blc[4]; H[5]=blc5[4]/blc[5]; h0=(H[0]+H[1]+H[2]/*+H[3]+H[4]+H[5]*/)/3; //********************************************比例尺结束 //各模型摄站摄影测量坐标 double Xps[3],Yps[3],Zps[3]; for(int i=0;i<3;i++) { if(i==0) { Xps[0]=0; Yps[0]=0; Zps[0]=h0*f; } else if(i==1) { Xps[i] = Xps[i - 1] + h0 * bx; Yps[i] = Yps[i - 1] + h0 * by1; Zps[i] = Zps[i - 1] + h0 * bz1; } else { Xps[i] = Xps[i - 1] + h0 *k1* bx; Yps[i] = Yps[i - 1] + h0 * k1*by2; Zps[i] = Zps[i - 1] + h0 * k1*bz2; } } //计算最终的模型点坐标 for (int i=0;i<15;i++) { mddata1[i].X1=mddata1[i].X1*h0; mddata1[i].Z1=mddata1[i].Z1*h0+h0*f; mddata1[i].Y1=mddata1[i].Y1*h0; } for (int i=0;i<8;i++) { mddata2[i].X1=Xps[1]+k1*mddata2[i].X1*h0; mddata2[i].Y1=(Yps[1]+k1*h0*N12[i]*data22[i].Y1+k1*h0*N22[i]*data22[i].Y2+Yps[2])/2; mddata2[i].Z1=Zps[1]+k1*mddata2[i].Z1*h0; } for (int i=0;i<11;i++) { mddata3[i].X1=Xps[2]+k2*k1*mddata3[i].X1*h0; mddata3[i].Y1=(Yps[2]+k2*k1*h0*N13[i]*data32[i].Y1+k2*k1*h0*N23[i]*data32[i].Y2+Yps[2]+k2*k1*h0*by3)/2; mddata3[i].Z1=Zps[2]+k2*k1*mddata3[i].Z1*h0; } // 求dx dy double dXt,dYt,dXp,dYp; dXt=data4[0].X-data4[3].X; dYt=data4[0].Y-data4[3].Y; dXp=mddata1[0].X1-mddata3[8].X1; dYp=mddata1[0].Y1-mddata3[8].Y1; //计算转换系数a b T double a,b,T; a=(dXp*dYt+dYp*dXt)/((dXt*dXt)+(dYt*dYt)); b=(dXp*dXt-dYp*dYt)/((dXt*dXt)+(dYt*dYt)); T=sqrt(a*a+b*b); //计算变换成的地面摄影测量坐标Xtp DATA3 data43[4]; for (int i=0;i<4;i++) { data43[i].X=b*(data4[i].X-data4[0].X)+(data4[i].Y-data4[0].Y)*a; data43[i].Y=a*(data4[i].X-data4[0].X)+(data4[i].Y-data4[0].Y)*(-b); data43[i].Z=T*(data4[i].Z-data4[0].Z); } //摄影测量坐标Xp DATA4 P3[10]={0}; point1 =0; for (int i=0;i<15;i++) { for (int j=0;j<4;j++) { if (mddata1[i].index==data4[j].index) { P3[point1].index=mddata1[i].index; P3[point1].x=mddata1[i].X1; P3[point1].y=mddata1[i].Y1; P3[point1].z=mddata1[i].Z1; point1++; } } } DATA4 P4[10]={0}; point2 =0; for (int i=0;i<11;i++) { for (int j=0;j<4;j++) { if (mddata3[i].index==data4[j].index) { P4[point2].index=mddata3[i].index; P4[point2].x=mddata3[i].X1; P4[point2].y=mddata3[i].Y1; P4[point2].z=mddata3[i].Z1; point2++; } } } //计算地面摄影测量坐标和摄影测量坐标 DATA3 tp[4],p[4]; for (int i=0;i<4;i++) { tp[i].X=data43[i].X; tp[i].Y=data43[i].Y; tp[i].Z=data43[i].Z; } for (int i=0;i<3;i++) { p[i].X=P3[i].x; p[i].Y=P3[i].y; p[i].Z=P3[i].z; } p[3].X=P4[0].x; p[3].Y=P4[0].y; p[3].Z=P4[0].z; //*******************************************计算绝对定向元素 double T0=1,X0=0,Y0=0,Z0=0,tra1=0,tra2=0,tra3=0; double result4[7]={0}; double L4[12]={0}; double A[12][7]={0}; double AT[7][12]; double ATA[7][7]; double AA[7][12]; int ii=0; for(ii=1; ;ii++) { //R1=E R2由初值迭代计算 //计算初始值 X0=X0+result4[0], Y0=Y0+result4[1], Z0=Z0+result4[2], T0=T0+result4[3], tra1=tra1+result4[4], tra2=tra2+result4[5], tra3=tra3+result4[6]; //矩阵九项各值 double a1=cos(tra1)*cos(tra3)-sin(tra1)*sin(tra2)*sin(tra3), a2=-cos(tra1)*sin(tra3)-sin(tra1)*sin(tra2)*cos(tra3), a3=-sin(tra1)*cos(tra2), b1=cos(tra2)*sin(tra3), b2=cos(tra2)*cos(tra3), b3=-sin(tra2), c1=sin(tra1)*cos(tra3)+cos(tra1)*sin(tra2)*sin(tra3), c2=-sin(tra1)*sin(tra3)+cos(tra1)*sin(tra2)*cos(tra3), c3=cos(tra1)*cos(tra2); //计算矩阵L for (int i=0;i<4;i++) { int j=i*3; L4[j]=tp[i].X-T0*(a1*p[i].X+a2*p[i].Y+a3*p[i].Z)-X0; L4[j+1]=tp[i].Y-T0*(b1*p[i].X+b2*p[i].Y+b3*p[i].Z)-Y0; L4[j+2]=tp[i].Z-T0*(c1*p[i].X+c2*p[i].Y+c3*p[i].Z)-Z0; } //计算A矩阵 for(int i=0;i<12;i++) { for (int j=0;j<7;j++) { if (i%3==j) { A[i][j]=1; } else if ((i==0 || i==3|| i==6|| i==9)&&j==3) { A[i][j]=p[i/3].X; } else if ((i==2 || i==5|| i==8|| i==11)&&j==4) { A[i][j]=p[i/3].X; } else if ((i==1 || i==4|| i==7|| i==10)&&j==6) { A[i][j]=p[i/3].X; } else if ((i==1 || i==4|| i==7|| i==10)&&j==3) { A[i][j]=p[i/3].Y; } else if ((i==2 || i==5|| i==8|| i==11)&&j==5) { A[i][j]=p[i/3].Y; } else if ((i==0 || i==3|| i==6|| i==9)&&j==6) { A[i][j]=-p[i/3].Y; } else if ((i==2 || i==5|| i==8|| i==11)&&j==3) { A[i][j]=p[i/3].Z; } else if ((i==1 || i==4|| i==7|| i==10)&&j==5) { A[i][j]=-p[i/3].Z; } else if ((i==0 || i==3|| i==6|| i==9)&&j==4) { A[i][j]=-p[i/3].Z; } } } //计算A的转置 for (int i=0;i<12;i++) { for (int j=0;j<7;j++) { AT[j][i]=A[i][j]; } } //计算A的转置和A的乘积 double t; for(int i=0;i<7;i++) { for (int m=0;m<7;m++) { t=0; for (int j=0;j<12;j++) { t=AT[i][j]*A[j][m]+t; } ATA[i][m]=t; } } //求逆 InverseMatrix(*ATA,7); //计算(ATA)-1AT double tt; for(int i=0;i<7;i++) { for (int m=0;m<12;m++) { tt=0; for (int j=0;j<7;j++) { tt=ATA[i][j]*AT[j][m]+tt; } AA[i][m]=tt; } } //计算最终改正结果 for(int i=0;i<7;i++) { double o=0; for( int j=0;j<12;j++) { o=AA[i][j]*L4[j]+o; } result4[i]=o; } if(fabs(result4[0])<0.000001 && fabs(result4[1])<0.000001 && fabs(result4[2])<0.000001 && fabs(result4[3])<0.000001 && fabs(result4[4])<0.000001 && fabs(result4[5])<0.000001 && fabs(result4[6])<0.000001 ) { break; } } //*******************************************计算绝对定向元素结束 // 把摄影测量坐标变换为地面摄影测量坐标 X0=X0+result4[0], Y0=Y0+result4[1], Z0=Z0+result4[2], T0=T0+result4[3], tra1=tra1+result4[4], tra2=tra2+result4[5], tra3=tra3+result4[6]; DATA3 down1[15],down2[8],down3[11]; for (int i=0;i<15;i++) { down1[i].index=mddata1[i].index; down1[i].X=T0*(mddata1[i].X1-tra3*mddata1[i].Y1-tra1*mddata1[i].Z1)+X0; down1[i].Y=T0*(tra3*mddata1[i].X1+mddata1[i].Y1-tra2*mddata1[i].Z1)+Y0; down1[i].Z=T0*(tra1*mddata1[i].X1+tra2*mddata1[i].Y1+mddata1[i].Z1)+Z0; } for (int i=0;i<8;i++) { down2[i].index=mddata2[i].index; down2[i].X=T0*(mddata2[i].X1-tra3*mddata2[i].Y1-tra1*mddata2[i].Z1)+X0; down2[i].Y=T0*(tra3*mddata2[i].X1+mddata2[i].Y1-tra2*mddata2[i].Z1)+Y0; down2[i].Z=T0*(tra1*mddata2[i].X1+tra2*mddata2[i].Y1+mddata2[i].Z1)+Z0; } for (int i=0;i<11;i++) { down3[i].index=mddata3[i].index; down3[i].X=T0*(mddata3[i].X1-tra3*mddata3[i].Y1-tra1*mddata3[i].Z1)+X0; down3[i].Y=T0*(tra3*mddata3[i].X1+mddata3[i].Y1-tra2*mddata3[i].Z1)+Y0; down3[i].Z=T0*(tra1*mddata3[i].X1+tra2*mddata3[i].Y1+mddata3[i].Z1)+Z0; } //计算最终的大地坐标 DATA3 finish1[15],finish2[8],finish3[11]; for (int i=0;i<15;i++) { finish1[i].index=down1[i].index; finish1[i].X=(b*down1[i].X+a*down1[i].Y)/T/T+data4[0].X; finish1[i].Y=(a*down1[i].X-b*down1[i].Y)/T/T+data4[0].Y; finish1[i].Z=(down1[i].Z)/T+data4[0].Z; } for (int i=0;i<8;i++) { finish2[i].index=down2[i].index; finish2[i].X=(b*down2[i].X+a*down2[i].Y)/T/T+data4[0].X; finish2[i].Y=(a*down2[i].X-b*down2[i].Y)/T/T+data4[0].Y; finish2[i].Z=(down2[i].Z)/T+data4[0].Z; } for (int i=0;i<11;i++) { finish3[i].index=down3[i].index; finish3[i].X=(b*down3[i].X+a*down3[i].Y)/T/T+data4[0].X; finish3[i].Y=(a*down3[i].X-b*down3[i].Y)/T/T+data4[0].Y; finish3[i].Z=(down3[i].Z)/T+data4[0].Z; } //计算差值 double D[5][4]={0}; for(i=0;i<5;i++) { for(int j=0;j<15;j++) { if(data5[i].index==finish1[j].index) { D[i][0]=data5[i].index; D[i][1]=data5[i].x1-finish1[j].X; D[i][2]=data5[i].y1-finish1[j].Y; D[i][3]=data5[i].z1-finish1[j].Z; break; } } for(int j=0;j<8;j++) { if(data5[i].index==finish2[j].index) { D[i][0]=data5[i].index; D[i][1]=data5[i].x1-finish2[j].X; D[i][2]=data5[i].y1-finish2[j].Y; D[i][3]=data5[i].z1-finish2[j].Z; break; } } for(int j=0;j<11;j++) { if(data5[i].index==finish3[j].index) { D[i][0]=data5[i].index; D[i][1]=data5[i].x1-finish3[j].X; D[i][2]=data5[i].y1-finish3[j].Y; D[i][3]=data5[i].z1-finish3[j].Z; break; } } } //写入文件 FILE *fp2; fp2=fopen("result.txt","w"); fprintf(fp2," 相对定向最终结果:\n " ); fprintf(fp2," %10.8f %f %f %f %f \n",U1,V1,tra11,tra12,tra13); fprintf(fp2," %10.8f %f %f %f %f \n",U2,V2,tra21,tra22,tra23); fprintf(fp2," %10.8f %f %f %f %f \n",U3,V3,tra31,tra32,tra33); fprintf(fp2," 循环次数:\n " ); fprintf(fp2," %d \n",i1); fprintf(fp2," %d \n",i2); fprintf(fp2," %d \n",i3); fprintf(fp2," 最后一次的循环改正数:\n " ); fprintf(fp2," U V φ0 ω0 k0 \n"); for(int j=0;j<5;j++) { fprintf(fp2," %.18f ",result1[j]); } fprintf(fp2," \n"); for(int j=0;j<5;j++) { fprintf(fp2," %.18f ",result2[j]); printf("\n" ); } fprintf(fp2," \n"); for(int j=0;j<5;j++) { fprintf(fp2," %.18f ",result3[j]); } fprintf(fp2,"\n计算出的比例尺 \n"); fprintf(fp2," h0 = %f ",h0); fprintf(fp2,"\n计算出的 k1 k2 \n"); fprintf(fp2," k1 k2 \n"); fprintf(fp2," %f %f ",k1,k2); fprintf(fp2,"\n计算出的 a b T \n"); fprintf(fp2," a b T \n"); fprintf(fp2," %f %f %f",a,b,T); fprintf(fp2,"\n计算出的绝对定向元素 \n"); fprintf(fp2," T0 X0 Y0 Z0 tra1 tra2 tra3\n"); fprintf(fp2," %f %f %f %f %f %f %f \n",T0,X0,Y0,Z0,tra1,tra2,tra3); fprintf(fp2,"\n 最后的大地坐标 \n"); for(int j=0;j<15;j++) { fprintf(fp2," %f %f %f \n",finish1[i].X,finish1[i].Y,finish1[i].Z); } fprintf(fp2,"\n"); for(int j=0;j<8;j++) { fprintf(fp2," %f %f %f\n ",finish2[i].X,finish2[i].Y,finish2[i].Z); } fprintf(fp2,"\n"); for(int j=0;j<11;j++) { fprintf(fp2," %f %f %f \n",finish3[i].X,finish3[i].Y,finish3[i].Z); } fprintf(fp2,"\n 与待定点的差值 \n"); for(int j=0;j<5;j++) { fprintf(fp2," %f %f %f \n", D[j][1], D[j][2], D[j][3]); } fprintf(fp2,"\n"); fclose(fp2); //写入文件结束 return 0; }