sheyingceliang.rar_后方交会_摄影测量_摄影测量代码

#include <stdio.h> #include <math.h> #include <stdlib.h> #define N 4 int m; double xiangzb[4][2],wuzb[4][3],R[3][3]; double X0,Y0,Z0,a,b,c; //定义初值 double a1,a2,a3,b1,b2,b3,c1,c2,c3; //定义R矩阵的各个元素 double f=0.15324; //定义焦距 double tempx[4],tempy[4]; //保存像点坐标的近似值 double temp1[4],temp2[4],temp3[4]; double C[6][12],D[6][8],E[6]; //存放中间结果 double A[4][2][6],L[4][2]; double X[4][6],A1[8][6],L1[8]; void bilichi() { double xiangju,wuju; xiangju=(xiangzb[2][0]-xiangzb[0][0])*(xiangzb[2][0]-xiangzb[0][0])+(xiangzb[2][1]-xiangzb[0][1])*(xiangzb[2][1]-xiangzb[0][1]); wuju=(wuzb[2][0]-wuzb[0][0])*(wuzb[2][0]-wuzb[0][0])+(wuzb[2][1]-wuzb[0][1])*(wuzb[2][1]-wuzb[0][1]); xiangju=sqrt(xiangju); wuju=sqrt(wuju); m=(int)(wuju/xiangju/10000+0.5); m=m*10000; } void qiuchuzhi() //求初值 { bilichi(); for(int i=0;i<4;i++) { X0+=wuzb[i][0]; Y0+=wuzb[i][1]; Z0+=wuzb[i][2]; } X0=X0/4; Y0=Y0/4; Z0=m*f+Z0/4; } void qiuR() { R[0][0]=a1=cos(a)*cos(c)-sin(a)*sin(b)*sin(c); R[0][1]=a2=-cos(a)*sin(c)-sin(a)*sin(b)*cos(c); R[0][2]=a3=-sin(a)*cos(b); R[1][0]=b1=cos(b)*sin(c); R[1][1]=b2=cos(b)*cos(c); R[1][2]=b3=-sin(b); R[2][0]=c1=sin(a)*cos(c)+cos(a)*sin(b)*sin(c); R[2][1]=c2=-sin(a)*sin(c)+cos(a)*sin(b)*cos(c); R[2][2]=c3=cos(a)*cos(b); } void qiuxiangzbjinshizhi() { int i; for(i=0;i<4;i++) //逐点计算像点坐标的近似值 { temp1[i]=a1*(wuzb[i][0]-X0)+b1*(wuzb[i][1]-Y0)+c1*(wuzb[i][2]-Z0);//代入共线方程 temp2[i]=a2*(wuzb[i][0]-X0)+b2*(wuzb[i][1]-Y0)+c2*(wuzb[i][2]-Z0); temp3[i]=a3*(wuzb[i][0]-X0)+b3*(wuzb[i][1]-Y0)+c3*(wuzb[i][2]-Z0); tempx[i]=(-1)*f*temp1[i]/temp3[i];//求得像点坐标的近似值 tempy[i]=(-1)*f*temp2[i]/temp3[i]; } } void wuchafangcheng() { int i,j,k; double m[6]={0,0,0,0,0,0}; double n[6]={0,0,0,0,0,0}; for(i=0;i<N;i++) { A[i][0][0]=(a1*f+a3*tempx[i])/temp3[i]; A[i][0][1]=(b1*f+b3*tempx[i])/temp3[i]; A[i][0][2]=(c1*f+c3*tempx[i])/temp3[i]; A[i][0][3]=tempy[i]*sin(b)-(tempx[i]/f*(tempx[i]*cos(c)-tempy[i]*sin(c))+f*cos(c))*cos(b); A[i][0][4]=(-1)*f*sin(c)-tempx[i]/f*(tempx[i]*sin(c)+tempy[i]*cos(c)); A[i][0][5]=tempy[i]; A[i][1][0]=(a2*f+a3*tempy[i])/temp3[i]; A[i][1][1]=(b2*f+b3*tempy[i])/temp3[i]; A[i][1][2]=(c2*f+c3*tempy[i])/temp3[i]; A[i][1][3]=-tempx[i]*sin(b)-(tempy[i]/f*(tempx[i]*cos(c)-tempy[i]*sin(c))-f*sin(c))*cos(b); A[i][1][4]=(-1)*f*cos(c)-tempy[i]/f*(tempx[i]*sin(c)+tempy[i]*cos(c)); A[i][1][5]=(-1)*tempx[i]; L[i][0]=xiangzb[i][0]-tempx[i]; L[i][1]=xiangzb[i][1]-tempy[i]; } for(i=0;i<4;i++) //将四个控制点的系数矩阵组合成一个矩阵 for(j=0;j<6;j++) { A1[2*i][j]=A[i][0][j]; A1[2*i+1][j]=A[i][1][j]; } for(i=0;i<4;i++) //将L矩阵也类似转化为8*1矩阵 { L1[2*i]=L[i][0]; L1[2*i+1]=L[i][1]; } for(i=0;i<6;i++) for(j=0;j<6;j++) { C[i][j]=0; for(k=0;k<8;k++) C[i][j]+=A1[k][i]*A1[k][j]; } for(i=0;i<6;i++){C[0][6+i]=0;}C[0][6]=1; for(i=0;i<6;i++){C[1][6+i]=0;}C[1][7]=1; for(i=0;i<6;i++){C[2][6+i]=0;}C[2][8]=1; for(i=0;i<6;i++){C[3][6+i]=0;}C[3][9]=1; for(i=0;i<6;i++){C[4][6+i]=0;}C[4][10]=1; for(i=0;i<6;i++){C[5][6+i]=0;}C[5][11]=1; for(i=0;i<6;i++) { n[i]=C[i][i]; for(j=i;j<12;j++) { if(n[i]==0) { printf("不存在逆矩阵\n"); exit(0); } C[i][j]=C[i][j]/n[i]; } for(j=0;j<6;j++) { if(i==j)continue; m[j]=C[j][i]; for(k=0;k<12;k++) C[j][k]=C[j][k]-C[i][k]*m[j]; } } //C矩阵的后面部分就是逆矩阵了 for(i=0;i<6;i++) for(j=0;j<8;j++) { D[i][j]=0; for(k=0;k<6;k++) D[i][j]+=C[i][k+6]*A1[j][k]; } for(i=0;i<6;i++) for(j=0;j<1;j++) { E[i]=0; for(k=0;k<8;k++) E[i]+=D[i][k]*L1[k]; } X0=X0+E[0]; Y0=Y0+E[1]; Z0=Z0+E[2]; a=a+E[3]; b=b+E[4]; c=c+E[5]; } void main() { int i,j,count=0; double as,bs,cs; xiangzb[0][0]=-0.08615;xiangzb[0][1]=-0.06899; xiangzb[1][0]=-0.05340;xiangzb[1][1]=0.08221; xiangzb[2][0]=-0.01478;xiangzb[2][1]=-0.07663; xiangzb[3][0]=0.01046;xiangzb[3][1]=0.06443; wuzb[0][0]=36589.41;wuzb[0][1]=25273.32;wuzb[0][2]=2195.17; wuzb[1][0]=37631.08;wuzb[1][1]=31324.51;wuzb[1][2]=728.69; wuzb[2][0]=39100.97;wuzb[2][1]=24934.98;wuzb[2][2]=2386.50; wuzb[3][0]=40426.54;wuzb[3][1]=30319.81;wuzb[3][2]=757.31; qiuchuzhi(); do { qiuR(); qiuxiangzbjinshizhi(); wuchafangcheng(); count++; as=fabs(E[3]); bs=fabs(E[4]); cs=fabs(E[5]); }while( as>0.0000001 || bs>0.0000001 || cs>0.0000001); printf("Xs=%lf\nYs=%lf\nZs=%lf\n",X0,Y0,Z0); printf("R矩阵为:\n"); for(i=0;i<3;i++) { for(j=0;j<3;j++) printf("%f ",R[i][j]); printf("\n"); } }