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

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;i<4;i++)
{
for(j=0;j<5;j++)
{
if(j<3)
{
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;i<4;i++)
{
for(j=3;j<5;j++)
{
if(j==3)
zuobiao[i,j]=zuobiao[i,j]-x0;
else
zuobiao[i,j]=zuobiao[i,j]-y0;
}
}
//计算和确定初值
doublezs0=m*f,xs0=0,ys0=0;
for(i=0;i<4;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;i<8;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
{//计算旋转矩阵r
r[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(om