public void Seven3d(List <double> B1, List <double> L1,
out List <double> B2, out List <double> L2,
SevenPara sev, Ellipsoid ell1, Ellipsoid ell2)
{
B2 = new List <double>();
L2 = new List <double>();
if (B1 == null || L1 == null || sev == null || ell1 == null || ell2 == null)
{
return;
}
if (B1.Count != L1.Count)
{
throw new ArgumentException("Number of coordinates is not uniform");
}
double dB, dL, dH;
for (int i = 0; i < B1.Count; i++)
{
Seven2d(out dB, out dL, B1[i], L1[i], sev, ell1, ell2);
B2.Add(B1[i] + dB);
L2.Add(B1[i] + dL);
}
}
//[DllImport("GeoFunC.dll", CallingConvention = CallingConvention.Cdecl)]
//public static extern int Sum(int a, int b);
public static void Main()
{
//int c = Sum(100, 200);
double X, Y, Z;
double b = 35d;
double l = 112d;
double h = 1000d;
Coordinate.BLH2XYZ(b * Angle.D2R, l * Angle.D2R, h, out X, out Y, out Z, Ellipsoid.ELLIP_XIAN80);
Console.WriteLine(string.Format("{0} {1} {2} {3} {4} {5}", b, l, h, X, Y, Z));
Coordinate.XYZ2BLH(X, Y, Z, out b, out l, out h, Ellipsoid.ELLIP_XIAN80);
b *= Angle.R2D;
l *= Angle.R2D;
Console.WriteLine(string.Format("{0} {1} {2} {3} {4} {5}", b, l, h, X, Y, Z));
double dB, dL, dH;
SevenPara para = new SevenPara();
para.XRot = -1;
para.YRot = -1;
para.ZRot = -1;
para.M = 1;
para.XOff = 100;
para.YOff = 100;
para.ZOff = 100;
Trans trans = new Trans();
b = 35;
l = 112;
h = 1000;
trans.Seven3d(out dB, out dL, out dH, b * Angle.D2R, l * Angle.D2R, h, para, Ellipsoid.ELLIP_XIAN80, Ellipsoid.ELLIP_CGCS2000);
//double b1 = Angle.Arc2DMS(b * Angle.D2R + dB);
//double l1 = Angle.Arc2DMS(l * Angle.D2R + dL);
double b1 = (b * Angle.D2R + dB) * Angle.R2D;
double l1 = (l * Angle.D2R + dL) * Angle.R2D;
double h1 = h + dH;
Console.WriteLine("三维七参数结果 {0} {1} {2}", b1, l1, h1);
trans.Seven2d(out dB, out dL, b * Angle.D2R, l * Angle.D2R, para, Ellipsoid.ELLIP_XIAN80, Ellipsoid.ELLIP_CGCS2000);
double b2 = (b * Angle.D2R + dB) * Angle.R2D;
double l2 = (l * Angle.D2R + dL) * Angle.R2D;
Console.WriteLine("三维七参数结果 {0} {1} {2}", b2, l2, h);
Console.ReadKey();
}
/// <summary>
/// 二维七参数，高程设置为固定值800
/// </summary>
/// <param name="dB"></param>
/// <param name="dL"></param>
/// <param name="b"></param>
/// <param name="l"></param>
/// <param name="para"></param>
/// <param name="ell1"></param>
/// <param name="ell2"></param>
public void Seven2d(out double dB, out double dL, double b, double l, SevenPara para, Ellipsoid ell1, Ellipsoid ell2)
{
dB = 0d;
dL = 0d;
double dH = 0d;
//double M = ell1.M(b);
//double N = ell1.N(b);
//dL = -Math.Sin(l) / N / Math.Cos(b) * para.XOff + Math.Cos(l) / N / Math.Cos(b) * para.YOff +
// Math.Tan(b) * Math.Cos(l) * para.XRot * Angle.S2R + Math.Tan(b) * Math.Sin(l) * para.YRot * Angle.S2R - para.ZRot * Angle.S2R;
//dB = -Math.Sin(b) * Math.Cos(l) / M - Math.Sin(b) * Math.Sin(l) / M + Math.Cos(b) / M -
// Math.Sin(l) * para.XRot * Angle.S2R + Math.Cos(l) * para.YRot * Angle.S2R -
// -N / M * ell1.E1 * Math.Sin(b) * Math.Cos(b) * para.M * 1e-6 +
// N / M / ell1.A * ell1.E1 * Math.Sin(b) * Math.Cos(b) * (ell2.A - ell1.A) + (2 - ell1.E1 * Math.Sin(b) * Math.Sin(b)) / (1 - ell1.Alpha) * Math.Sin(b) * Math.Cos(b) * (ell2.Alpha - ell2.Alpha);
Seven3d(out dB, out dL, out dH, b, l, 800, para, ell1, ell2);
}
//// 经测试，精度无法达到要求
//public void Seven3d(out double dB, out double dL, out double dH, double b, double l, double h, SevenPara para, Ellipsoid ell1, Ellipsoid ell2)
//{
// dB = 0d;
// dL = 0d;
// dH = 0d;
// double N = ell1.N(b);
// double M = ell1.M(b);
// //double p = 180 * 3600 / Angle.PI;
// double p = 1d;
// dL = p * -Math.Sin(l) / (N + h) / Math.Cos(b) * para.XOff + p * Math.Cos(l) / (N + h) / Math.Cos(b) * para.YOff +
// (N * (1 - ell1.E1) + h) / (N + h) * Math.Tan(b) * Math.Cos(l) * para.XRot * Angle.S2R + (N * (1 - ell1.E1) + h) / (N + h) * Math.Tan(b) * Math.Sin(l) * para.YRot * Angle.S2R - para.ZRot * Angle.S2R;
// dB = -p * Math.Sin(b) * Math.Cos(l) / (M + h) * para.XOff - p * Math.Sin(b) * Math.Sin(l) / (M + h) * para.YOff + Math.Cos(b) / (M + h) * para.ZOff -
// ((N + h) - N * ell1.E1 * Math.Pow(Math.Sin(b), 2)) / (M + h) * Math.Sin(l) * para.XRot * Angle.S2R + ((N + h) - N * ell1.E1 * Math.Pow(Math.Sin(b), 2)) / (M + h) * Math.Cos(l) * para.YRot * Angle.S2R -
// p * N / M * ell1.E1 * Math.Sin(b) * Math.Cos(b) * para.M * 1e-6 +
// p * N / M / ell1.A * ell1.E1 * Math.Sin(b) * Math.Cos(b) * (ell2.A - ell1.A) + p * (2 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) / (1 - ell1.Alpha) * Math.Sin(b) * Math.Cos(b) * (ell2.Alpha - ell1.Alpha);
// dH = Math.Cos(b) * Math.Cos(l) * para.XOff + Math.Cos(b) * Math.Sin(l) * para.YOff + Math.Sin(b) * para.ZOff -
// N * ell1.E1 * Math.Sin(b) * Math.Cos(b) * Math.Sin(l) * para.XRot * Angle.S2R + N * ell1.E1 * Math.Sin(b) * Math.Cos(b) * Math.Cos(l) * para.YRot * Angle.S2R +
// ((N + h) - N * ell1.E1 * Math.Pow(Math.Sin(b), 2)) * para.M * 1e-6 -
// N / ell1.A * (1 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) * (ell2.A - ell1.A) + M / (1 - ell1.Alpha) * (1 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) * Math.Pow(Math.Sin(b), 2) * (ell2.Alpha - ell1.Alpha);
// //dL = p * -Math.Sin(l) / (N + h) / Math.Cos(b) * para.XOff + p * Math.Cos(l) / (N + h) / Math.Cos(b) * para.YOff +
// // Math.Tan(b) * Math.Cos(l) * para.XRot * Angle.S2R + Math.Tan(b) * Math.Sin(l) * para.YRot * Angle.S2R - para.ZRot * Angle.S2R;
// //dB = -p * Math.Sin(b) * Math.Cos(l) / (M + h) * para.XOff - p * Math.Sin(b) * Math.Sin(l) / (M + h) * para.YOff + Math.Cos(b) / (M + h) * para.ZOff -
// // Math.Sin(l) * para.XRot * Angle.S2R + Math.Cos(l) * para.YRot * Angle.S2R -
// // p * N / (M+h) * ell1.E1 * Math.Sin(b) * Math.Cos(b) * para.M * 1e-6 +
// // p * N / (M+h) / ell1.A * ell1.E1 * Math.Sin(b) * Math.Cos(b) * (ell2.A - ell1.A) + p * M*(2 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) /(M+h)/ (1 - ell1.Alpha) * Math.Sin(b) * Math.Cos(b) * (ell2.Alpha - ell1.Alpha);
// //dH = Math.Cos(b) * Math.Cos(l) * para.XOff + Math.Cos(b) * Math.Sin(l) * para.YOff + Math.Sin(b) * para.ZOff -
// // N * ell1.E1 * Math.Sin(b) * Math.Cos(b) * Math.Sin(l) * para.XRot * Angle.S2R + N * ell1.E1 * Math.Sin(b) * Math.Cos(b) * Math.Cos(l) * para.YRot * Angle.S2R +
// // ((N + h) - N * ell1.E1 * Math.Pow(Math.Sin(b), 2)) * para.M * 1e-6 -
// // N / ell1.A * (1 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) * (ell2.A - ell1.A) + M / (1 - ell1.Alpha) * (1 - ell1.E1 * Math.Pow(Math.Sin(b), 2)) * Math.Pow(Math.Sin(b), 2) * (ell2.Alpha - ell1.Alpha);
//}
/// <summary>
/// 七参数转换
/// </summary>
/// <param name="dB">改正量(弧度)</param>
/// <param name="dL">改正量(弧度)</param>
/// <param name="dH">改正量(米)</param>
/// <param name="B">纬度(弧度)</param>
/// <param name="L">精度(弧度)</param>
/// <param name="H">大地高(米)</param>
/// <param name="para">七参数</param>
/// <param name="ell1">椭球1</param>
/// <param name="ell2">椭球2</param>
public void Seven3d(out double dB, out double dL, out double dH,
double B, double L, double H,
SevenPara para, Ellipsoid ell1, Ellipsoid ell2)
{
dB = 0d; dL = 0d; dH = 0d;
double X, Y, Z, BB, LL, HH;
Coordinate.BLH2XYZ(B, L, H, out X, out Y, out Z, ell1);
double XX = (1 + para.M * 1e-6) * (X + Y * para.ZRot * Angle.S2R - Z * para.YRot * Angle.S2R) + para.XOff;
double YY = (1 + para.M * 1e-6) * (-X * para.ZRot * Angle.S2R + Y + Z * para.XRot * Angle.S2R) + para.YOff;
double ZZ = (1 + para.M * 1e-6) * (X * para.YRot * Angle.S2R - Y * para.XRot * Angle.S2R + Z) + para.ZOff;
Coordinate.XYZ2BLH(XX, YY, ZZ, out BB, out LL, out HH, ell2);
dB = BB - B;
dL = LL - L;
dH = HH - H;
}
/// <summary>
///
/// </summary>
/// <param name="dB">改正量(弧度)</param>
/// <param name="dL">改正量(弧度)</param>
/// <param name="dH">改正量(米)</param>
/// <param name="B">纬度(弧度)</param>
/// <param name="L">精度(弧度)</param>
/// <param name="H">大地高(米)</param>>
/// <param name="xoff">米</param>
/// <param name="yoff">米</param>
/// <param name="zoff">米</param>
/// <param name="xrot">角度秒</param>
/// <param name="yrot">角度秒</param>
/// <param name="zrot">角度秒</param>
/// <param name="m">ppm</param>
/// <param name="a1">米</param>
/// <param name="f1">扁率倒数</param>
/// <param name="a2">米</param>
/// <param name="f2">扁率倒数</param>
public void Seven3d(out double dB, out double dL, out double dH,
double B, double L, double H,
double xoff, double yoff, double zoff, double xrot, double yrot, double zrot, double m,
double a1, double f1, double a2, double f2)
{
SevenPara para = new SevenPara
{
XOff = xoff,
YOff = yoff,
ZOff = zoff,
XRot = xrot,
YRot = yrot,
ZRot = zrot,
M = m,
};
Ellipsoid ell1 = new Ellipsoid(a1, f1);
Ellipsoid ell2 = new Ellipsoid(a2, f2);
Seven3d(out dB, out dL, out dH, B, L, H, para, ell1, ell2);
}
/// <summary>
/// 1七参数转换
/// </summary>
/// <param name="X1"></param>
/// <param name="Y1"></param>
/// <param name="Z1"></param>
/// <param name="sev"></param>
/// <param name="X2"></param>
/// <param name="Y2"></param>
/// <param name="Z2"></param>
public void Seven3d(double X1, double Y1, double Z1, SevenPara sev,
out double X2, out double Y2, out double Z2)
{
Seven3d(X1, Y1, Z1, sev.XOff, sev.YOff, sev.ZOff, sev.XRot, sev.YRot, sev.ZRot, sev.M,
out X2, out Y2, out Z2);
}
/// <summary>
/// 计算七参数(椭球1到椭球2)
/// </summary>
/// <param name="b1">纬度(弧度)</param>
/// <param name="l1">经度(弧度)</param>
/// <param name="h1">大地高</param>
/// <param name="b2">纬度(弧度)</param>
/// <param name="l2">经度(弧度)</param>
/// <param name="h2">大地高</param>
/// <param name="ell1">椭球1</param>
/// <param name="ell2">椭球2</param>
/// <param name="maxRes">最大残差(m)</param>
/// <returns></returns>
public static SevenPara CalParaIter(List <double> b1, List <double> l1, List <double> h1,
List <double> b2, List <double> l2, List <double> h2,
Ellipsoid ell1, Ellipsoid ell2)
{
if (b1 is null || l1 is null || b2 is null || l2 is null || ell1 is null || ell2 is null)
{
throw new ArgumentNullException("公共点不能为空");
}
if ((h1 is null) || (h2 is null))
{
throw new ArgumentNullException("至少需要一个坐标系的高程值");
}
if (h1 is null)
{
h1 = h2;
}
if (h2 is null)
{
h2 = h1;
}
var leng = new List <int> {
b1.Count, l1.Count, h1.Count, b2.Count, l2.Count, h2.Count
};
var pointNum = leng.Min();
if (pointNum < 3)
{
throw new ArgumentException("公共点不足");
}
if (pointNum == 3)
{
return(CalPara(b1, l1, h1, b2, l2, h2, ell1, ell2));
}
//// 最终选取的公共点坐标
List <double> b1Com = new List <double>();
List <double> l1Com = new List <double>();
List <double> b2Com = new List <double>();
List <double> l2Com = new List <double>();
List <double> h1Com = new List <double>();
List <double> h2Com = new List <double>();
SevenPara sev = new SevenPara();
Trans trans = new Trans();
var indexes = Enumerable.Range(0, pointNum).ToList();
//// 最大残差
var maxDiff = 1d;
var maxInde = 0;
while (indexes.Count > 3)
{
b1Com.Clear();
l1Com.Clear();
h1Com.Clear();
b2Com.Clear();
l2Com.Clear();
h2Com.Clear();
foreach (var index in indexes)
{
b1Com.Add(b1[index]);
l1Com.Add(l1[index]);
h1Com.Add(h1[index]);
b2Com.Add(b2[index]);
l2Com.Add(l2[index]);
h2Com.Add(h2[index]);
}
sev = CalPara(b1Com, l1Com, h1Com, b2Com, l2Com, h2Com, ell1, ell2);
//// 转换后的blh
List <double> b2Trans, l2Trans, h2Trans;
trans.Seven3d(b1Com, l1Com, h1Com, out b2Trans, out l2Trans, out h2Trans, sev, ell1, ell2);
//// 转换后的xyz
List <double> X2Trans, Y2Trans, Z2Trans;
Coordinate.BLH2XYZ(b2Trans, l2Trans, h2Trans, out X2Trans, out Y2Trans, out Z2Trans, Ellipsoid.ELLIP_CGCS2000);
//// 原始的xyz
List <double> X2Tem, Y2Tem, Z2Tem;
Coordinate.BLH2XYZ(b2Com, l2Com, h2Com, out X2Tem, out Y2Tem, out Z2Tem, Ellipsoid.ELLIP_CGCS2000);
///// 计算残差(单位米)
var diff = (from i in Enumerable.Range(0, b2Com.Count)
select new
{
Index = i,
Diff = Math.Sqrt(Math.Pow(X2Tem[i] - X2Trans[i], 2) + Math.Pow(Y2Trans[i] - Y2Tem[i], 2) + Math.Pow(Z2Trans[i] - Z2Tem[i], 2))
}).ToList();
double sigma = Math.Sqrt(diff.Sum(d => d.Diff * d.Diff) / indexes.Count);
var max = diff.OrderByDescending(d => d.Diff).First();
maxInde = max.Index;
maxDiff = max.Diff;
if (maxDiff > sigma * 3d)
{
indexes.Remove(maxInde);
}
else
{
break;
}
}
return(sev);
}
public static SevenPara CalParaYao(List <double> b1, List <double> l1, List <double> h1,
List <double> b2, List <double> l2, List <double> h2,
Ellipsoid ell1, Ellipsoid ell2)
{
double[] p = new double[49];
double[] x = new double[7];
double[] l = new double[3000];
double[] w = new double[7];
double[] v = new double[3000];
char[] dh = new char[20];
double targetx = 0, targety = 0, targetz = 0;
double sourcex = 0, sourcey = 0d, sourcez = 0;
double targetb = 0d, targetl = 0d, targeth = 0d;
double sourceb = 0d, sourcel = 0d, sourceh = 0d;
double[,] a = new double[1500, 7];
double pvv, m0, targeta, targetf, sourcea, sourcef, targete, sourcee;;
double[] m = new double[7];
int i, j, n, k;
n = l1.Count;
int num = 7;
x[0] = x[1] = x[2] = x[3] = x[4] = x[5] = x[6] = 0;
m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = 0;
sourcea = ell1.A;
targeta = ell2.A;
sourcee = ell1.E1;
targete = ell2.E1;
int[] pos = { 3, 4, 5, 6 };
double P0 = 360d * 180d / Angle.PI;
for (i = 0; i < l1.Count; i++)
{
//sourceb = Angle.gd(b1[i]);
//sourcel = Angle.gd(l1[i]);
sourceb = b1[i];
sourcel = l1[i];
sourceh = h1[i];
//targetb = Angle.gd(b2[i]);
//targetl = Angle.gd(l2[i]);
targetb = b2[i];
targetl = l2[i];
targeth = h2[i];
Coordinate.compute_xyz(sourceb, sourcel, sourceh, sourcea, sourcee, ref sourcex, ref sourcey, ref sourcez);
Coordinate.compute_xyz(targetb, targetl, targeth, targeta, targete, ref targetx, ref targety, ref targetz);
a[3 * i + 0, 0] = 1;
a[3 * i + 0, 1] = 0;
a[3 * i + 0, 2] = 0;
a[3 * i + 0, pos[0]] = sourcex / 1000000.0;
a[3 * i + 0, pos[1]] = 0;
a[3 * i + 0, pos[2]] = -sourcez / P0;
a[3 * i + 0, pos[3]] = sourcey / P0;
a[3 * i + 1, 0] = 0;
a[3 * i + 1, 1] = 1;
a[3 * i + 1, 2] = 0;
a[3 * i + 1, pos[0]] = sourcey / 1000000.0;
a[3 * i + 1, pos[1]] = sourcez / P0;
a[3 * i + 1, pos[2]] = 0;
a[3 * i + 1, pos[3]] = -sourcex / P0;
a[3 * i + 2, 0] = 0;
a[3 * i + 2, 1] = 0;
a[3 * i + 2, 2] = 1;
a[3 * i + 2, pos[0]] = sourcez / 1000000.0;
a[3 * i + 2, pos[1]] = -sourcey / P0;
a[3 * i + 2, pos[2]] = sourcex / P0;
a[3 * i + 2, pos[3]] = 0;
l[3 * i + 0] = -targetx + sourcex;
l[3 * i + 1] = -targety + sourcey;
l[3 * i + 2] = -targetz + sourcez;
}
for (i = 0; i < num; i++)
{
for (j = i; j < num; j++)
{
p[i * num + j] = 0;
for (k = 0; k < 3 * n; k++)
{
p[i * num + j] += a[k, j] * a[k, i];
}
p[j * num + i] = p[i * num + j];
}
}
for (i = 0; i < num; i++)
{
w[i] = 0.0;
for (j = 0; j < 3 * n; j++)
{
w[i] += a[j, i] * l[j];
}
}
MatrixHelper.Inv(ref p, num);
for (i = 0; i < num; i++)
{
x[i] = 0;
for (j = 0; j < num; j++)
{
x[i] += -p[i * num + j] * w[j];
}
}
pvv = 0;
for (i = 0; i < 3 * n; i++)
{
v[i] = l[i];
for (j = 0; j < num; j++)
{
v[i] += a[i, j] * x[j];
}
pvv += v[i] * v[i];
}
m0 = Math.Sqrt(pvv / (3 * n - num + 0.000000001));
SevenPara sev = new SevenPara
{
XOff = x[0],
YOff = x[1],
ZOff = x[2],
M = x[3],
XRot = x[4],
YRot = x[5],
ZRot = x[6],
};
return(sev);
}
