摄影测量学单像空间后方交会程序设计作业

usingSystem;usingSystem.Collections.Generic;usingSystem.Linq;usingSystem.Text;namespace单像空间后方交会{classProgram{staticvoidMain(string[]args){intx0,y0,i,j;doublef,m;Console.Write(请输入像片比例尺：);m=double.Parse(Console.ReadLine());Console.Write(请输入像片的内方位元素x0:);//均以毫米为单位x0=int.Parse(Console.ReadLine());Console.Write(请输入像片的内方位元素y0:);y0=int.Parse(Console.ReadLine());Console.Write(请输入摄影机主距f:);f=double.Parse(Console.ReadLine());Console.WriteLine();//输入坐标数据double[,]zuobiao=newdouble[4,5];for(i=0;i4;i++){for(j=0;j5;j++){if(j3){Console.Write(请输入第{0}个点的第{1}个地面坐标：,i+1,j+1);zuobiao[i,j]=double.Parse(Console.ReadLine());}else{Console.Write(请输入第{0}个点的第{1}个像点坐标：,i+1,j-2);zuobiao[i,j]=double.Parse(Console.ReadLine());}}Console.WriteLine();}//归算像点坐标for(i=0;i4;i++){for(j=3;j5;j++){if(j==3)zuobiao[i,j]=zuobiao[i,j]-x0;elsezuobiao[i,j]=zuobiao[i,j]-y0;}}//计算和确定初值doublezs0=m*f,xs0=0,ys0=0;for(i=0;i4;i++){xs0=xs0+zuobiao[i,0];ys0=ys0+zuobiao[i,1];}xs0=xs0/4;ys0=ys0/4;//逐点计算误差方程系数double[,]xishu=newdouble[8,6];for(i=0;i8;i+=2){doublex,y;x=zuobiao[i/2,3];y=zuobiao[i/2,4];xishu[i,0]=xishu[i+1,1]=-1/m;xishu[i,1]=xishu[i+1,0]=0;xishu[i,2]=-x/(m*f);xishu[i,3]=-f*(1+x*x/(f*f));xishu[i,4]=xishu[i+1,3]=-x*y/f;xishu[i,5]=y;xishu[i+1,2]=-y/(m*f);xishu[i+1,4]=-f*(1+y*y/(f*f));xishu[i+1,5]=-x;}//计算逆阵double[,]dMatrix=matrixChe(matrixTrans(xishu),xishu);double[,]dReturn=ReverseMatrix(dMatrix,6);Console.WriteLine(逆矩阵为：);if(dReturn!=null){matrixOut(dReturn);}//求解过程doublephi0=0,omega0=0,kappa0=0;intq=0;double[,]r=newdouble[3,3];double[,]jinsi=newdouble[4,2];double[]chazhi=newdouble[8];double[]jieguo=newdouble[6];double[,]zhong=matrixChe(dReturn,matrixTrans(xishu));do{//计算旋转矩阵rr[0,0]=Math.Cos(phi0)*Math.Cos(kappa0)-Math.Sin(phi0)*Math.Sin(omega0)*Math.Sin(kappa0);r[0,1]=-Math.Cos(phi0)*Math.Sin(kappa0)-Math.Sin(phi0)*Math.Sin(omega0)*Math.Cos(kappa0);r[0,2]=-Math.Sin(phi0)*Math.Cos(omega0);r[1,0]=Math.Cos(omega0)*Math.Sin(kappa0);r[1,1]=Math.Cos(omega0)*Math.Cos(kappa0);r[1,2]=-Math.Sin(omega0);r[2,0]=Math.Sin(phi0)*Math.Cos(kappa0)+Math.Cos(phi0)*Math.Sin(omega0)*Math.Sin(kappa0);r[2,1]=-Math.Sin(phi0)*Math.Sin(kappa0)+Math.Cos(phi0)*Math.Sin(omega0)*Math.Cos(kappa0);r[2,2]=Math.Cos(phi0)*Math.Cos(omega0);//计算x,y的近似值for(i=0;i4;i++){jinsi[i,0]=-f*(r[0,0]*(zuobiao[i,0]-xs0)+r[1,0]*(zuobiao[i,1]-ys0)+r[2,0]*(zuobiao[i,2]-zs0))/(r[0,2]*(zuobiao[i,0]-xs0)+r[1,2]*(zuobiao[i,1]-ys0)+r[2,2]*(zuobiao[i,2]-zs0));jinsi[i,1]=-f*(r[0,1]*(zuobiao[i,0]-xs0)+r[1,1]*(zuobiao[i,1]-ys0)+r[2,1]*(zuobiao[i,2]-zs0))/(r[0,2]*(zuobiao[i,0]-xs0)+r[1,2]*(zuobiao[i,1]-ys0)+r[2,2]*(zuobiao[i,2]-zs0));}for(i=0;i8;i+=2){chazhi[i]=zuobiao[i/2,3]-jinsi[i/2,0];chazhi[i+1]=zuobiao[i/2,4]-jinsi[i/2,1];}for(i=0;izhong.GetLength(0);i++){doublek=0;for(j=0;jzhong.GetLength(1);j++){k=k+zhong[i,j]*chazhi[j];}jieguo[i]=k;}//求新的近似值xs0+=jieguo[0];ys0+=jieguo[1];zs0+=jieguo[2];phi0+=jieguo[3];omega0+=jieguo[4];kappa0+=jieguo[5];q++;if(q1000)break;}while((Math.Abs(jieguo[0])0.020||Math.Abs(jieguo[1])0.020)||Math.Abs(jieguo[2])0.020);Console.WriteLine(共进行了{0}次运算,q);Console.WriteLine(旋转矩阵为);matrixOut(r);for(i=0;ijieguo.GetLength(0);i++){Console.Write(第{0}个外方位元素为：{1},i+1,jieguo[i]);}}//矩阵转置publicstaticdouble[,]matrixTrans(double[,]X){double[,]A=X;double[,]C=newdouble[A.GetLength(1),A.GetLength(0)];for(inti=0;iA.GetLength(1);i++)for(intj=0;jA.GetLength(0);j++){C[i,j]=A[j,i];}returnC;}//矩阵输出publicstaticvoidmatrixOut(double[,]X){double[,]C=X;for(inti=0;iC.GetLength(0);i++){for(intj=0;jC.GetLength(1);j++){Console.Write({0},C[i,j]);}Console.Write(\n);}}//二维矩阵相乘publicstaticdouble[,]matrixChe(double[,]X,double[,]Y){inti,j,n;doublem;double[,]C=X;double[,]D=Y;double[,]E=newdouble[C.GetLength(0),C.GetLength(0)];for(i=0;iC.GetLength(0);i++){for(n=0;nC.GetLength(0);n++){m=0;for(j=0;jC.GetLength(1);j++){m=m+C[i,j]*D[j,n];}E[i,n]=m;}}returnE;}//计算行列式的值publicstaticdoubleMatrixValue(double[,]MatrixList,intLevel){double[,]dMatrix=newdouble[Level,Level];for(inti=0;iLevel;i++)for(intj=0;jLevel;j++)dMatrix[i,j]=MatrixList[i,j];doublec,x;intk=1;for(inti=0,j=0;iLevel&&jLevel;i++,j++){if(dMatrix[i,j]==0){intm=i;for(;dMatrix[m,j]==0;m++);if(m==Level)return0;else{for(intn=j;nLevel;n++){c=dMatrix[i,n];dMatrix[i,n]=dMatrix[m,n];dMatrix[m,n]=c;}k*=(-1);}}for(ints=Level-1;si;s--){x=dMatrix[s,j];for(intt=j;tLevel;t++)dMatrix[s,t]-=dMatrix[i,t]*(x/dMatrix[i,j]);}}doublesn=1;for(inti=0;iLevel;i++){if(dMatrix[i,i]!=0)sn*=dMatrix[i,i];elsereturn0;}returnk*sn;}//计算逆阵publicstaticdouble[,]ReverseMatrix(double[,]dMatrix,intLevel){doubledMatrixValue=MatrixValue(dMatrix,Level);if(dMatrixValue==0)returnnull;double[,]dReverseMatrix=newdouble[Level,2*Level];doublex,c;for(inti=0;iLevel;i++){for(intj=0;j2*Level;j++){if(jLevel)dReverseMatrix[i,j]=dMatrix[i,j];elsedReverseMatrix[i,j]=0;}dReverseMatrix[i,Level+i]=1;}for(inti=0,j=0;iLevel&&jLevel;i++,j++){if(dReverseMatrix[i,j]==0){intm=i;for(;dMatrix[m,j]==0;m++);if(m==Level)returnnull;else{for(intn=j;n2*Level;n++)dReverseMatrix[i,n]+=dReverseMatrix[m,n];}}x=dRever