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

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

namespace单像空间后方交会

{

class Program

{

static void Main(string[] args)

{

int x0, y0, i, j; double f, 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 = new double[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;

}

}

//计算和确定初值

double zs0 = 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 = new double[8, 6];

for (i = 0; i < 8; i += 2)

{

double x, 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);

}

//求解过程

double phi0 = 0, omega0 = 0, kappa0 = 0; int q = 0;

double[,] r = new double[3, 3];

double[,] jinsi = new double[4, 2];

double[] chazhi = new double[8];

double[] jieguo = new double[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(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; i < 4; 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));