单像空间后方交会.docx

using System;using System.Collections.Generic;using System.ComponentModel;using System.Data;using System.Drawing;using System.Text;using System.Windows.Forms;using System.IO;using System.Text.RegularExpressions;namespace 单像空间后方交会 public partial class Form1 : Form public Form1() InitializeComponent(); private void button1_Click(object sender, EventArgs e) string path = Directory.GetCurrentDirectory(); OpenFileDialog OpenDlg = new OpenFileDialog(); /通过openfiledialog打开数据文件 OpenDlg.Filter = 文本文件|*.*|C#文件|*.cs|所有文件|*.*; OpenDlg.FilterIndex = 1; OpenDlg.InitialDirectory = path; if (OpenDlg.ShowDialog() = DialogResult.OK) /打开文件对话框中选择的文件名 string fname = OpenDlg.FileName; /创建从字符串进行读取的StringReader对象 StreamReader sr = File.OpenText(fname); string str; while (str = sr.ReadLine() != null) /将读出的字符串在richTextBox1中显示; this.richTextBox1.Text += str; this.richTextBox1.Text += n; double x = new double4; /像点横坐标 double y=new double4;/像点纵坐标 /定义数组大小 double X = new double4;/像点对应地面点横坐标 double Y = new double4;/像点对应地面点纵坐标 double Z = new double4;/像点对应地面点Z坐标 double X1 = new double4;/共线方程分子 double Y1 = new double4;/共线方程分子 double Z1 = new double4;/共线方程分母 double Xs;/摄影中心在地面摄影测量坐标系中的坐标 double Ys;/摄影中心在地面摄影测量坐标系中的坐标 double Zs;/摄影中心在地面摄影测量坐标系中的坐标 double Q = 0;/外方位元素的角元素 double W = 0;/外方位元素的角元素 double K = 0;/外方位元素的角元素 /定义数组大小 double, R = 1, 0, 0 , 0, 1, 0 , 0, 0, 1 ; double, A = new double8, 6; double, AT = new double6, 8; double, ATA = new double6, 6; double, ATAN = new double6, 6; double, ATPL = new double6, 1; double, V = new double6, 1; double, L = new double8, 1; double, XX = new double6, 1; /初始化像主点坐标 double x0 = 0.0, y0 = 0.0; /将richtextbox中的值赋给字符串a string a = richTextBox1.Text; string ContentLines = a.Split(Environment.NewLine.ToCharArray(); /分行读取 string filecontent = ContentLines0; /读入单像空间后方交会基本信息数据 string splitstr = s1,10; /分割字符串 string substring1 = Regex.Split(filecontent, splitstr); double f = Convert.ToDouble(substring10) / 1000; int i = 0; /读取像点坐标与对应的地面点坐标 for (i = 0; i 4; i+) filecontent = ContentLines2 + i; substring1 = Regex.Split(filecontent, splitstr); xi = Convert.ToDouble(substring11) / 1000; yi = Convert.ToDouble(substring12) / 1000; Xi = Convert.ToDouble(substring13); Yi = Convert.ToDouble(substring14); Zi = Convert.ToDouble(substring15); /计算比例尺 double b = System.Math.Sqrt(X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + System.Math.Sqrt(X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + System.Math.Sqrt(X0 - X3) * (X0 - X3) + (Y0 - Y3) * (Y0 - Y3) + System.Math.Sqrt(X2 - X3) * (X2 - X3) + (Y2 - Y3) * (Y2 - Y3) + System.Math.Sqrt(X3 - X1) * (X3 - X1) + (Y3 - Y1) * (Y3 - Y1) + System.Math.Sqrt(X2 - X1) * (X2 - X1) + (Y2 - Y1) * (Y2 - Y1); double c = System.Math.Sqrt(x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + System.Math.Sqrt(x0 - x2) * (x0 - x2) + (y0 - y2) * (y0 - y2) + System.Math.Sqrt(x0 - x3) * (x0 - x3) + (y0 - y3) * (y0 - y3) + System.Math.Sqrt(x2 - x3) * (x2 - x3) + (y2 - y3) * (y2 - y3) + System.Math.Sqrt(x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1) + System.Math.Sqrt(x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1); double m = b / c; /摄影中心在地面摄影测量坐标系中的坐标 Xs = (X0 + X1 + X2 + X3) / 4; Ys = (Y0 + Y1 + Y2 + Y3) / 4; Zs = (Z0 + Z1 + Z2 + Z3) / 4 + m * f; /航高 double H = m * f; int count = 0; do count+; /组成旋转矩阵 R0, 0 = System.Math.Cos(Q) * System.Math.Cos(K) - System.Math.Sin(Q) * System.Math.Sin(W) * System.Math.Sin(K); R0, 1 = -System.Math.Cos(Q) * System.Math.Sin(K) - System.Math.Sin(Q) * System.Math.Sin(W) * System.Math.Cos(K); R0, 2 = -System.Math.Sin(Q) * System.Math.Cos(W); R1, 0 = System.Math.Cos(W) * System.Math.Sin(K); R1, 1 = System.Math.Cos(W) * System.Math.Cos(K); R1, 2 = -System.Math.Sin(W); R2, 0 = System.Math.Sin(Q) * System.Math.Cos(K) + System.Math.Cos(Q) * System.Math.Sin(W) * System.Math.Sin(K); R2, 1 = -System.Math.Sin(Q) * System.Math.Sin(K) + System.Math.Cos(Q) * System.Math.Sin(W) * System.Math.Cos(K); R2, 2 = System.Math.Cos(Q) * System.Math.Cos(W); int k = 0, j = 0; for (i = 0, k = 0, j = 0; i 4; i+) /共线方程的分子与分母 X1i = R0, 0 * (Xi - Xs) + R1, 0 * (Yi - Ys) + R2, 0 * (Zi - Zs); Y1i = R0, 1 * (Xi - Xs) + R1, 1 * (Yi - Ys) + R2, 1 * (Zi - Zs); Z1i = R0, 2 * (Xi - Xs) + R1, 2 * (Yi - Ys) + R2, 2 * (Zi - Zs); /误差方程常数项 Lj, 0 = xi - (-f * X1i / Z1i); Lj + 1, 0 = yi - (-f * Y1i / Z1i); j = j + 2; /误差方程系数矩阵 Ak, 0 = (R0, 0 * f + R0, 2 * (xi - x0) / Z1i; Ak, 1 = (R1, 0 * f + R1, 2 * (xi - x0) / Z1i; Ak, 2 = (R2, 0 * f + R2, 2 * (xi - x0) / Z1i; Ak, 3 = (yi - y0) * System.Math.Sin(W) - (xi - x0) * (xi - x0) * System.Math.Cos(K) - (yi - y0) * System.Math.Sin(K) / f + f * System.Math.Cos(K) * System.Math.Cos(W); Ak, 4 = -f * System.Math.Sin(K) - (xi - x0) * (xi - x0) * System.Math.Sin(K) + (yi - y0) * System.Math.Cos(K) / f; Ak, 5 = yi - y0; Ak + 1, 0 = (R0, 1 * f + R0, 2 * (yi - y0) / Z1i; Ak + 1, 1 = (R1, 1 * f + R1, 2 * (yi - y0) / Z1i; Ak + 1, 2 = (R2, 1 * f + R2, 2 * (yi - y0) / Z1i; Ak + 1, 3 = -(xi - x0) * System.Math.Sin(W) - (yi - y0) * (xi - x0) * System.Math.Cos(K) - (yi - y0) * System.Math.Sin(K) / f - f * System.Math.Sin(K) * System.Math.Cos(W); Ak + 1, 4 = -f * System.Math.Cos(K) - (yi - y0) * (xi - x0) * System.Math.Sin(K) + (yi - y0) * System.Math.Cos(K) / f; Ak + 1, 5 = -(xi - x0); k = k + 2; /系数矩阵的转置 for (i = 0; i 8; i+) for (k = 0; k 6; k+) ATk, i = Ai, k; k = 0; /计算AT*A=ATA for (i = 0; i 6; i+) for (j = 0; j = i; j+) for (k = 0; k 8; k+) ATAi, j += ATi, k * Ak, j; /计算AT*L=ATL for (i = 0; i 6; i+) ATPLi, 0 = 0; for (k = 0; k 8; k+) ATPLi, 0 += ATi, k * Lk, 0; /将ATA转化为上三角矩阵 for (i = 0; i 6; i+) for (j = 0; j = j) ATANi, j = ATAi, j; else ATANi, j = 0.0; /将ATAN转化为一元矩阵atan double atan = new double21; k = 0; for (i = 0; i 6; i+) for (j = 0; j = i; j+) atank+ = ATANi, j; /求atan的逆矩阵 inverse(atan, 6); /将一元矩阵atan再次转化为6*6矩阵ATAN k = 0; for (i = 0; i 6; i+) for (j = 0; j = i; j+) ATANi, j = atank+; ATANj, i = ATANi, j; /求解误差方程结果V=ATAN*ATPL for (i = 0; i 6; i+) Vi, 0 = 0; for (k = 0; k = 0.001 | V1, 0 = 0.001 | V2, 0 = 0.001); /条件：当V0, 0，V1, 0，V2, 0的值小于0.001，退出循环 /在richtextbox2中输出结果 richTextBox2.Text += 外方位元素 XS, YS, ZS, Q, W, K 分别是： ; richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += Xs.ToString(f11); richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += Ys.ToString(f11); richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += Zs.ToString(f11); richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += Q.ToString(f11); richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += W.ToString(f11); richTextBox2.Text += n; richTextBox2.Text += ; richTextBox2.Text += K.ToString(f11); richTextBox2.Text += n; /转置函数 public void inverse(double b, int n) double a0=new doublen; for(int k=0;kn;k+) double a00=b0; if(a00+1.0=1.0) a0=null; for(int i=1;in;i+) double ai0 = bi*(i+1)/2; if (i = n - k - 1) a0i = -ai0 / a00; else a0i = ai0 / a00; for(int j=1;j=i;j+)b(i-1)*i/2+j-1=bi*(i+1)/2+j+ai0*a0j; for(int i=1;in;i+) b(n-1)*n/2+i-1=a0i; bn*(n+1)/2-1=1.0/a00; a0 = null; private