printf(\各外方位元素精度为：\\n\
printf(\4],mi[5]); printf(\迭代次数为：%d\\n\\n\\n\\n\ fclose(fp); }
void youbian() { FILE *fp = NULL; FILE *fp1 = NULL; if((fp=fopen(\ { printf(\ return; } if((fp1=fopen(\ { printf(\ return; }
//像点坐标和地面点坐标 double imagecontrol[4][2]={0.0}; double groundcontrol[4][3]={0.0}; //摄影比例尺分母 double m = 10177; double f=0.15; long i,j,k;
for(i=0; i<4; i++) { for(j=0; j<2; j++) { fscanf(fp, \ imagecontrol[i][j] /= 1000.0; } for(k=0; k<3; k++) { fscanf(fp1, \ }
} fclose(fp); fclose(fp1);
//计算外方位元素初始值 for( i=0;i<4;i++) { Xs2+=groundcontrol[i][0]; Ys2+=groundcontrol[i][1]; Zs2+=groundcontrol[i][2]; } Xs2/=4.0; Ys2/=4.0; Zs2/=4.0; Zs2+=m*f; double R[3][3]={0.0}; double L3=0.0,L1=0.0,L2=0.0; double L[8][1]={0.0},x=0.0,y=0.0; double A[8][6]={0.0},AT[6][8]={0.0},ATA[6][6]={0.0},B[6][8]={0.0}; double V[6][1]={0.0}; int n=0; do { R[0][0]=cos(p02)*cos(k02)-sin(p02)*sin(w02)*sin(k02); R[0][1]=(-1)*cos(p02)*sin(k02)-sin(p02)*sin(w02)*cos(k02); R[0][2]=(-1)*sin(p02)*cos(w02); R[1][0]=cos(w02)*sin(k02); R[1][1]=cos(w02)*cos(k02); R[1][2]=(-1)*sin(w02); R[2][0]=sin(p02)*cos(k02)+cos(p02)*sin(w02)*sin(k02); R[2][1]=(-1)*sin(p02)*sin(k02)+cos(p02)*sin(w02)*cos(k02); R[2][2]=cos(p02)*cos(w02); for(i=0,j=0;j<4;i+=2,j++) { //计算像点坐标的近似值 L1=R[0][0]*(groundcontrol[j][0]-Xs2)+R[1][0]*(groundcontrol[j][1]-Ys2)+R[2][0]*(groundcontrol[j][2]-Zs2);
L2=R[0][1]*(groundcontrol[j][0]-Xs2)+R[1][1]*(groundcontrol[j][1]-Ys2)+R[2][1]*(groundcontrol[j][2]-Zs2); L3=R[0][2]*(groundcontrol[j][0]-Xs2)+R[1][2]*(groundcontrol[j][1]-Ys2)+R[2][2]*(groundcontrol[j][2]-Zs2); x=(-1)*f*L1/L3; y=(-1)*f*L2/L3;
//计算常数项矩阵 L[2*j][0]=imagecontrol[j][0]-x; L[2*j+1][0]=imagecontrol[j][1]-y; //计算系数矩阵 A[i][0]=(R[0][0]*f+R[0][2]*imagecontrol[j][0])/L3; A[i][1]=(R[1][0]*f+R[1][2]*imagecontrol[j][0])/L3; A[i][2]=(R[2][0]*f+R[2][2]*imagecontrol[j][0])/L3; A[i][3]=imagecontrol[j][1]*sin(w02)-((imagecontrol[j][0]/f)*(imagecontrol[j][0]*cos(k02)-imagecontrol[j][1]*sin(k02))+f*cos(k02))*cos(w02); A[i][4]=(-1)*f*sin(k02)-(imagecontrol[j][0]/f)*(imagecontrol[j][0]*sin(k02)+imagecontrol[j][1]*cos(k02)); A[i][5]=imagecontrol[j][1]; A[i+1][0]=(R[0][1]*f+R[0][2]*imagecontrol[j][1])/L3; A[i+1][1]=(R[1][1]*f+R[1][2]*imagecontrol[j][1])/L3; A[i+1][2]=(R[2][1]*f+R[2][2]*imagecontrol[j][1])/L3; A[i+1][3]=(-1)*imagecontrol[j][0]*sin(w02)-((imagecontrol[j][1]/f)*(imagecontrol[j][0]*cos(k02)-imagecontrol[j][1]*sin(k02))-f*sin(k02))*cos(w02); A[i+1][4]=(-1)*f*cos(k02)-(imagecontrol[j][1]/f)*(imagecontrol[j][0]*sin(k02)+imagecontrol[j][1]*cos(k02)); A[i+1][5]=(-1)*imagecontrol[j][0]; } transpose(&A[0][0],&AT[0][0],8,6); multiply(&AT[0][0],&A[0][0],&ATA[0][0],6,8,6); inMerse1(*ATA,6);
multiply(&ATA[0][0],&AT[0][0],&B[0][0],6,6,8); multiply(&B[0][0],&L[0][0],&V[0][0],6,8,1);
Xs2+=V[0][0]; Ys2+=V[1][0]; Zs2+=V[2][0]; p02+=V[3][0]; w02+=V[4][0]; k02+=V[5][0]; n++; }while(fabs(V[3][0])>=0.00001||fabs(V[4][0])>=0.00001||fabs(V[5][0])>=0.00001); R[0][0]=cos(p02)*cos(k02)-sin(p02)*sin(w02)*sin(k02); R[0][1]=(-1)*cos(p02)*sin(k02)-sin(p02)*sin(w02)*cos(k02); R[0][2]=(-1)*sin(p02)*cos(w02); R[1][0]=cos(w02)*sin(k02); R[1][1]=cos(w02)*cos(k02); R[1][2]=(-1)*sin(w02); R[2][0]=sin(p02)*cos(k02)+cos(p02)*sin(w02)*sin(k02); R[2][1]=(-1)*sin(p02)*sin(k02)+cos(p02)*sin(w02)*cos(k02); R[2][2]=cos(p02)*cos(w02); //进行未知数的精度评定 double AV[8][1],X[8][1],XT[1][8],XTX[1][1],mo,D[6][6],mi[6]; multiply(&A[0][0],&V[0][0],&AV[0][0],8,6,1); for(i=0;i<8;i++) X[i][0]=AV[i][0]-L[i][0]; transpose(&X[0][0],&XT[0][0],8,1); multiply(&XT[0][0],&X[0][0],&XTX[0][0],1,8,1); mo=XTX[0][0]/2; for(i=0;i<6;i++) for(j=0;j<6;j++) D[i][j]=mo*ATA[i][j]; for(i=0;i<6;i++) { mi[i]=sqrt(D[i][i]); } printf(\右片结果为：\\n\\n\
printf(\旋转矩阵R为：\\n\ shuchu(&R[0][0],3,3); printf(\外方为元素为：\\n\ printf(\ printf(\ printf(\ printf(\ printf(\ printf(\ printf(\各外方位元素精度为：\\n\
printf(\4],mi[5]); printf(\迭代次数为：%d\\n\ fclose(fp); }
//求矩阵a的转置矩阵b，a为m行、n列 void transpose(double *a, double *b, int m, int n) { int i,j; for(i=0;i //矩阵a乘以矩阵b，结果存储在c中，a为m×n大小，b为n×l大小 void multiply(double *a, double *b, double *c, int m, int n, int l) { int i,j,k; double t; for(i=0;i