{System;System.Collections.Generic;System.Linq;System.Text;namespace 单像空间后方交会{class Program{static void Main( string [] args)for (j = 0; j < 5; j++)if (j < 3)"请输入第 {0} 个点的第 {1} 个地面坐标： ", i + 1, j+ 1); double .Parse( Console .ReadLine());"请输入第 {0} 个点的第 {1} 个像点坐标： ", i + 1, j - 2);double .Parse( Console .ReadLine());Console .WriteLine();// 归算像点坐标(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++)elseusing using using using x0 = y0 = int x0, y0, i, j; double f, m;Console .Write( " 请输入像片比例尺： ");double .Parse( Console .ReadLine());Console .Write( " 请输入像片的内方位元素 x0:" ); // 均以毫米为单位 int .Parse( Console .ReadLine());Console .Write( " 请输入像片的内方位元素 y0:" );int .Parse( Console .ReadLine());Console .Write( " 请输入摄影机主距 f:" );double .Parse( Console .ReadLine());Console .WriteLine();// 输入坐标数据 double [,] zuobiao = new double [4, 5];(i = 0; i < 4; i++)forConsole .Write( zuobiao[i, j] = Console .Write( zuobiao[i, j] = forxs0 = 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;double [,] r = double [,] jinsi = double [] chazhi = double [] jieguo ={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));int q = 0;new double [3, 3]; new double [4, 2];new double [8];new double [6];r[0, 0] = Math.Sin(kappa0);r[0, 1] = -Math.Cos(kappa0); r[0, 2] = - r[1, 0] = r[1, 1] = r[1, 2] = - r[2,0] = Math.Sin(kappa0);r[2, 1] = - double [,] zhong = matrixChe(dReturn,matrixTrans(xishu)); do// 计算旋转矩阵 r Math.Cos(phi0) *Math.Cos(kappa0) - Math.Sin(phi0) *Math.Sin(omega0) * Math.Cos(phi0) * Math.Sin(kappa0) - Math.Sin(phi0) * Math.Sin(omega0) * Math.Sin(phi0) * Math.Cos(omega0) * Math.Cos(omega0) * Math.Sin(omega0);Math.Sin(phi0) *Math.Cos(omega0);Math.Sin(kappa0);Math.Cos(kappa0);Math.Cos(kappa0) + Math.Cos(phi0) * Math.Sin(omega0) * Math.Sin(phi0) * Math.Sin(kappa0) + Math.Cos(phi0) *Math.Sin(omega0) *Math.Cos(phi0) * //计算x,y 的近似值for (i = 0; i < 4; i++)Math.Cos(omega0);}for ( int j = 0; j < C.GetLength(1); j++){}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[2]) > 0.020); Math.Abs(jieguo[1]) > 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; ( int i = 0; i < C.GetLength(0);i++)for Console .Write( " {0}" , C[i, j]);for ( int s = Level - 1; s > i; s--){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 double [C.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 + C[i, j] * D[j,n];}E[i, n] = m;return E;计算行列式的值// public static double MatrixValue( double [,] MatrixList, double [,] dMatrix = new double [Level, Level]; for( int i = 0; i < Level; i++)for ( int j = 0; j < Level; j++)dMatrix[i, j] = MatrixList[i, j];double c, x;k = 1;( int i = 0, j = 0; i < Level && j < Level;i++, j++)int for if (dMatrix[i, j] ==0)int m = i;for (; dMatrix[m, j] == 0;m++) ; if (m == Level)return 0;elsefor ( int n = j; n < Level;n++){c = dMatrix[i, n];dMatrix[i, n] = dMatrix[m,n]; dMatrix[m, n] = c;}k *= (-1);x = dMatrix[s, j];for ( int t = j; t < Level;t++)int Level)} for ( int s = Level - 1; s > i; s--){dMatrix[s, t] -= dMatrix[i, t] * (x / dMatrix[i, j]);double sn = 1; for ( int i = 0; i < Level; i++) if (dMatrix[i, i] != 0) sn *= dMatrix[i,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 double [Level, 2 * Level]; double x, c; for ( int i = 0; i < Level; i++)for ( int j = 0; j < 2 * Level; j++) if (j < Level) dReverseMatrix[i, j] = dMatrix[i, j]; else dReverseMatrix[i, j] = 0; } dReverseMatrix[i, Level + i] =1; for ( int i = 0, j = 0; i < Level && j < Level; i++, j++) if (dReverseMatrix[i, j] == 0) int m = i;for (; dMatrix[m, j] == 0;m++) ; if (m == Level) return null ; else for ( int n = j; n < 2 * Level; n++) dReverseMatrix[i, n] += dReverseMatrix[m, n]; } } x = dReverseMatrix[i, j]; if (x != 1) for ( int n = j; n < 2 * Level; n++) if (dReverseMatrix[i, n] != 0) dReverseMatrix[i, n] /= x; x = dReverseMatrix[s,j];} }for ( int t = j; t < 2 * Level; t++) dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x);( int i = Level - 2; i >= 0; i--)for ( int j = i + 1; j < Level;j++) if (dReverseMatrix[i, j] !=0)c = dReverseMatrix[i, j];for ( int n = j; n < 2 * Level; n++)dReverseMatrix[i, n] -= (c * dReverseMatrix[j, n]);double [,] dReturn = new double [Level, Level]; for ( int i = 0; i < Level; i++)for ( int j = 0; j < Level; j++)dReturn[i, j] = dReverseMatrix[i, j + Level];return dReturn;for。