解析空中三角测量程序代码

using System;using System.Collections.Generic;using ponentModel;using System.Data;using System.Drawing;using System.Text;using System.Windows.Forms;using System.IO;using System.Data.OleDb;namespace 解析空中三角测量{public partial class Form1 : Form{public Form1(){InitializeComponent();}#region/////////////////////////////////////////////////////////////定义静态变量///////////////////////////////////////////////////////////////const int N = 150;int[] C = new int[3] { 15, 8, 11 };//各像对像点个数int[][] LDot = new int[3][];//像点点号int[] Control_Points = new int[4];//控制点点号int[] CheckPoint = new int[5];//检查点点号. double f = 153.033 / 1000.0;//主距double m;//比例尺//像对像点坐标double[][] x1 = new double[3][];double[][] y1 = new double[3][];double[][] x2 = new double[3][];double[][] y2 = new double[3][];//像点的像空间辅助坐标double[][] X1 = new double[3][];double[][] Y1 = new double[3][];double[][] Z1 = new double[3][];double[][] X2 = new double[3][];double[][] Y2 = new double[3][];double[][] Z2 = new double[3][];//相对定向元素double[] φ1 = new do uble[3];double[] ω1 = new double[3];double[] κ1 = new double[3];double[] φ2 = new double[3];double[] ω2 = new double[3];double[] κ2 = new double[3];double[] u = new double[3];double[] v = new double[3];//摄影基线分量double[] bx = new double[3];double[] by = new double[3];double[] bz = new double[3];//左、右像片投影系数N1,N2double[][] N1 = new double[3][];double[][] N2 = new double[3][];//上下视差Qdouble[][] Q = new double[3][];//模型点像空间辅助坐标double[][] Xm = new double[3][];double[][] Ym = new double[3][];double[][] Zm = new double[3][];//三个直线元素double[] Xs = new double[4];double[] Ys = new double[4];double[] Zs = new double[4];//模型点摄影测量坐标double[][] Xp = new double[3][];double[][] Yp = new double[3][];double[][] Zp = new double[3][];//绝对定向元素double ΔX, ΔY, ΔZ, λ, Φ, Ω,Κ;//控制点大地坐标double[] Xta = new double[4];double[] Yta = new double[4];double[] Zta = new double[4];//控制点地面摄影测量坐标double[] Xtpa = new double[4];double[] Ytpa = new double[4];double[] Ztpa = new double[4];//控制点摄影测量坐标double[] Xpa = new double[4];double[] Ypa = new double[4];double[] Zpa = new double[4];//模型点地面摄影测量坐标double[][] Xtp = new double[3][];double[][] Ytp = new double[3][];double[][] Ztp = new double[3][];//模型点大地坐标double[][] Xt = new double[3][];double[][] Yt = new double[3][];double[][] Zt = new double[3][];//检查点大地坐标double[] oXt = new double[5];double[] oYt = new double[5];double[] oZt = new double[5];//检查点的误差double[] oX = new double[5];double[] oY = new double[5];double[] oZ = new double[5];#endregion#region/////////////////////////////////////////////////////////////导入数据///////////////////////////////////////////////////////////////////private void 导入像点坐标数据ToolStripMenuItem_Click(object sender, EventArgs e){OpenFileDialog FILE = new OpenFileDialog();FILE.Filter = "文本文件(*.txt)|*.txt| 所有文件(*.*)|*.*";FILE.FilterIndex = 1;if (FILE.ShowDialog() == DialogResult.OK){StreamReader MSR = new StreamReader(FILE.FileName);string Line;//读取每行信息放在该字符串中double[,] Temp = new double[34, 5];//每行信息存放在该二维数组中int temp = 0;//将*.txt文件信息读到Temp[,]二维数组中while ((Line = MSR.ReadLine()) != null){char[] tt = new char[] { '\r', '\n' };string[] split = Line.Split(tt); //字符串转换为字符串数组string[] numbers;//读取的每行信息记录在数组中for (int i = 0; i < split.Length; i++){numbers = split[i].Split(' ');for (int j = 0, k = 0; j < numbers.Length && numbers[j] != ""; j++) Temp[temp, k++] = double.Parse(numbers[j]);}temp++;}MSR.Close();//用Temp中的数据赋值，并将单位换成米,并实例化像点坐标for (int i = 0; i < 3; i++){LDot[i] = new int[C[i]];x1[i] = new double[C[i]];y1[i] = new double[C[i]];x2[i] = new double[C[i]];y2[i] = new double[C[i]];}//像对1的坐标for (int i = 0; i < 15; i++){LDot[0][i] = (int)Temp[i, 0];x1[0][i] = Temp[i, 1] / 1000000.0;y1[0][i] = Temp[i, 2] / 1000000.0;x2[0][i] = Temp[i, 3] / 1000000.0;y2[0][i] = Temp[i, 4] / 1000000.0;}//像对2的坐标for (int i = 0; i < 8; i++){LDot[1][i] = (int)Temp[i + 15, 0];x1[1][i] = Temp[i + 15, 1] / 1000000.0;y1[1][i] = Temp[i + 15, 2] / 1000000.0;x2[1][i] = Temp[i + 15, 3] / 1000000.0;y2[1][i] = Temp[i + 15, 4] / 1000000.0;}//像对3的坐标for (int i = 0; i < 11; i++){LDot[2][i] = (int)Temp[i + 23, 0];x1[2][i] = Temp[i + 23, 1] / 1000000.0;y1[2][i] = Temp[i + 23, 2] / 1000000.0;x2[2][i] = Temp[i + 23, 3] / 1000000.0;y2[2][i] = Temp[i + 23, 4] / 1000000.0;}//显示像对坐标数据for (int i = 0; i < 34; i++){ListViewItem a;a = lst像对坐标数据.Items.Add(Temp[i, 0].ToString());a.SubItems.Add(Temp[i, 1].ToString());a.SubItems.Add(Temp[i, 2].ToString());a.SubItems.Add(Temp[i, 3].ToString());a.SubItems.Add(Temp[i, 4].ToString());}}}private void 导入控制点坐标数据ToolStripMenuItem_Click_1(object sender, EventArgs e){OpenFileDialog FILE = new OpenFileDialog();FILE.Filter = "文本文件(*.txt)|*.txt| 所有文件(*.*)|*.*";FILE.FilterIndex = 1;if (FILE.ShowDialog() == DialogResult.OK){StreamReader MSR = new StreamReader(FILE.FileName);string Line;//读取每行信息放在该字符串中double[,] Temp = new double[34, 5];//每行信息存放在该二维数组中int temp = 0;//将*.txt文件信息读到Temp[,]二维数组中while ((Line = MSR.ReadLine()) != null){char[] tt = new char[] { '\r', '\n' };string[] split = Line.Split(tt); //字符串转换为字符串数组string[] numbers;//读取的每行信息记录在数组中for (int i = 0; i < split.Length; i++){numbers = split[i].Split(' ');for (int j = 0, k = 0; j < numbers.Length && numbers[j] != ""; j++)Temp[temp, k++] = double.Parse(numbers[j]);}temp++;}MSR.Close();//用Temp中的数据赋值for (int i = 0; i < 4; i++){Control_Points[i] = (int)Temp[i, 0];Xta[i] = Temp[i, 1];Yta[i] = Temp[i, 2];Zta[i] = Temp[i, 3];}//显示控制点坐标数据for (int i = 0; i < 4; i++){ListViewItem a;a = lst控制点数据.Items.Add(Temp[i, 0].ToString());a.SubItems.Add(Temp[i, 1].ToString());a.SubItems.Add(Temp[i, 2].ToString());a.SubItems.Add(Temp[i, 3].ToString());}tab数据.SelectedTab = tab控制点数据;}}private void 导入检查点坐标数据ToolStripMenuItem_Click(object sender, EventArgs e){OpenFileDialog FILE = new OpenFileDialog();FILE.Filter = "文本文件(*.txt)|*.txt| 所有文件(*.*)|*.*";FILE.FilterIndex = 1;if (FILE.ShowDialog() == DialogResult.OK){StreamReader MSR = new StreamReader(FILE.FileName);string Line;//读取每行信息放在该字符串中double[,] Temp = new double[34, 5];//每行信息存放在该二维数组中int temp = 0;//将*.txt文件信息读到Temp[,]二维数组中while ((Line = MSR.ReadLine()) != null){char[] tt = new char[] { '\r', '\n' };string[] split = Line.Split(tt); //字符串转换为字符串数组string[] numbers;//读取的每行信息记录在数组中for (int i = 0; i < split.Length; i++){numbers = split[i].Split(' ');for (int j = 0, k = 0; j < numbers.Length && numbers[j] != ""; j++)Temp[temp, k++] = double.Parse(numbers[j]);}temp++;}MSR.Close();//用Temp中的数据赋值for (int i = 0; i < 5; i++){CheckPoint[i] = (int)Temp[i, 0];oXt[i] = Temp[i, 1];oYt[i] = Temp[i, 2];oZt[i] = Temp[i, 3];}//显示控制点坐标数据for (int i = 0; i < 5; i++){ListViewItem a;a = lst检查点数据.Items.Add(Temp[i, 0].ToString());a.SubItems.Add(Temp[i, 1].ToString());a.SubItems.Add(Temp[i, 2].ToString());a.SubItems.Add(Temp[i, 3].ToString());}}tab数据.SelectedTab = tab检查点坐标数据;}#endregion#region/////////////////////////////////////////////////////////////矩阵运算/////////////////////////////////////////////////////////////////////矩阵的转置运算private void Transpose(double[,] A, double[,] A T){int i, j;for (i = 0; i < A.GetLength(1); i++){for (j = 0; j < A.GetLength(0); j++){A T[i, j] = A[j, i];}}}//矩阵的乘法运算private void Matrix(double[,] AT, double[,] A, double[,] ATA){int i, j, k;for (i = 0; i < ATA.GetLength(0); i++){for (j = 0; j < ATA.GetLength(1); j++){A TA[i, j] = 0;for (k = 0; k < A T.GetLength(1); k++)ATA[i, j] += AT[i, k] * A[k, j];}}}// 矩阵求逆private void Inverse(double[,] A,double[,] AR){int i,j,h,k;int n = A.GetLength(0);double P;double[,] Q=new double[A.GetLength(0),2*A.GetLength(1)];for(i=0;i<n;i++) //1 2 3 //构造高斯矩阵for(j=0;j<n;j++) //2 3 4Q[i,j]=A[i,j]; //5 3 2for(i=0;i<n;i++)for(j=n;j<2*n;j++){ //1 2 3 1 0 0if(i+n==j) //2 3 4 0 1 0Q[i,j]=1; //5 3 2 0 0 1elseQ[i,j]=0;}for(h=k=0;k<n-1;k++,h++)//消去对角线以下的数据for(i=k+1;i<n;i++){if(Q[k,h]==0) // 0 X X 1 0 0for(int g=0;g<n;g++) // X X X 0 1 0if(Q[g,h]!=0) // X X x 0 0 1{ // 这种特殊情况的判断以及处理方式for(j=0;j<2*n;j++)Q[k,j]-=Q[g,j];break;}if(Q[i,h]==0) //将矩阵化为X X X X X Xcontinue; // 0 X X X X XP=Q[k,h]/Q[i,h]; // 0 0 X X X Xfor(j=0;j<2*n;j++) // 的形式{Q[i,j]*=P;Q[i,j]-=Q[k,j];}}for(h=k=n-1;k>0;k--,h--) // 消去对角线以上的数据for(i=k-1;i>=0;i--) // 此时无需考虑{if(Q[i,h]==0) // X X X X X Xcontinue; // 0 X X X X XP=Q[k,h]/Q[i,h]; // 0 0 0 X X Xfor(j=0;j<2*n;j++) //这种情况，因为此时矩阵对应的行列式值为0，即该矩阵不存在逆矩阵{Q[i,j]*=P;Q[i,j]-=Q[k,j];}}for(i=0;i<n;i++)//将对角线上数据化为1{P=1.0/Q[i,i];for(j=0;j<2*n;j++)Q[i,j]*=P;}for(i=0;i<n;i++) //提取逆矩阵for(j=0;j<n;j++)AR[i,j]=Q[i,j+n];}#endregion#region/////////////////////////////////////////////////////////////相对定向/////////////////////////////////////////////////////////////////// //初始化private void Initial(int i){//初始值if (i == 0){φ1[0] = 0;ω1[0] = 0;κ1[0] = 0;φ2[0] = 0;ω2[0] = 0;κ2[0] = 0;}else{φ1[i] = φ2[i - 1];ω1[i] = ω2[i - 1];κ1[i] = κ2[i - 1];φ2[i] = 0;ω2[i] = 0;κ2[i] = 0;}u[i] = 0;v[i] = 0;bx[i] = 200.0 / 1000.0;//像点像空间辅助坐标X1[i] = new double[C[i]];Y1[i] = new double[C[i]];Z1[i] = new double[C[i]];X2[i] = new double[C[i]];Y2[i] = new double[C[i]];Z2[i] = new double[C[i]];//投影系数和上下视差N1[i] = new double[C[i]];N2[i] = new double[C[i]];Q[i] = new double[C[i]];//模型点像空间辅助坐标Xm[i] = new double[C[i]];Ym[i] = new double[C[i]];Zm[i] = new double[C[i]];}//相对定向private void Directional(int i){//定义变量int n = 0;//统计迭代次数double d = 0.00000001;//精度double[,] R1 = new double[3, 3];//旋转矩阵R1double[,] R2 = new double[3, 3];//旋转矩阵R2double[,] A = new double[C[i], 5];//系数阵Adouble[,] AT = new double[5, C[i]];//A的转置ATdouble[,] ATA = new double[5, 5];//AT与A的乘积A TAdouble[,] ATAR = new double[5, 5];///ATA的逆阵ATARdouble[,] ATART = new double[5, C[i]];//ATAR的转置A TARTdouble[,] L = new double[C[i], 1];//误差方程中的常数项double[,] dx = new double[5, 1];//相对定向元素改正数Initial(i);//左片旋转矩阵R1,书79页R1[0, 0] = Math.Cos(φ1[i]) * Math.Cos(κ1[i]) - Math.Sin(φ1[i]) * Math.Sin(ω1[i]) * Math.Sin(κ1[i]);R1[0, 1] = -Math.Cos(φ1[i]) * Math.Sin(κ1[i]) - Math.Sin(φ1[i]) * Math.Sin(ω1[i]) * Math.Cos(κ1[i]);R1[0, 2] = -Math.Sin(φ1[i]) * Math.Cos(ω1[i]);R1[1, 0] = Math.Cos(ω1[i]) * Math.Sin(κ1[i]);R1[1, 1] = Math.Cos(ω1[i]) * Math.Cos(κ1[i]);R1[1, 2] = -Math.Sin(ω1[i]);R1[2, 0] = Math.Sin(φ1[i]) * Math.Cos(κ1[i]) + Math.Cos(φ1[i]) * Math.Sin(ω1[i]) * Math.Sin(κ1[i]);R1[2, 1] = -Math.Sin(φ1[i]) * Math.Sin(κ1[i]) + Math.Cos(φ1[i]) * Math.Sin(ω1[i]) * Math.Cos(κ1[i]);R1[2, 2] = Math.Cos(φ1[i]) * Math.Cos(ω1[i]);//左片像点像空辅坐标for (int j = 0; j < C[i]; j++){X1[i][j] = R1[0, 0] * x1[i][j] + R1[0, 1] * y1[i][j] - R1[0, 2] * f;Y1[i][j] = R1[1, 0] * x1[i][j] + R1[1, 1] * y1[i][j] - R1[1, 2] * f;Z1[i][j] = R1[2, 0] * x1[i][j] + R1[2, 1] * y1[i][j] - R1[2, 2] * f;}do{//by,bzby[i] = bx[i] * u[i];bz[i] = bx[i] * v[i];//右片旋转矩阵R2,书79页R2[0, 0] = Math.Cos(φ2[i]) * Math.Cos(κ2[i]) - Math.Sin(φ2[i]) * Math.Si n(ω2[i]) * Math.Sin(κ2[i]);R2[0, 1] = -Math.Cos(φ2[i]) * Math.Sin(κ2[i]) - Math.Sin(φ2[i]) * Math.Sin(ω2[i]) * Math.Cos(κ2[i]);R2[0, 2] = -Math.Sin(φ2[i]) * Math.Cos(ω2[i]);R2[1, 0] = Math.Cos(ω2[i]) * Math.Sin(κ2[i]);R2[1, 1] = Math.Cos(ω2[i]) * Math.Cos(κ2[i]);R2[1, 2] = -Math.Sin(ω2[i]);R2[2, 0] = Math.Sin(φ2[i]) * Math.Cos(κ2[i]) + Math.Cos(φ2[i]) * Math.Sin(ω2[i]) * Math.Sin(κ2[i]);R2[2, 1] = -Math.Sin(φ2[i]) * Math.Sin(κ2[i]) + Math.Cos(φ2[i]) * Math.Sin(ω2[i]) * Math.Cos(κ2[i]);R2[2, 2] = Math.Cos(φ2[i]) * Math.Cos(ω2[i]);for (int j = 0; j < C[i]; j++){//右片像点像空辅坐标X2[i][j] = R2[0, 0] * x2[i][j] + R2[0, 1] * y2[i][j] - R2[0, 2] * f;Y2[i][j] = R2[1, 0] * x2[i][j] + R2[1, 1] * y2[i][j] - R2[1, 2] * f;Z2[i][j] = R2[2, 0] * x2[i][j] + R2[2, 1] * y2[i][j] - R2[2, 2] * f;//N1,N2,QN1[i][j] = (bx[i] * Z2[i][j] - bz[i] * X2[i][j]) / (X1[i][j] * Z2[i][j] - X2[i][j] * Z1[i][j]);N2[i][j] = (bx[i] * Z1[i][j] - bz[i] * X1[i][j]) / (X1[i][j] * Z2[i][j] - X2[i][j] * Z1[i][j]);Q[i][j] = N1[i][j] * Y1[i][j] - N2[i][j] * Y2[i][j] - by[i];//误差方程系数矩阵A,书83页A[j, 0] = bx[i];A[j, 1] = -(Y2[i][j] / Z2[i][j]) * bx[i];A[j, 2] = -((X2[i][j] * Y2[i][j]) / Z2[i][j]) * N2[i][j];A[j, 3] = -(Z2[i][j] + Y2[i][j] * Y2[i][j] / Z2[i][j]) * N2[i][j];A[j, 4] = X2[i][j] * N2[i][j];//常数项LL[j, 0] = Q[i][j];}//A的转置ATTranspose(A, A T);//AT与A的乘积A TAMatrix(A T, A, ATA);//ATA的逆阵ATARInverse(ATA, A TAR);//ATAR与AT的乘积ATARTMatrix(A TAR, A T, ATART);//ATART *与L的乘积dxMatrix(A TART, L, dx);n++;if (n > N){MessageBox.Show("计算" + (n - 1)+"次后计算失败！", "请检查迭代次数和限差！", MessageBoxButtons.OK, MessageBoxIcon.Warning);break;}//相对定向元素新值u[i] += dx[0, 0];v[i] += dx[1, 0];φ2[i] += dx[2, 0];ω2[i] += dx[3, 0];κ2[i] += dx[4, 0];} while (Math.Abs(dx[0, 0]) >= d || Math.Abs(dx[1, 0]) >= d || Math.Abs(dx[2, 0]) >= d || Math.Abs(dx[3, 0]) >= d || Math.Abs(dx[4, 0]) >= d);//显示相对定向元素ListViewItem a;a = lst相对定向元素.Items.Add((i + 1).ToString());a.SubItems.Add(u[i].ToString("0.000000"));a.SubItems.Add(v[i].ToString("0.000000"));a.SubItems.Add(φ2[i].ToString("0.000000"));a.SubItems.Add(ω2[i].ToString("0.000000"));a.SubItems.Add(κ2[i].ToString("0.000000"));//模型点的像空间辅助坐标for (int j = 0; j < C[i]; j++){Xm[i][j] = N1[i][j] * X1[i][j];Ym[i][j] = (N1[i][j] * Y1[i][j] + N2[i][j] * Y2[i][j] + by[i]) * 0.5;Zm[i][j] = N1[i][j] * Z1[i][j];}tab处理.SelectedTab = tab相对定向;}private void 相对定向ToolStripMenuItem_Click(object sender, EventArgs e){//相对定向3次for (int i = 0; i < 3; i++)Directional(i);}#endregion#region/////////////////////////////////////////////////////////////模型连接,绝对定向,结果检查/////////////////////////////////////////////////private void 模型连接ToolStripMenuItem_Click_1(object sender, EventArgs e){//摄站的摄影测量坐标double[] Xps = new double[4];double[] Yps = new double[4];double[] Zps = new double[4];//归化系数double[] k = new double[3];double[] kk = new double[2];//比例尺归化系数for (int i = 0; i < 2; i++){int num = 0;for (int l = 0; l < C[i]; l++){for (int j = 0; j < C[i + 1]; j++){if (LDot[i][l] == LDot[i + 1][j]){if (i == 0)kk[i] += (Zm[i][l] - bz[i]) / Zm[i + 1][j];elsekk[i] += (Zm[i][l] - bz[i]) * kk[i - 1] / Zm[i + 1][j];num++;}}}kk[i] = kk[i] / num;//平均数}k[0] = 1;k[1] = kk[0];k[2] = kk[1];lblK1.Text = "k1=" + k[1].ToString("0.000000");lblK2.Text = "k2=" + k[2].ToString("0.000000");//比例尺m(1201,1205,1206)double[] MID = new double[3];/////////////////////////////////////////////每段长度比MID[0] = (Math.Sqrt((Xta[0] - Xta[1]) * (Xta[0] - Xta[1]) + (Yta[0] - Yta[1]) * (Yta[0] - Yta[1]))) /(Math.Sqrt((x1[0][0] - x1[0][5]) * (x1[0][0] - x1[0][5]) + (y1[0][0] - y1[0][5]) * (y1[0][0] - y1[0][5])));MID[1] = (Math.Sqrt((Xta[0] - Xta[2]) * (Xta[0] - Xta[2]) + (Yta[0] - Yta[2]) * (Yta[0] - Yta[2]))) /(Math.Sqrt((x1[0][0] - x1[0][6]) * (x1[0][0] - x1[0][6]) + (y1[0][0] - y1[0][6]) * (y1[0][0] - y1[0][6])));MID[2] = (Math.Sqrt((Xta[2] - Xta[1]) * (Xta[2] - Xta[1]) + (Yta[2] - Yta[1]) * (Yta[2] - Yta[1]))) /(Math.Sqrt((x1[0][6] - x1[0][5]) * (x1[0][6] - x1[0][5]) + (y1[0][6] - y1[0][5]) * (y1[0][6] - y1[0][5])));m = (MID[0] + MID[1] +MID[2]) / 3;lblm.Text = "m=" + m.ToString("0.000000");//模型摄站S的摄影测量坐标for (int i = 0; i < 4; i++){if (i == 0){Xps[0] = 0.0;Yps[0] = 0.0;Zps[0] = m * f;}else{Xps[i] = Xps[i - 1] + m * k[i - 1] * bx[i - 1];Yps[i] = Yps[i - 1] + m * k[i - 1] * by[i - 1];Zps[i] = Zps[i - 1] + m * k[i - 1] * bz[i - 1];}}//实例化模型点的摄影测量坐标for (int i = 0; i < 3; i++){Xp[i] = new double[C[i]];Yp[i] = new double[C[i]];Zp[i] = new double[C[i]];}//各模型点的摄影测量坐标for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){Xp[i][j] = Xps[i] + k[i] * m * N1[i][j] * X1[i][j];Yp[i][j] = 0.5 * (Yps[i] + k[i] * m * N1[i][j] * Y1[i][j] + Yps[i + 1] + k[i] * m * N2[i][j] * Y2[i][j]);Zp[i][j] = Zps[i] + k[i] * m * N1[i][j] * Z1[i][j];}ListViewItem a;for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){a = lst模型连接.Items.Add((LDot[i][j]).ToString());a.SubItems.Add(Xp[i][j].ToString("0.0000000"));a.SubItems.Add(Yp[i][j].ToString("0.0000000"));a.SubItems.Add(Zp[i][j].ToString("0.0000000"));}tab处理.SelectedTab = tab模型连接;}private void 绝对定向ToolStripMenuItem_Click(object sender, EventArgs e) {double ΔXt = 0, ΔYt = 0;//大地坐标差double ΔXp = 0, ΔYp = 0;//摄影测量坐标差double a, b, λ1;int n_1 = 0;//标记控制点1bool flag;//选定1和2两个控制点//从第一个模型里找到1点flag = false;for (int i = 0; i < 4; i++){for (int j = 0; j < C[0]; j++){if (LDot[0][j] == Control_Points[i]){ΔXt = Xta[i];ΔYt = Yta[i];ΔXp = Xp[0][j];ΔYp = Yp[0][j];n_1 = i;flag = true;}if (flag)break;}if (flag)break;}//从第三个模型里找到2点flag = false;for (int i = 0; i < 4; i++){for (int j = 0; j < C[2]; j++){if (LDot[2][j] == Control_Points[i]){ΔXt = Xta[i] - ΔXt;ΔYt = Yt a[i] - ΔYt;ΔXp = Xp[2][j] - ΔXp;ΔYp = Yp[2][j] - ΔYp;flag = true;}if (flag)break;}if (flag)break;}//a,b,λ1,书100页a = (ΔXp * ΔYt + ΔYp * ΔXt) / (ΔXt * ΔXt + ΔYt * ΔYt);b = (ΔXp * ΔXt - ΔYp * ΔYt) / (ΔXt * ΔXt + ΔYt * ΔYt);λ1 = Math.Sqrt(a * a + b * b);//控制点大地坐标转换为地面摄影测量坐标,书100页，6-2-11 for (int i = 0; i < 4; i++){Xtpa[i] = b * (Xta[i] - Xta[n_1]) + a * (Yta[i] - Yta[n_1]);Ytpa[i] = a * (Xta[i] - Xta[n_1]) - b * (Yta[i] - Yta[n_1]);Ztpa[i] = λ1 * (Zta[i] - Zta[n_1]);}//相同点号的控制点与对应的模型点for (int l = 0; l < 4; l++)for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){if (LDot[i][j] == Control_Points[l]){Xpa[l] = Xp[i][j];Ypa[l] = Yp[i][j];Zpa[l] = Zp[i][j];}}int n = 0;double d = 0.0000001;double[,] R = new double[3, 3];//旋转矩阵Rdouble[,] A = new double[12, 7];//系数阵Adouble[,] AT = new double[7, 12];//A的转置ATdouble[,] A TA = new double[7, 7];//AT与A的乘积A TA double[,] ATAR = new double[7, 7];//ATA的逆阵ATAR double[,] ATART = new double[7, 12];//ATAR的转置ATARTdouble[,] L = new double[12, 1];//常数项double[,] dx = new double[7, 1];//改正数dxΔX = 0; ΔY = 0; ΔZ = 0; λ = 1; Φ = 0; Ω = 0; Κ = 0; //绝对定向元素的初始值for (int i = 0; i < 4; i++)//系数矩阵A{A[3 * i, 0] = 1;A[3 * i, 1] = 0;A[3 * i, 2] = 0;A[3 * i, 3] = Xpa[i];A[3 * i, 4] = -Zpa[i];A[3 * i, 5] = 0;A[3 * i, 6] = -Ypa[i];A[3 * i + 1, 0] = 0;A[3 * i + 1, 1] = 1;A[3 * i + 1, 2] = 0;A[3 * i + 1, 3] = Ypa[i];A[3 * i + 1, 4] = 0;A[3 * i + 1, 5] = -Zpa[i];A[3 * i + 1, 6] = Xpa[i];A[3 * i + 2, 0] = 0;A[3 * i + 2, 1] = 0;A[3 * i + 2, 2] = 1;A[3 * i + 2, 3] = Zpa[i];A[3 * i + 2, 4] = Xpa[i];A[3 * i + 2, 5] = Ypa[i];A[3 * i + 2, 6] = 0;}do{//计算旋转矩阵R,书79页R[0, 0] = Math.Cos(Φ) * Math.Cos(Κ) - Math.Sin(Φ) * Math.Sin(Ω) * Math.Sin(Κ);R[0, 1] = -Math.Cos(Φ) * Math.Sin(Κ) - Math.Sin(Φ) * Math.Sin(Ω) * Math.Cos(Κ);R[0, 2] = -Math.Sin(Φ) * Math.Cos(Ω);R[1, 0] = Math.Cos(Ω) * Math.Sin(Κ);R[1, 1] = Math.Cos(Ω) * Math.Cos(Κ);R[1, 2] = -Math.Sin(Ω);R[2, 0] = Math.Sin(Φ) * Math.Cos(Κ) + Math.Cos(Φ) * Math.Sin(Ω) * Math.Sin(Κ);R[2, 1] = -Math.Sin(Φ) * Math.Sin(Κ) + Math.Cos(Φ) * Math.Sin(Ω) * Math.Cos(Κ);R[2, 2] = Math.Cos(Φ) * Math.Cos(Ω);//c3//常数项Lfor (int i = 0; i < 4; i++){L[3 * i, 0] = Xtpa[i] - λ * (Xpa[i] - Κ * Ypa[i] - Φ * Zpa[i]) - ΔX;L[3 * i + 1, 0] = Ytpa[i] - λ * (Κ * Xpa[i] + Ypa[i] - Ω * Zpa[i]) - ΔY;L[3 * i + 2, 0] = Ztpa[i] - λ * (Φ * Xpa[i] + Ω * Ypa[i] + Zpa[i]) - ΔZ;}//A的转置ATTranspose(A, A T);//AT与A的乘积A TAMatrix(A T, A, ATA);//ATA的逆矩阵A TARInverse(ATA,A TAR);//ATAR与AT的乘积ATARTMatrix(A TAR, A T, ATART);//ATART与L的乘积dxMatrix(A TART, L, dx);n++;if (n > N){MessageBox.Show("计算" + (n - 1) + "次后计算失败！", "请检查迭代次数和限差！", MessageBoxButtons.OK, MessageBoxIcon.Warning);break;}//绝对定向元素新值ΔX += dx[0, 0];ΔY += dx[1, 0];ΔZ += dx[2, 0];λ += dx[3, 0];Φ += dx[4, 0];Ω += dx[5, 0];Κ += dx[6, 0];} while (Math.Abs(dx[0, 0]) >= d || Math.Abs(dx[1, 0]) >= d || Math.Abs(dx[2, 0]) >= d || Math.Abs(dx[3, 0]) >= d|| Math.Abs(dx[4, 0]) >= d || Math.Abs(dx[5, 0]) >= d || Math.Abs(dx[6, 0]) >= d);lblΔX.Text = "ΔX=" + ΔX.ToString("0.000000");lblΔY.Text = "ΔY=" + ΔY.ToString("0.000000");lblΔZ.Text = "ΔZ=" + ΔZ.ToString("0.000000");lblλ.Text = "λ=" + λ.ToString("0.000000");lblΦ.Text = "Φ=" + Φ.ToString("0.000000");lblΩ.Text = "Ω=" + Ω.ToString("0.000000");lblΚ.Text = "Κ=" + Κ.ToString("0.000000");//加密点的大地坐标//实例化模型点地面摄影测量坐标和大地坐标for (int i = 0; i < 3; i++){Xtp[i] = new double[C[i]];Ytp[i] = new double[C[i]];Ztp[i] = new double[C[i]];Xt[i] = new double[C[i]];Yt[i] = new double[C[i]];Zt[i] = new double[C[i]];}//模型点地面摄影测量坐标for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){Xtp[i][j] = λ * (Xp[i][j] - Κ * Yp[i][j] - Φ * Zp[i][j]) + ΔX;Ytp[i][j] = λ * (Κ * Xp[i][j] + Yp[i][j] - Ω * Zp[i][j]) + ΔY;Ztp[i][j] = λ * (Φ * Xp[i][j] + Ω * Yp[i][j] + Zp[i][j]) + ΔZ;}//地面摄影测量坐标转换为大地坐标for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){Xt[i][j] = (b * Xtp[i][j] + a * Ytp[i][j]) / (λ1 * λ1) + Xta[n_1];Yt[i][j] = (a * Xtp[i][j] - b * Ytp[i][j]) / (λ1 * λ1) + Yta[n_1];Zt[i][j] = Ztp[i][j] / λ1 + Zta[n_1];}ListViewItem g;for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){g = lst绝对定向.Items.Add((LDot[i][j]).ToString());g.SubItems.Add(Xt[i][j].ToString("0.000000"));g.SubItems.Add(Yt[i][j].ToString("0.000000"));g.SubItems.Add(Zt[i][j].ToString("0.000000"));}tab处理.SelectedTab = tab绝对定向;}private void 结果检查ToolStripMenuItem_Click(object sender, EventArgs e){//////////////////////////////////////////////////////////////////////////计算检查点与与之对应像点解求的大地坐标之间的差值for (int l = 0; l < 5; l++)for (int i = 0; i < 3; i++)for (int j = 0; j < C[i]; j++){if (CheckPoint[l] == LDot[i][j]){oX[l] = Xt[i][j] - oXt[l];oY[l] = Yt[i][j] - oYt[l];oZ[l] = Zt[i][j] - oZt[l];}}//////////////////////////////////////////////////////////////////////////在窗口中显示检查结果ListViewItem g;for (int i = 0; i < 5; i++){g = lst检查结果.Items.Add((CheckPoint[i]).ToString());g.SubItems.Add(oX[i].ToString("0.000000"));g.SubItems.Add(oY[i].ToString("0.000000"));g.SubItems.Add(oZ[i].ToString("0.000000"));}tab处理.SelectedTab = tab结果检查;}#endregion#region/////////////////////////////////////////////////////////////退出///////////////////////////////////////////////////////////////////////private void 退出ToolStripMenuItem_Click(object sender, EventArgs e){SaveFileDialog FILE = new SaveFileDialog();FILE.Filter = "文本文件(*.txt)|*.txt| 所有文件(*.*)|*.*";FILE.FilterIndex = 1;if (FILE.ShowDialog() == DialogResult.OK){StreamWriter MSW = new StreamWriter(FILE.FileName);MSW.WriteLine("########################################1.相对定向元素####################################");for (int i = 0; i < 3; i++){MSW.WriteLine("像对" + Convert.ToString(i + 1));MSW.WriteLine("u=" + u[i].ToString("0.000000"));MSW.WriteLine("v=" + v[i].ToString("0.000000"));MSW.WriteLine("φ=" + φ2[i].ToString("0.000000"));MSW.WriteLine("ω=" + ω2[i].ToString("0.000000"));MSW.WriteLine("κ=" + κ2[i].ToString("0.000000"));MSW.WriteLine(" ");}for (int i = 0; i < 3; i++){MSW.WriteLine("第" + (i + 1) + "个模型点的像空间辅助坐标：");MSW.WriteLine(" ");MSW.WriteLine("点号" + "\t\t" + "Xm" + "\t\t" + " Ym" + "\t\t" + " Zm");for (int j = 0; j < C[i]; j++){MSW.WriteLine(LDot[i][j] + " " + Xm[i][j].ToString("0.0000000") + " " + Ym[i][j].ToString("0.0000000") + " " + Zm[i][j].ToString("0.0000000"));}MSW.WriteLine(" ");}MSW.WriteLine("########################################2.模型连接########################################");for (int i = 0; i < 3; i++){MSW.WriteLine("第" + (i + 1) + "个模型点的摄影测量坐标：");MSW.WriteLine(" ");MSW.WriteLine("点号" + "\t\t" + "Xp" + "\t\t" + " Yp" + "\t\t" + " Zp");for (int j = 0; j < C[i]; j++){MSW.WriteLine(LDot[i][j] + " " + Xp[i][j].ToString("0.0000000") + " " + Yp[i][j].ToString("0.0000000") + " " + Zp[i][j].ToString("0.0000000"));}MSW.WriteLine(" ");}MSW.WriteLine(" ");MSW.WriteLine("########################################3.绝对定向########################################");MSW.WriteLine(" ");MSW.WriteLine("绝对定向元素：");MSW.WriteLine("ΔX=" + ΔX.ToString("0.000000"));MSW.WriteLine("ΔY=" + ΔY.ToString("0.000000"));MSW.WriteLine("ΔZ=" + ΔZ.ToString("0.000000"));MSW.WriteLine("λ=" + λ.ToString("0.000000"));MSW.WriteLine("Φ=" + Φ.ToString("0.000000"));MSW.WriteLine("Ω=" + Ω.ToString("0.000000"));MSW.WriteLine("Κ=" + Κ.ToString("0.000000"));MSW.WriteLine(" ");for (int i = 0; i < 3; i++){MSW.WriteLine("第" + (i + 1) + "个模型点的地面摄影测量坐标为：");MSW.WriteLine(" ");MSW.WriteLine("点号" + "\t\t" + "Xtp" + "\t\t" + "Ytp" + "\t\t" + "Ztp");for (int j = 0; j < C[i]; j++){MSW.WriteLine(LDot[i][j] + "\t" + " " + Xtp[i][j].ToString("0.000000") + "\t" + " " + Ytp[i][j].ToString("0.000000") + "\t" + " " + Ztp[i][j].ToString("0.000000"));}MSW.WriteLine(" ");}for (int i = 0; i < 3; i++){MSW.WriteLine("第" + (i + 1) + "个像对的大地坐标为：");MSW.WriteLine(" ");MSW.WriteLine("点号" + "\t\t" + "Xt" + "\t\t" + "Yt" + "\t\t" + "Zt");for (int j = 0; j < C[i]; j++){MSW.WriteLine(LDot[i][j] + "\t" + " " + Xt[i][j].ToString("0.000000") + "\t" + " " + Yt[i][j].ToString("0.000000") + "\t" + " " + Zt[i][j].ToString("0.000000"));}MSW.WriteLine(" ");}MSW.WriteLine(" ");MSW.WriteLine("########################################4.结果检查########################################");MSW.WriteLine(" ");MSW.WriteLine("点号" + "\t\t" + "ΔXt" + "\t\t" + "ΔYt" + "\t\t" + "ΔZt");for (int i = 0; i < 5; i++){MSW.WriteLine(CheckPoint[i] + "\t\t" + oX[i].ToString("0.000000") + "\t\t" + oY[i].ToString("0.000000") + "\t\t" + oZ[i].ToString("0.000000"));}MSW.Close();MessageBox.Show("成功保存到" + FILE.FileName);}。