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));

}

for (i = 0; i < 8; 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; i < zhong.GetLength(0); i++) {

double k = 0;

for (j = 0; j < zhong.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 (q > 1000) 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; i < jieguo.GetLength(0); i++) {

Console.Write(\第{0}个外方位元素为：{1}\, i + 1, jieguo[i]); }

}

//矩阵转置

public static double[,] matrixTrans(double[,] X) {

double[,] A = X;

double[,] C = new double[A.GetLength(1), A.GetLength(0)]; for (int i = 0; i < A.GetLength(1); i++) for (int j = 0; j < A.GetLength(0); j++) {

C[i, j] = A[j, i]; }

return C; }

//矩阵输出

public static void matrixOut(double[,] X) {

double[,] C = X;

for (int i = 0; i < C.GetLength(0); i++) {

for (int j = 0; j < C.GetLength(1); j++) {

Console.Write(\, C[i, j]); }