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

精品文档，仅供学习与交流，如有侵权请联系网站删除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 double4, 5; for (i = 0; i < 4; i+) for (j = 0; j < 5; j+) if (j < 3) Console.Write("请输入第0个点的第1个地面坐标：", i + 1, j + 1); zuobiaoi, j = double.Parse(Console.ReadLine(); else Console.Write("请输入第0个点的第1个像点坐标：", i + 1, j - 2); zuobiaoi, j = double.Parse(Console.ReadLine(); Console.WriteLine(); /归算像点坐标 for (i = 0; i < 4; i+) for (j = 3; j < 5; j+) if (j = 3) zuobiaoi, j = zuobiaoi, j - x0; else zuobiaoi, j = zuobiaoi, j - y0; /计算和确定初值 double zs0 = m * f, xs0 = 0, ys0 = 0; for (i = 0; i < 4; i+) xs0 = xs0 + zuobiaoi, 0; ys0 = ys0 + zuobiaoi, 1; xs0 = xs0 / 4; ys0 = ys0 / 4; /逐点计算误差方程系数 double, xishu = new double8, 6; for (i = 0; i < 8; i += 2) double x, y; x = zuobiaoi / 2, 3; y = zuobiaoi / 2, 4; xishui, 0 = xishui + 1, 1 = -1 / m; xishui, 1 = xishui + 1, 0 = 0; xishui, 2 = -x / (m * f); xishui, 3 = -f * (1 + x * x / (f * f); xishui, 4 = xishui + 1, 3 = -x * y / f; xishui, 5 = y; xishui + 1, 2 = -y / (m * f); xishui + 1, 4 = -f * (1 + y * y / (f * f); xishui + 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 double3, 3; double, jinsi = new double4, 2; double chazhi = new double8; double jieguo = new double6; double, zhong = matrixChe(dReturn, matrixTrans(xishu); do /计算旋转矩阵r r0, 0 = Math.Cos(phi0) * Math.Cos(kappa0) - Math.Sin(phi0) * Math.Sin(omega0) * Math.Sin(kappa0); r0, 1 = -Math.Cos(phi0) * Math.Sin(kappa0) - Math.Sin(phi0) * Math.Sin(omega0) * Math.Cos(kappa0); r0, 2 = -Math.Sin(phi0) * Math.Cos(omega0); r1, 0 = Math.Cos(omega0) * Math.Sin(kappa0); r1, 1 = Math.Cos(omega0) * Math.Cos(kappa0); r1, 2 = -Math.Sin(omega0); r2, 0 = Math.Sin(phi0) * Math.Cos(kappa0) + Math.Cos(phi0) * Math.Sin(omega0) * Math.Sin(kappa0); r2, 1 = -Math.Sin(phi0) * Math.Sin(kappa0) + Math.Cos(phi0) * Math.Sin(omega0) * Math.Cos(kappa0); r2, 2 = Math.Cos(phi0) * Math.Cos(omega0); /计算x,y的近似值 for (i = 0; i < 4; i+) jinsii, 0 = -f * (r0, 0 * (zuobiaoi, 0 - xs0) + r1, 0 * (zuobiaoi, 1 - ys0) + r2, 0 * (zuobiaoi, 2 - zs0) / (r0, 2 * (zuobiaoi, 0 - xs0) + r1, 2 * (zuobiaoi, 1 - ys0) + r2, 2 * (zuobiaoi, 2 - zs0); jinsii, 1 = -f * (r0, 1 * (zuobiaoi, 0 - xs0) + r1, 1 * (zuobiaoi, 1 - ys0) + r2, 1 * (zuobiaoi, 2 - zs0) / (r0, 2 * (zuobiaoi, 0 - xs0) + r1, 2 * (zuobiaoi, 1 - ys0) + r2, 2 * (zuobiaoi, 2 - zs0); for (i = 0; i < 8; i += 2) chazhii = zuobiaoi / 2, 3 - jinsii / 2, 0; chazhii + 1 = zuobiaoi / 2, 4 - jinsii / 2, 1; for (i = 0; i < zhong.GetLength(0); i+) double k = 0; for (j = 0; j < zhong.GetLength(1); j+) k = k + zhongi, j * chazhij; jieguoi = k; /求新的近似值 xs0 += jieguo0; ys0 += jieguo1; zs0 += jieguo2; phi0 += jieguo3; omega0 += jieguo4; kappa0 += jieguo5; q+; if (q > 1000) break; while (Math.Abs(jieguo0) > 0.020 | Math.Abs(jieguo1) > 0.020) | Math.Abs(jieguo2) > 0.020); Console.WriteLine("共进行了0次运算", q); Console.WriteLine("旋转矩阵为"); matrixOut(r); for (i = 0; i < jieguo.GetLength(0); i+) Console.Write("第0个外方位元素为：1", i + 1, jieguoi); /矩阵转置 public static double, matrixTrans(double, X) double, A = X; double, C = new doubleA.GetLength(1), A.GetLength(0); for (int i = 0; i < A.GetLength(1); i+) for (int j = 0; j < A.GetLength(0); j+) Ci, j = Aj, 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(" 0", Ci, j); Console.Write("n"); /二维矩阵相乘 public static double, matrixChe(double, X, double, Y) int i, j, n; double m; double, C = X; double, D = Y; double, E = new doubleC.GetLength(0), C.GetLength(0); for (i = 0; i < C.GetLength(0); i+) for (n = 0; n < C.GetLength(0); n+) m = 0; for (j = 0; j < C.GetLength(1); j+) m = m + Ci, j * Dj, n; Ei, n = m; return E; /计算行列式的值 public static double MatrixValue(double, MatrixList, int Level) double, dMatrix = new doubleLevel, Level; for (int i = 0; i < Level; i+) for (int j = 0; j < Level; j+) dMatrixi, j = MatrixListi, j; double c, x; int k = 1; for (int i = 0, j = 0; i < Level && j < Level; i+, j+) if (dMatrixi, j = 0) int m = i; for (; dMatrixm, j = 0; m+) ; if (m = Level) return 0; else for (int n = j; n < Level; n+) c = dMatrixi, n; dMatrixi, n = dMatrixm, n; dMatrixm, n = c; k *= (-1); for (int s = Level - 1; s > i; s-) x = dMatrixs, j; for (int t = j; t < Level; t+) dMatrixs, t -= dMatrixi, t * (x / dMatrixi, j); double sn = 1; for (int i = 0; i < Level; i+) if (dMatrixi, i != 0) sn *= dMatrixi, i; else return 0; return k * sn; /计算逆阵 public static double, ReverseMatrix(double, dMatrix, int Level) double dMatrixValue = MatrixValue(dMatrix, Level); if (dMatrixValue = 0) return null; double, dReverseMatrix = new doubleLevel, 2 * Level; double x, c; for (int i = 0; i < Level; i+) for (int j = 0; j < 2 * Level; j+) if (j < Level) dReverseMatrixi, j = dMatrixi, j; else dReverseMatrixi, j = 0; dReverseMatrixi, Level + i = 1; for (int i = 0, j = 0; i < Level && j < Level; i+, j+) if (dReverseMatrixi, j = 0) int m = i; for (; dMatrixm, j = 0; m+) ; if (m = Level) return null; else for (int n = j; n < 2 * Level; n+) dReverseMatrixi, n += dReverseMatrixm, n; x = dReverseMatrixi, j; if (x != 1) for (int n = j; n < 2 * Level; n+) if (dReverseMatrixi, n != 0) dReverseMatrixi, n /= x; for (int s = Level - 1; s > i; s-) x = dReverseMatrixs, j; for (int t = j; t < 2 * Level; t+) dReverseMatrixs, t -= (dReverseMatrixi, t * x); for (int i = Level - 2; i >= 0; i-) for (int j = i + 1; j < Level; j+) if (dReverseMatrixi, j != 0) c = dReverseMatrixi, j; for (int n = j; n < 2 * Level; n+) dReverseMatrixi, n -= (c * dReverseMatrixj, n); double, dReturn = new doubleLevel, Level; for (int i = 0; i < Level; i+) for (int j = 0; j < Level; j+) dReturni, j = dReverseMatrixi, j + Level; return dReturn;【精品文档】第 4 页