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;
}
