后方交会程序

后方交会程序 using System; using System.Collections.Generic; using System.ComponentModel; using System.Data; using System.Drawing; using System.Text; using System.Windows.Forms; namespace 后方交会 { public partial class Form1 : Form { #region static int M=6; static int N=4; i; int double Φ, ω, k, m, Xs0, Ys0, Zs0, f, S1 = 0.0, S2 = 0.0, S3 = 0.0; double[] x = new double[4]{ -86.15, -53.40, -14.78, 10.46 }; double[] y = new double[4]{ -68.99, 82.21, -76.63, 64.43 }; double[] X = new double[4]{ 36589.41, 37631.08, 39100.97, 40426.54 }; double[] Y = new double[4]{ 25273.32, 31324.51, 24934.98, 30319.81 }; double[] Z = new double[4]{ 2195.17, 728.69, 2385.50, 757.31 }; double[] H = new double[N]; double[] I = new double[N]; double[] G = new double[N]; double[] a = new double[3]; double[] b = new double[3]; double[] c = new double[3]; double[] xo = new double[N]; double[] yo = new double[N]; double[,] L = new double[M, 1]; double[,] A = new double[M, 6]; double[,] B = new double[6, M]; double[,] C = new double[6, 6]; double[,] D = new double[6, 6]; double[,] E = new double[6, 6]; double[,] T = new double[6, 1]; #endregion public Form1() { InitializeComponent(); } private void Form1_Load(object sender, EventArgs e) { if( MessageBox.Show("欢迎使用本软件","欢迎 ",MessageBoxButtons.OKCancel, MessageBoxIcon.Information)==DialogResult.Cancel) { Application.Exit(); } //if (MessageBoxButtons.OKCancel == DialogResult.Cancel) //{ // Application.Exit(); //} // TODO: 这行代码将数据加载到“hFJFDataSet.Coordinate” 中。您可以根据需要移动或移除它。 this.coordinateTableAdapter.Fill(this.hFJFDataSet.Coordinate); // TODO: 这行代码将数据加载到表“hFJFDataSet.Coordinate” 中。您可以根据需要移动或移除它。 this.coordinateTableAdapter.Fill(this.hFJFDataSet.Coordinate); // TODO: 这行代码将数据加载到表“hFJFDataSet1.Coordinate” 中。您可以根据需要移动或移除它。 this.coordinateTableAdapter.Fill(this.hFJFDataSet.Coordinate); N = Convert.ToInt32(numericUpDown_N.Value); M = Convert.ToInt32(numericUpDown_M.Value); } private void btnCalculate_Click(object sender, EventArgs e) { if (textBox1.TextLength == 0||textBox2.TextLength == 0||textBox3.TextLength == 0||textBox4.TextLength==0) { MessageBox.Show("请先输入数据", "错误", MessageBoxButtons.OKCancel, MessageBoxIcon.Error); if (MessageBox.Show("你好～～～\r继续输入数字，单击“确 定”～～～\r放弃操作，单击“取消”～～～", "警告", MessageBoxButtons.OKCancel, MessageBoxIcon.Warning) == DialogResult.Cancel) { Application.Exit(); } return; } else { Initialize(); Matrix3(); DieDai(); Result(); } } //矩阵转置 public void Transpose(double[,] a, double[,] b) { try { int i, j; for (i = 0; i < 6; i++) { for (j = 0; j < M; j++) { b[i, j] = a[j, i]; } } } catch { return; } } //矩阵的乘法运算 public void Matrix1(double[,] a, double[,] b, double[,] c) { try { int i, j, k; for (i = 0; i < 6; i++) { for (j = 0; j < 6; j++) { c[i, j] = 0; for (k = 0; k < 2 * (N - 1); k++) c[i, j] += a[i, k] * b[k, j]; } } } catch { return; } } //矩阵的乘法运算(结果为6*1的矩阵) public void Matrix2(double[,] a, double[,] b, double[,] c) { try { int i, j, k; for (i = 0; i < 6; i++) { (j = 0; j < 1; j++) for { c[i, j] = 0; for (k = 0; k < 2 * (N - 1); k++) c[i, j] += a[i, k] * b[k, j]; } } } catch { return; } } //计算逆矩阵 public 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; // Init Reverse matrix 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++) ; (m == Level) if return null; else { // Add i-row with m-row for (int n = j; n < 2 * Level; n++) dReverseMatrix[i, n] += dReverseMatrix[m, n]; } } // Format the i-row with "1" start x = dReverseMatrix[i, j]; if (x != 1) { for (int n = j; n < 2 * Level; n++) if (dReverseMatrix[i, n] != 0) dReverseMatrix[i, n] /= x; } // Set 0 to the current column in the rows after current row for (int s = Level - 1; s > i; s--) { x = dReverseMatrix[s, j]; for (int t = j; t < 2 * Level; t++) dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x); } } // Format the first matrix into unit-matrix for (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; } public double MatrixValue(double[,] MatrixList, int Level) { 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; int k = 1; for (int i = 0, j = 0; i < Level && j < Level; i++, j++) { if (dMatrix[i, j] == 0) { int m = i; if (m == 0) return 0; else { for (; dMatrix[m - 2, j] == 0; m++) ; if (m == Level) return 0; else { // Row change between i-row and m-row for (int n = j; n < Level; n++) { c = dMatrix[i, n]; dMatrix[i, n] = dMatrix[m, n]; dMatrix[m, n] = c; } // Change value pre-value k *= (-1); } } } // Set 0 to the current column in the rows after current row for (int s = Level - 1; s > i; s--) { x = dMatrix[s, j]; for (int t = j; t < Level; t++) = dMatrix[i, t] * (x / dMatrix[i, dMatrix[s, t] -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 void Initialize() { for (i = 0; i < N; i++) { S1 += X[i]; S2 += Y[i]; S3 += Z[i]; } //确定外方位元素的初始值 Φ = ω = k = 0; f = Convert.ToDouble(this.textBox3.Text); m = Convert.ToDouble(this.textBox4.Text); //if (this.textBox4.TextLength == 0) //{ // MessageBox.Show("请输入数据"); //} //else //{ // m = Convert.ToDouble(this.textBox4.Text); //} //f = Convert.ToDouble(this.textBox3.Text); //m = Convert.ToDouble(this.textBox4.Text); //m = 50000; //f = 0.15324; Xs0 = S1 / N; Ys0 = S2 / N; Zs0 = m * f + S3 / N; //像片坐标的单位换算 for (i = 0; i < N-1; i++) { x[i] /= 1000.0; y[i] /= 1000.0; } } //计算外方位元素 public void Matrix3() { //旋转矩阵R a[0] = Math.Cos(Φ) * Math.Cos(k) - Math.Sin(Φ) * Math.Sin(ω) * Math.Sin(k); a[1] = -Math.Cos(Φ) * Math.Sin(k) - Math.Sin(Φ) * Math.Sin(ω) * Math.Cos(k); a[2] = -Math.Sin(Φ) * Math.Cos(ω); b[0] = Math.Cos(ω) * Math.Sin(k); b[1] = Math.Cos(ω) * Math.Cos(k); b[2] = -Math.Sin(ω); c[0] = Math.Sin(Φ) * Math.Cos(k) + Math.Cos(Φ) * Math.Sin(ω) * Math.Sin(k); c[1] = -Math.Sin(Φ) * Math.Sin(k) + Math.Cos(Φ) * Math.Sin(ω) * Math.Cos(k); c[2] = Math.Cos(Φ) * Math.Cos(ω); for (i = 0; i < N - 1; i++) { H[i] = a[0] * (X[i] - Xs0) + b[0] * (Y[i] - Ys0) + c[0] * (Z[i] - Zs0); I[i] = a[1] * (X[i] - Xs0) + b[1] * (Y[i] - Ys0) + c[1] * (Z[i] - Zs0); G[i] = a[2] * (X[i] - Xs0) + b[2] * (Y[i] - Ys0) + c[2] * (Z[i] - Zs0); //由共线条件方程式求得xo,yo的近似值 xo[i] = -f * (H[i]) / (G[i]); yo[i] = -f * (I[i]) / (G[i]); //常数项矩阵 L[2 * i, 0] = x[i] - xo[i]; L[2 * i + 1, 0] = y[i] - yo[i]; } for (i = 0; i < N - 1; i++) { 系数矩阵 // A[2 * i, 0] = (a[0] * f + a[2] * x[i]) / G[i]; A[2 * i, 1] = (b[0] * f + b[2] * x[i]) / G[i]; A[2 * i, 2] = (c[0] * f + c[2] * x[i]) / G[i]; A[2 * i, 3] = y[i] * Math.Sin(ω) - (x[i] * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) / f + f * Math.Cos(k)) * Math.Cos(ω); A[2 * i, 4] = -f * Math.Sin(k) - x[i] * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k)) / f; A[2 * i, 5] = y[i]; A[2 * i + 1, 0] = (a[1] * f + a[2] * y[i]) / G[i]; A[2 * i + 1, 1] = (b[1] * f + b[2] * y[i]) / G[i]; A[2 * i + 1, 2] = (c[1] * f + c[2] * y[i]) / G[i]; A[2 * i + 1, 3] = -x[i] * Math.Sin(ω) - (y[i] * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) / f - f * Math.Sin(k)) * Math.Cos(ω); A[2 * i + 1, 4] = -f * Math.Cos(k) - y[i] * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k)) / f; A[2 * i + 1, 5] = -x[i]; } Transpose(A, B);//调用Transpose方法,求得矩阵A的转置矩阵B Matrix1(B, A, C);//调用Matrix1方法,求矩阵A和矩阵B的乘积得 矩阵C E = ReverseMatrix(C, 6);//调用ReverseMatrix方法,求得矩阵C 的逆矩阵E Matrix1(E, B, D);//调用Matrix1方法,求得矩阵E和矩阵B的乘积 得矩阵D Matrix2(D, L, T);//调用Matrix2方法,求得矩阵D和矩阵L的乘积得矩阵T,也即为所要求的外方位元素 //计算外方位元素 Xs0 += T[0, 0]; Ys0 += T[1, 0]; Zs0 += T[2, 0]; Φ += T[3, 0]; ω += T[4, 0]; k += T[5, 0]; } //输出结果 public void Result() { lbl_Xs.Text = Xs0.ToString(); lbl_Ys.Text = Ys0.ToString(); lbl_Zs.Text = Zs0.ToString(); lbl_φ.Text = Φ.ToString(); lbl_ω.Text = ω.ToString(); lbl_k.Text = k.ToString(); } //反复调用Matrix3方法进行迭代运算,并判断是否满足限差 public void DieDai() { int n; n = Convert.ToInt32(numericUpDown_diedai.Value); if (Math.Abs(T[3, 0]) >= 0.000029 || Math.Abs(T[4, 0]) >= 0.000029 || Math.Abs(T[5, 0]) >= 0.000029) { for (int d = 1; d < n; d++) { Matrix3(); for (i = 3; i < 6; i++) { for (int j = 0; j < 1; j++) { Console.WriteLine(T[i, j]); } } if (Math.Abs(T[3, 0]) <= 0.000029 && Math.Abs(T[4, 0]) <= 0.000029 && Math.Abs(T[5, 0]) <= 0.000029) { Result(); break; } } if (Math.Abs(T[3, 0]) >= 0.000029 || Math.Abs(T[4, 0]) >= 0.000029 || Math.Abs(T[5, 0]) >= 0.000029) { MessageBox.Show("已达到指定迭代次数,迭代结 束!!!"); } } else { Result(); } } private void btnClear(object sender, EventArgs e) { Xs0 = 0; Ys0 = 0; Zs0 = 0; Φ = 0; ω = 0; k = 0; lbl_Xs.Text = ""; lbl_Ys.Text = ""; lbl_Zs.Text = ""; lbl_φ.Text = ""; lbl_ω.Text = ""; lbl_k.Text = ""; textBox1.Text = ""; textBox2.Text = ""; textBox3.Text = ""; textBox4.Text = ""; if (MessageBox.Show("是否清除地面控制点的地面坐标及其对应 像点的像片坐标", "警告", MessageBoxButtons.YesNo, MessageBoxIcon.Warning) == DialogResult.Yes) { dataGridView1.DataSource = null; } } } }