public static XYZ CoordinateTransform(Geo.Coordinates.IXYZ old, XyzFrameConvertParam p) { double X, Y, Z; X = p.Dx + old.X * (1 + p.m) + (old.Y * p.RzRad - old.Z * p.RyRad); Y = p.Dy + old.Y * (1 + p.m) + (old.Z * p.RxRad - old.X * p.RzRad); Z = p.Dz + old.Z * (1 + p.m) + (old.X * p.RyRad - old.Y * p.RxRad); return(new XYZ(X, Y, Z)); }
/// <summary> /// 参数估计 /// </summary> /// <returns></returns> public XyzFrameConvertParam Estimate() { var obsVector = new Vector(ObsCount); //构建观测向量 for (int i = 0; i < CoordCount; i++) { int index = 3 * i; var oldXy = OldOrTobeConvertingXys[i]; var newXy = NewOrTargedXys[i]; obsVector[index + 0] = newXy.X - oldXy.X; obsVector[index + 1] = newXy.Y - oldXy.Y; obsVector[index + 2] = newXy.Z - oldXy.Z; } //构建系数阵 Matrix coeffOfParam = new Matrix(ObsCount, 7); for (int i = 0; i < CoordCount; i++) { int index = 3 * i; var oldXy = OldOrTobeConvertingXys[i]; //平移参数XYZ,0-2,单位:m coeffOfParam[index + 0, 0] = 1; coeffOfParam[index + 1, 1] = 1; coeffOfParam[index + 2, 2] = 1; //尺度参数,3,单位:ppm,求出的直接为 ppm double factor = 1.0;// 1e-6; coeffOfParam[index + 0, 3] = oldXy.X * factor; coeffOfParam[index + 1, 3] = oldXy.Y * factor; coeffOfParam[index + 2, 3] = oldXy.Z * factor; //旋转参数,4-6, 单位:s,需要转换为rad factor = 1.0;// CoordConsts.SecToRadMultiplier; coeffOfParam[index + 0, 5] = -oldXy.Z * factor; coeffOfParam[index + 0, 6] = oldXy.Y * factor; coeffOfParam[index + 1, 4] = oldXy.Z * factor; coeffOfParam[index + 1, 6] = -oldXy.X * factor; coeffOfParam[index + 2, 4] = -oldXy.Y * factor; coeffOfParam[index + 2, 5] = oldXy.X * factor; } AdjustObsMatrix obsMatrix = new AdjustObsMatrix(); obsMatrix.SetCoefficient(coeffOfParam).SetObservation(obsVector); var adjuster = new ParamAdjuster(); this.ResultMatrix = adjuster.Run(obsMatrix); var ested = ResultMatrix.Estimated; var result = new XyzFrameConvertParam( new XYZ(ested[0], ested[1], ested[2]), //meter new XYZ(ested[4], ested[5], ested[6]) * CoordConsts.RadToSecMultiplier, //second ested[3] * 1e6 //ppm ); //输出精度信息 var RMSVector = ResultMatrix.StdOfEstimatedParam; StringBuilder sb = new StringBuilder(); sb.AppendLine("单位权中误差:" + this.ResultMatrix.StdDev.ToString("G6")); sb.Append("平移参数(m):" + result.Offset); sb.AppendLine(", 标准差:" + RMSVector[0].ToString("G6") + ", " + RMSVector[1].ToString("G6") + ", " + RMSVector[2].ToString("G6")); sb.Append("旋转参数(s):" + result.RotationAngleSecond); sb.AppendLine(", 标准差:" + RMSVector[4].ToString("G6") + ", " + RMSVector[5].ToString("G6") + ", " + RMSVector[6].ToString("G6")); sb.Append("尺度参数(ppm):" + result.ScaleFactorPpm.ToString("G6")); sb.AppendLine(", 标准差:" + RMSVector[3].ToString("G6")); log.Info(sb.ToString()); return(result); }
/// <summary> /// 构造函数 /// </summary> /// <param name="xyConvertParam"></param> public XyzFrameConverter(XyzFrameConvertParam xyConvertParam) { this.ConvertParam = xyConvertParam; }