using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; namespace 单像空间后方交会解算 { class Program { static void Main(string[] args) { #region 变量定义 交会解算 jh = new 交会解算(); double f, x0, y0; //内方位元素 int No; //控制点计数 int lineNo; //线条的数目 double sum = 0; int k = 0; //用来须要对进行循环的做为数组下标 double scale = 0; //平均比例尺 double H = 0; //平均航高 double[] L; //用存放地面距离 double[] GeoX = null, GeoY = null, GeoZ = null; //地面点坐标 double[] Phox = null, Phoy = null; //像点观测值 double[,] R = new double[3, 3]; //声明旋转矩阵 double a1, a2, a3, b1, b2, b3, c1, c2, c3; //旋转元素 double Xs, Ys, Zs, Phi, Omega, Kappa; //定义外方位元素 double[] x, y; //像点近似值 double[,] A ,AT,RevA,Const; //定义偏差方程系数矩阵和转置矩阵,A的逆矩阵和AT*constx_y double XBar, YBar, ZBar; //将共线条件方程简化 double[,] constx_y; //用于存放偏差方程常数项 double[,] X; //最后的结果矩阵 double dX, dY, dZ, dPhi, dOmega, dKappa; //最后的结果 string yesno="Y"; //若是是Y则继续迭代 int times = 1; //迭代次数 #endregion Console.WriteLine("欢迎使用这个程序计算单像空间后方交会"); #region 初始化内方位元素 Console.Write("摄影主距f:");//用来存内方位元素 f = 153.24; Console.WriteLine(f+"mm"); Console.Write("内方位元素x0:"); x0 = 0; Console.WriteLine(x0+"mm"); Console.Write("内方位元素y0:"); y0 = 0; Console.WriteLine(y0+"mm"); #endregion #region 输入地面摄影测量坐标 Console.WriteLine("输入的控制点的个数：4"); No = 4; GeoX = new double[No]; //存入地面坐标 GeoY = new double[No]; GeoZ = new double[No]; GeoX[0] = 36589410; GeoX[1] = 37631080; GeoX[2] = 39100970; GeoX[3] = 40426540; GeoY[0] = 25273320; GeoY[1] = 31324510; GeoY[2] = 24934980; GeoY[3] = 30319810; GeoZ[0] = 2195170; GeoZ[1] = 728690; GeoZ[2] = 2386500; GeoZ[3] = 757310; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("单位：mm"); for (int i = 0; i < No; i++)//遍历摄影测量坐标 { Console.WriteLine("第{0}个点的地面摄影测量坐标X/Y/Z：\t{1}，\t{2}，\t{3}", i + 1, GeoX[i], GeoY[i], GeoZ[i]); } #endregion #region 输入像点观测值 Phox =new double [No ]; Phoy = new double[No]; Phox [0]=-86.15; Phox [1]=-53.4; Phox [2]=-14.78; Phox[3] = 10.46; Phoy [0]=-68.99; Phoy [1]=82.21; Phoy [2]=-76.63 ; Phoy[3] = 64.43; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("单位：mm"); for (int i = 0; i < No; i++) { Console.WriteLine("第{0}个控制点的对应像点坐标x/y：\t{1}， \t{2}", i + 1, Phox[i], Phoy[i]); }//遍历像点坐标 #endregion #region 计算比例尺和航高 lineNo = jh.LineNo(No); L = new double[lineNo]; //用来存放地面距离 for (int i = 0; i < No; i++)//计算地面点的距离 { for (int j = i + 1; j < No; j++) { L[k] = jh.GetLength(GeoX, GeoY, i, j); k++; } } k = 0; //归零 double[] l = new double[lineNo]; for (int i = 0; i < No; i++)//计算像点的距离 { for (int j = i + 1; j < No; j++) { l[k] = jh.GetLength(Phox, Phoy, i, j); k++; } } k = 0; //归零 for (int i = 0; i < lineNo; i++)//比例尺求和 { sum += l[i] / L[i]; } scale = sum / lineNo;//记录平均比例尺 sum = 0; //归零 Console.WriteLine("比例尺：{0}", scale); H = f / scale; //平均航高 Console.WriteLine("平均航高：{0}", H); #endregion #region 初始化外方位元素并计算旋转矩阵 for (int i = 0; i < No; i++)//计算Xs { sum += GeoX[i]; } Xs = sum / No; sum = 0; //归零 for (int i = 0; i < No; i++)//Ys计算 { sum += GeoY[i]; } Ys = sum / No; sum = 0; //归零 Zs = H;//Zs计算 Phi = 0; Omega = 0; Kappa = 0; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("单位：mm"); Console.WriteLine(" \tXs\tYs\tZs\tPhi\tOmega\tKappa"); Console.WriteLine("外方位元素近似值：\t{0}\t{1}\t{2}\t{3}\t{4}\t{5}",Xs ,Ys ,Zs ,Phi ,Omega ,Kappa ); while (yesno == "y" || yesno == "Y") { //计算旋转矩阵 jh.CalRotMat(R, Phi, Omega, Kappa); a1 = R[0, 0]; a2 = R[0, 1]; a3 = R[0, 2]; b1 = R[1, 0]; b2 = R[1, 1]; b3 = R[1, 2]; c1 = R[2, 0]; c2 = R[2, 1]; c3 = R[2, 2];//为方便后续计算用abc来代替具体的元素索引 Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("旋转矩阵是:"); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { Console.Write("\t{0}", R[i, j]); } Console.WriteLine(); } //输出旋转矩阵 #endregion #region 计算像点近似值 x = new double[No]; y = new double[No]; for (int i = 0; i < No; i++)//计算像点坐标近似值 { XBar = a1 * (GeoX[i] - Xs) + b1 * (GeoY[i] - Ys) + c1 * (GeoZ[i] - Zs); YBar = a2 * (GeoX[i] - Xs) + b2 * (GeoY[i] - Ys) + c2 * (GeoZ[i] - Zs); ZBar = a3 * (GeoX[i] - Xs) + b3 * (GeoY[i] - Ys) + c3 * (GeoZ[i] - Zs); x[i] = -f * XBar / ZBar; y[i] = -f * YBar / ZBar; } Console.WriteLine("像点近似值是: x\\y mm"); for (int i = 0; i < No; i++) { Console.WriteLine(" \t{0} \t{1}", x[i], y[i]); }//近似值输出 #endregion #region 计算偏差方程系数与常数项 A = new double[2 * No, 6]; for (int i = 0; i < 2 * No; i++) { XBar = a1 * (GeoX[k] - Xs) + b1 * (GeoY[k] - Ys) + c1 * (GeoZ[k] - Zs); YBar = a2 * (GeoX[k] - Xs) + b2 * (GeoY[k] - Ys) + c2 * (GeoZ[k] - Zs); ZBar = a3 * (GeoX[k] - Xs) + b3 * (GeoY[k] - Ys) + c3 * (GeoZ[k] - Zs); if (i % 2 == 0)//数组第奇数行a11,a31等 { A[i, 0] = (a1 * f + a3 * Phox[k]) / ZBar;//a11 A[i, 1] = (b1 * f + b3 * Phox[k]) / ZBar;//a12 A[i, 2] = (c1 * f + c3 * Phox[k]) / ZBar;//a13 A[i, 3] = Phoy[k] * Math.Sin(Omega) - (Phox[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa)) / f + f * Math.Cos(Kappa)) * Math.Cos(Omega);//a14 A[i, 4] = -f * Math.Sin(Kappa) - Phox[k] * (Phox[k] * Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa)) / f;//a15 A[i, 5] = Phoy[k]; //a16 } else //偶数行 { A[i, 0] = (a2 * f + a3 * Phoy[k]) / ZBar;//a21 A[i, 1] = (b2 * f + b3 * Phoy[k]) / ZBar;//a22 A[i, 2] = (c2 * f + c3 * Phoy[k]) / ZBar;//a23 A[i, 3] = -Phox[k] * Math.Sin(Omega) - (Phoy[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa) / f) - f * Math.Sin(Kappa)) * Math.Cos(Omega);//a24 A[i, 4] = -f * Math.Cos(Kappa) - (Phoy[k] * (Phoy[k] - Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa))) / f;//a25 A[i, 5] = -Phox[k];//a26 k++; } } k = 0;//归零 Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("偏差方程系数矩阵A是:"); for (int i = 0; i < 2 * No; i++)//行 { for (int j = 0; j < 6; j++) { Console.Write("{0 ,2}\t", A[i, j]); } Console.WriteLine(); }//输出系数矩阵 constx_y = new double[2 * No, 1]; for (int i = 0; i < 2 * No; i++) { if (i % 2 == 0) //奇数行即关于x的 constx_y[i, 0] = Phox[k] - x[k]; else //偶数行 { constx_y[i, 0] = Phoy[k] - y[k]; k++; } } k = 0; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("像点观测值减近似值矩阵是: mm"); for (int i = 0; i < 2 * No; i++) { Console.WriteLine(constx_y[i, 0]); } #endregion #region 计算A的转置乘以A和A的转置和AT*Const AT = new double[6, 2 * No]; AT = jh.MatTrans(A); //计算a的转置矩阵 Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("偏差方程系数矩阵转置矩阵AT是:"); for (int i = 0; i < 6; i++)//行 { for (int j = 0; j < 2 * No; j++) { Console.Write("{0 ,2}\t", AT[i, j]); } Console.WriteLine(); } //计算AT*A double[,] MultiAT_A = new double[6, 6]; MultiAT_A = jh.MultiMat(AT, A); Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("AT*A是:"); for (int i = 0; i < 6; i++)//行 { for (int j = 0; j < 6; j++) { Console.Write("{0 ,2}\t", MultiAT_A[i, j]); } Console.WriteLine(); } RevA = jh.ReverseMatrix(MultiAT_A); //计算AT*A的逆矩阵 Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("AT*A的逆矩阵是:"); for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { Console.Write("{0}\t", RevA[i, j]); } Console.WriteLine(); } Const = new double[6, 1]; Const = jh.MultiMat(AT, constx_y); Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("AT*Const矩阵是："); for (int i = 0; i < 6; i++) { Console.WriteLine(Const[i, 0]); } #endregion #region 获得最后结果 X = new double[6, 1]; X = jh.MultiMat(RevA, Const); dX = X[0, 0]; dY = X[1, 0]; dZ = X[2, 0]; dPhi = X[3, 0]; dOmega = X[4, 0]; dKappa = X[5, 0]; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("dx\tdy\tdz\tdphi\tdomega\tdkappa\t"); Console.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t", dX, dY, dZ, dPhi, dOmega, dKappa); Xs += dX; Ys += dY; Zs += dZ; Phi += dPhi; Omega += dOmega; Kappa += dKappa; Console.WriteLine("------------------------------------------------------------------------------------------------------------------------"); Console.WriteLine("Xs\tYs\tZs\tPhi\tOmega\tKappa\t"); Console.WriteLine("{0}\t{1}\t{2}\t{3}\t{4}\t{5}\t", Xs, Ys, Zs, Phi, Omega, Kappa); #endregion times++; //手动判断是否须要进行迭代 //Console.WriteLine("是否须要下一次迭代？Y\\N 当前已迭代{0}次",times ); //yesno = Console.ReadLine(); if (Math .Abs ( dX) < 500) yesno = "N"; } Console.WriteLine("当前迭代{0}次",times ); } } }
新建一个类web
using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; namespace 单像空间后方交会解算 { public class 交会解算 { public int LineNo(int pointno)//根据控制点数量用来递归计算须要多少条线来计算比例尺 { if (pointno == 1) return 0; else return pointno - 1 + LineNo(pointno - 1); } public double GetLength(double [] X,double []Y,int i,int j)//计算两点的距离 { return Math.Sqrt((X[i] - X[j]) * (X[i]-X[j]) + (Y[i] - Y[j]) * (Y[i] - Y[j])); } public void CalRotMat(double[,] R, double Phi, double Omega, double Kappa) { R[0, 0] = Math.Cos(Phi) * Math.Cos(Kappa) - Math.Sin(Phi) * Math.Sin(Omega) * Math.Sin(Kappa); R[0, 1] = -Math.Cos(Phi) * Math.Sin(Kappa) - Math.Sin(Phi) * Math.Sin(Omega) * Math.Cos(Kappa); R[0, 2] =-Math.Sin(Phi) * Math.Cos(Omega); R[1, 0] =Math.Cos(Omega) * Math.Sin(Kappa); R[1, 1] = Math.Cos(Omega) * Math.Cos(Kappa); R[1, 2] =-Math.Sin(Omega); R[2, 0] =Math.Sin(Phi) * Math.Cos(Kappa) + Math.Cos(Phi) * Math.Sin(Omega) * Math.Sin(Kappa); R[2, 1] =-Math.Sin(Phi) * Math.Sin(Kappa) + Math.Cos(Phi) * Math.Sin(Omega) * Math.Cos(Kappa); R[2, 2] = Math.Cos(Phi) * Math.Cos(Omega); } public double[,] MatTrans(double[,] B) //实现矩阵转置 { int m, n; m = B.GetLength(0); n = B.GetLength(1); double[,] C = new double[n, m]; for (int i = 0; i < m; i++) { for (int j = 0; j < n; j++) { C[j, i] = B[i, j]; } } return C; } public double[,] ReverseMatrix(double[,] Array)//矩阵求逆 { int m = 0; int n = 0; m = Array.GetLength(0); n = Array.GetLength(1); double[,] array = new double[2 * m + 1, 2 * n + 1]; for (int k = 0; k < 2 * m + 1; k++) //初始化数组 { for (int t = 0; t < 2 * n + 1; t++) { array[k, t] = 0.00000000; } } for (int i = 0; i < m; i++) { for (int j = 0; j < n; j++) { array[i, j] = Array[i, j]; } } for (int k = 0; k < m; k++) { for (int t = n; t <= 2 * n; t++) { if ((t - k) == m) { array[k, t] = 1.0; } else { array[k, t] = 0; } } } //获得逆矩阵 for (int k = 0; k < m; k++) { if (array[k, k] != 1) { double bs = array[k, k]; array[k, k] = 1; for (int p = k + 1; p < 2 * n; p++) { array[k, p] /= bs; } } for (int q = 0; q < m; q++) { if (q != k) { double bs = array[q, k]; for (int p = 0; p < 2 * n; p++) { array[q, p] -= bs * array[k, p]; } } else { continue; } } } double[,] NI = new double[m, n]; for (int x = 0; x < m; x++) { for (int y = n; y < 2 * n; y++) { NI[x, y - n] = array[x, y]; } } return NI; } public double[,] MultiMat(double[,] A,double[,] B)//矩阵乘法A*B { int m, n,a,b; n = A.GetLength(0); //A行数 a = A.GetLength(1); //A和B的列数 m = B.GetLength(1); //B列数 //a==b则能够相乘 double[,] C = new double[n, m]; //最终相乘的结果 for (int i = 0; i < n; i++) //n是第一个矩阵的行数 { double sum = 0; for (int j = 0; j < m; j++) //m是第二个矩阵的列数 { for (int k = 0; k < a; k++) //n是第二个矩阵的行数 { sum +=A[i,k]*B[k,j]; } C[i, j] = sum; } } return C; } } }
例题是武汉大学出版社摄影测量学95页第三题。数组