资源描述
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 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 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 i; s-) x = dReverseMatrixs, j; for (int t = j; t = 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;
展开阅读全文