/// <summary> /// 利用7参数求新坐标系的坐标 /// </summary> /// <param name="aPtSource">点的源坐标系的坐标</param> /// <param name="sep">已经知道的7参数</param> /// <param name="aPtTo">输出: 点的新坐标系的坐标</param> public static XYZCoordinate Para7Convert(XYZCoordinate aPtSource, SevenPara sep) { double k = sep.scale; double a2 = k * sep.Ax; double a3 = k * sep.Ay; double a4 = k * sep.Az; XYZCoordinate ToCoordinate = new XYZCoordinate(); ToCoordinate.X = sep.deltaX + k * aPtSource.X - a3 * aPtSource.Z + a4 * aPtSource.Y; ToCoordinate.Y = sep.deltaY + k * aPtSource.Y + a2 * aPtSource.Z - a4 * aPtSource.X; ToCoordinate.Z = sep.deltaZ + k * aPtSource.Z - a2 * aPtSource.Y + a3 * aPtSource.X; return(ToCoordinate); }
/// <summary> /// 根据3个或者3个以上的点的两套坐标系的坐标计算7参数(最小二乘法) 适用于小角度转换 bursa模型 /// </summary> /// <param name="aPtSource">已知点的源坐标系的坐标</param> /// <param name="aPtTo">已知点的新坐标系的坐标</param> /// <param name="sep">输出: 7参数</param> public static SevenPara Calc7Para(List <XYZCoordinate> aPtSource, List <XYZCoordinate> aPtTo, out double rms) { #region 给A B 矩阵赋值 double[,] arrA = new double[aPtSource.Count * 3, 7]; // 如果是4个已知点, 12 * 7矩阵 A*X=B中的矩阵A for (int i = 0; i < arrA.GetLength(0); i++) { if (i % 3 == 0) { arrA[i, 0] = 1; arrA[i, 1] = 0; arrA[i, 2] = 0; arrA[i, 3] = aPtSource[i / 3].X; arrA[i, 4] = 0; arrA[i, 5] = -aPtSource[i / 3].Z; arrA[i, 6] = aPtSource[i / 3].Y; } else if (i % 3 == 1) { arrA[i, 0] = 0; arrA[i, 1] = 1; arrA[i, 2] = 0; arrA[i, 3] = aPtSource[i / 3].Y; arrA[i, 4] = aPtSource[i / 3].Z; arrA[i, 5] = 0; arrA[i, 6] = -aPtSource[i / 3].X; } else if (i % 3 == 2) { arrA[i, 0] = 0; arrA[i, 1] = 0; arrA[i, 2] = 1; arrA[i, 3] = aPtSource[i / 3].Z; arrA[i, 4] = -aPtSource[i / 3].Y; arrA[i, 5] = aPtSource[i / 3].X; arrA[i, 6] = 0; } } double[,] arrB = new double[aPtSource.Count * 3, 1]; // A * X = B 中的矩阵B, 如果有4个点,就是 12*1矩阵 for (int i = 0; i <= arrB.GetLength(0) - 1; i++) { if (i % 3 == 0) { arrB[i, 0] = aPtTo[i / 3].X; } else if (i % 3 == 1) { arrB[i, 0] = aPtTo[i / 3].Y; } else if (i % 3 == 2) { arrB[i, 0] = aPtTo[i / 3].Z; } } #endregion Matrix mtrA = new Matrix(arrA); // A矩阵 Matrix mtrAT = Matrix.Transpose(mtrA); // A的转置 Matrix mtrB = new Matrix(arrB); // B矩阵 Matrix mtrATmulA = Matrix.Multiply(mtrAT, mtrA); // A的转置×A // 求(A的转置×A)的逆矩阵 mtrATmulA = Matrix.Inverse(mtrATmulA); // A的转置 × B Matrix mtrATmulB = Matrix.Multiply(mtrAT, mtrB); // A的转置 * B // 结果 Matrix mtrResult = Matrix.Multiply(mtrATmulA, mtrATmulB); SevenPara sep = new SevenPara(); sep.deltaX = mtrResult[0, 0]; sep.deltaY = mtrResult[1, 0]; sep.deltaZ = mtrResult[2, 0]; sep.scale = mtrResult[3, 0]; sep.Ax = mtrResult[4, 0] / sep.scale; sep.Ay = mtrResult[5, 0] / sep.scale; sep.Az = mtrResult[6, 0] / sep.scale; //计算均方根误差rms List <XYZCoordinate> pTo = new List <XYZCoordinate>(); for (int i = 0; i < aPtSource.Count; i++) { pTo.Add(Para7Convert(aPtSource[i], sep)); } rms = 0; for (int i = 0; i < aPtTo.Count; i++) { double deltaX = aPtTo[i].X - pTo[i].X; double deltaY = aPtTo[i].Y - pTo[i].Y; double oneSigma = Math.Sqrt((Math.Pow(deltaX, 2) + Math.Pow(deltaY, 2)) / 2); rms += oneSigma; } rms = rms / aPtTo.Count; return(sep); }
/// <summary> /// 12参数最小二乘解大角度7参数 /// </summary> /// <param name="cameraManager"></param> /// <param name="XcAndXwPairs"></param> /// <param name="X_ins"></param> /// <returns></returns> public static SevenPara CalBigAngle7Paras(List <XYZCoordinate> aPtSource, List <XYZCoordinate> aPtTo) { Matrix A = new Matrix(3 * aPtSource.Count, 12); Matrix B = new Matrix(3 * aPtSource.Count, 1); for (int i = 0; i < 3 * aPtSource.Count; i++) { if (i % 3 == 0) { A[i, 0] = aPtSource[i / 3].X; A[i, 1] = aPtSource[i / 3].Y; A[i, 2] = aPtSource[i / 3].Z; A[i, 3] = 0; A[i, 4] = 0; A[i, 5] = 0; A[i, 6] = 0; A[i, 7] = 0; A[i, 8] = 0; A[i, 9] = 1; A[i, 10] = 0; A[i, 11] = 0; } else if (i % 3 == 1) { A[i, 0] = 0; A[i, 1] = 0; A[i, 2] = 0; A[i, 3] = aPtSource[i / 3].X; A[i, 4] = aPtSource[i / 3].Y; A[i, 5] = aPtSource[i / 3].Z; A[i, 6] = 0; A[i, 7] = 0; A[i, 8] = 0; A[i, 9] = 0; A[i, 10] = 1; A[i, 11] = 0; } else if (i % 3 == 2) { A[i, 0] = 0; A[i, 1] = 0; A[i, 2] = 0; A[i, 3] = 0; A[i, 4] = 0; A[i, 5] = 0; A[i, 6] = aPtSource[i / 3].X; A[i, 7] = aPtSource[i / 3].Y; A[i, 8] = aPtSource[i / 3].Z; A[i, 9] = 0; A[i, 10] = 0; A[i, 11] = 1; } } for (int i = 0; i < 3 * aPtTo.Count; i++) { if (i % 3 == 0) { B[i, 0] = aPtTo[i / 3].X; } else if (i % 3 == 1) { B[i, 0] = aPtTo[i / 3].Y; } else if (i % 3 == 2) { B[i, 0] = aPtTo[i / 3].Z; } } Matrix R12 = Matrix.Inverse(Matrix.Transpose(A) * A) * (Matrix.Transpose(A) * B); //根据R_w_ins的矩阵元素可解出各个姿态角 double s = Math.Sqrt(Math.Pow(R12[0, 0], 2) + Math.Pow(R12[3, 0], 2) + Math.Pow(R12[6, 0], 2)); double cosRoll = Math.Sqrt(Math.Pow(R12[0, 0], 2) + Math.Pow(R12[3, 0], 2)) / s; double sinRoll = -R12[6, 0] / s; double roll = (sinRoll >= 0) ? (Math.Acos(cosRoll)) : (-Math.Acos(cosRoll)); double sinPitch = R12[7, 0] / Math.Cos(roll) / s; double cosPitch = R12[8, 0] / Math.Cos(roll) / s; double pitch = (sinPitch >= 0) ? (Math.Acos(cosPitch)) : (-Math.Acos(cosPitch)); double sinYaw = -R12[3, 0] / Math.Cos(roll) / s; double cosYaw = R12[0, 0] / Math.Cos(roll) / s; double yaw = (sinYaw >= 0) ? (Math.Acos(cosYaw)) : (-Math.Acos(cosYaw)); yaw = (yaw >= 0) ? yaw : (-yaw); roll = roll * 180 / Math.PI; pitch = pitch * 180 / Math.PI; yaw = yaw * 180 / Math.PI; SevenPara sevPara = new SevenPara(); sevPara.deltaX = R12[9, 0]; sevPara.deltaY = R12[10, 0]; sevPara.deltaZ = R12[11, 0]; sevPara.Ax = pitch; sevPara.Ay = roll; sevPara.Az = yaw; sevPara.scale = s; return(sevPara); }