攝影測量學(xué)單像空間后方交會(huì)程序設(shè)計(jì)作業(yè)

1、.using System;using System.Collections.Generic;using System.Linq;using System.Text;namespace 單像空間后方交會(huì) class Program static void Main(string args) int x0, y0, i, j; double f, m; Console.Write(請輸入像片比例尺：); m = double.Parse(Console.ReadLine(); Console.Write(請輸入像片的內(nèi)方位元素x0:);/均以毫米為單位 x0 = int.Parse(Consol

2、e.ReadLine(); Console.Write(請輸入像片的內(nèi)方位元素y0:); y0 = int.Parse(Console.ReadLine(); Console.Write(請輸入攝影機(jī)主距f:); f = double.Parse(Console.ReadLine(); Console.WriteLine(); /輸入坐標(biāo)數(shù)據(jù) double, zuobiao = new double4, 5; for (i = 0; i 4; i+) for (j = 0; j 5; j+) if (j 3) Console.Write(請輸入第0個(gè)點(diǎn)的第1個(gè)地面坐標(biāo)：, i + 1, j +

3、 1); zuobiaoi, j = double.Parse(Console.ReadLine(); else Console.Write(請輸入第0個(gè)點(diǎn)的第1個(gè)像點(diǎn)坐標(biāo)：, i + 1, j - 2); zuobiaoi, j = double.Parse(Console.ReadLine(); Console.WriteLine(); /歸算像點(diǎn)坐標(biāo) 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

4、; /計(jì)算和確定初值 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; /逐點(diǎn)計(jì)算誤差方程系數(shù) 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

5、= -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; /計(jì)算逆陣 double, dMatrix =matrixChe(matrixTrans(x

6、ishu), 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 double

7、6; double, zhong = matrixChe(dReturn, matrixTrans(xishu); do /計(jì)算旋轉(zhuǎn)矩陣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.Co

8、s(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(ome

9、ga0) * Math.Cos(kappa0); r2, 2 = Math.Cos(phi0) * Math.Cos(omega0); /計(jì)算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); ji

10、nsii, 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 - jins

11、ii / 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(共進(jìn)行了0次運(yùn)算, q); Console.WriteLine(旋轉(zhuǎn)矩陣為); matrixOut(r); for (i = 0; i jieguo.GetLength(0); i+) Console.Write

12、(第0個(gè)外方位元素為：1, i + 1, jieguoi); /矩陣轉(zhuǎn)置 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) do

13、uble, 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)

14、; 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; /計(jì)算行列式的值 public static double MatrixValue(double, MatrixList, int Level) double, dMatrix = new doubleLevel, Level; for (int i = 0; i Level; i+) f

15、or (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+) dMatr

16、ixs, 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; /計(jì)算逆陣 public static double, ReverseMatrix(double, dMatrix, int Level) double dMatrixValue = MatrixValue(dMatrix, Level); if (dMatrixValue = 0)

17、 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+)

18、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 (dReve