//计算旋转矩阵 internal static double[,] GetRotationMatrix(double[,] RotationAngles) { //RotationAngles[3, 1] = [φ; ω; к] double phi = RotationAngles[0, 0]; double omega = RotationAngles[1, 0]; double kappa = RotationAngles[2, 0]; double[,] Rphi = { { Math.Cos(phi), 0, -Math.Sin(phi) }, { 0, 1, 0 }, { Math.Sin(phi), 0, Math.Cos(phi) } }; double[,] Romega = { { 1, 0, 0 }, { 0, Math.Cos(omega), -Math.Sin(omega) }, { 0, Math.Sin(omega), Math.Cos(omega) } }; double[,] Rkappa = { { Math.Cos(kappa), -Math.Sin(kappa), 0 }, { Math.Sin(kappa), Math.Cos(kappa), 0 }, { 0, 0, 1 } }; return(MatrixComputation.Multiply(MatrixComputation.Multiply(Rphi, Romega), Rkappa)); }
//求Q internal static double[,] GetQ(double[,] A, double[,] X, double[,] L, int PointCount) { /* * 平差课本 * (P130,7-3-2) 单位权中误差σ0 = √((V^(T)*PV)/r) * P = I * (P112,7-1-4)V = AX - L * r = n - t * (P132,表7-9) Qxx = NBB^(-1) = (A^(T)*PA)^(-1) * (P133,7-3-15) σx = σ0 * √Qxx */ double m0 = Math.Sqrt((MatrixComputation.Multiply(MatrixComputation.Transpose(MatrixComputation.Subtract(MatrixComputation.Multiply(A, X), L)), MatrixComputation.Subtract(MatrixComputation.Multiply(A, X), L))[0, 0] / (2 * PointCount - 6))); double[,] Q = MatrixComputation.Inverse(MatrixComputation.Multiply(MatrixComputation.Transpose(A), A)); for (int i = 0; i < Q.GetLength(0); i++) { for (int j = 0; j < Q.GetLength(1); j++) { Q[i, j] = Math.Sqrt(Q[i, j]) * m0; } } return(Q); }
/// <summary> /// 全选主元高斯消去法求解矩阵的秩 /// </summary> /// <returns>矩阵的秩</returns> public int Rank() { return(MatrixComputation.Rank(elements)); }
/// <summary> /// 矩阵的转置 /// </summary> /// <returns>返回转置后的矩阵</returns> public Matrix Transpose() { return(MatrixComputation.Transpose(elements)); }
/// <summary> /// 全选主元高斯-约当法求逆矩阵 /// </summary> /// <returns>逆矩阵</returns> public Matrix Inverse() { return(MatrixComputation.Inverse(elements)); }
public static Matrix operator /(Matrix A, Matrix B) { return(MatrixComputation.Divide(A.elements, B.elements)); }
public static Matrix operator *(Matrix A, Matrix B) { return(MatrixComputation.Multiply(A.elements, B.elements)); }
public static Matrix operator -(Matrix A, Matrix B) { return(MatrixComputation.Subtract(A.elements, B.elements)); }
public static Matrix operator +(Matrix A, Matrix B) { return(MatrixComputation.Add(A.elements, B.elements)); }
public static Matrix operator -(Matrix M) { return(MatrixComputation.UnaryMinus(M.elements)); }
//求改正数 internal static double[,] GetX(double[,] A, double[,] L) { //X = (A^(T)A)^(-1)*A^(T)*L return(MatrixComputation.Multiply(MatrixComputation.Multiply(MatrixComputation.Inverse((MatrixComputation.Multiply(MatrixComputation.Transpose(A), A))), MatrixComputation.Transpose(A)), L)); }
internal static void CalcMain(List <ResectionPoint> PhotoPoint, List <ResectionPoint> GroundPoint, double m, double f, double Limit, int ReTimeLimit, ref double[,] Result, ref int ReTime, out double[,] RM, out double[,] Q) { ResectionPoint CenterPoint = new ResectionPoint(); //摄影中心点 //逐点计算Xs、Ys、Zs foreach (ResectionPoint Point in GroundPoint) { Result[0, 0] += Point.X; Result[1, 0] += Point.Y; Result[2, 0] += Point.Z; } Result[0, 0] /= GroundPoint.Count; Result[1, 0] /= GroundPoint.Count; Result[2, 0] = m * f + Result[2, 0] / GroundPoint.Count; //根据点的个数创建矩阵 double[,] A = new double[2 * PhotoPoint.Count, 6]; //A矩阵,每个点2*6 double[,] L = new double[2 * PhotoPoint.Count, 1]; //L矩阵,每个点2*1 double[,] RotationAngles = new double[3, 1]; //角元素矩阵,3*1 double[,] X = { { 0 }, { 0 }, { 0 }, { 0 }, { 0 }, { 0 } }; //改正数矩阵,6*1 do { //检查迭代次数 if (ReTime > ReTimeLimit) { throw new Exception("迭代次数超出限制!"); } //每次迭代开始对本次计算的外方位元素重新赋值 CenterPoint.X = Result[0, 0]; CenterPoint.Y = Result[1, 0]; CenterPoint.Z = Result[2, 0]; Array.Copy(Result, 3, RotationAngles, 0, 3); //通过三个角元素建立旋转矩阵 RM = GetRotationMatrix(RotationAngles); //逐点计算并构造A,L int CurrentRow = 0; //当前行计数器 for (int i = 0; i < PhotoPoint.Count; i++) { ResectionPoint AvgPoint = GetAvg(GroundPoint[i], CenterPoint, RM); //合并重复项,简化变量 //逐点获取A、L double[,] ATmp = GetA(AvgPoint, RotationAngles, RM, f); double[,] LTmp = GetL(PhotoPoint[i], AvgPoint, f); //拷贝暂存数组 Array.Copy(ATmp, 0, A, A.GetLength(1) * CurrentRow, ATmp.GetLength(0) * ATmp.GetLength(1)); Array.Copy(LTmp, 0, L, L.GetLength(1) * CurrentRow, LTmp.GetLength(0) * LTmp.GetLength(1)); CurrentRow += 2; //单个点赋值完毕,当前行数+2 } X = GetX(A, L); //计算改正量 Result = MatrixComputation.Add(Result, X); //外方位元素矩阵加改正量 ReTime++; //迭代次数+1 } while (Math.Abs(X[3, 0]) > Limit || Math.Abs(X[4, 0]) > Limit || Math.Abs(X[5, 0]) > Limit); //当改正量大于阈值时继续迭代 Q = GetQ(A, X, L, GroundPoint.Count); //计算完毕后的误差分析 }