private Geodetic_Point Direct(_3D_Point XYZ)
            {
                double e_2  = ep.data.e_2,
                       e2_2 = ep.data.e2_2,
                       a    = ep.data.a,
                       b    = ep.data.b;
                double r    = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y)),
                       u0   = Math.Atan(XYZ.Z / r * Math.Sqrt(1 + e2_2)),
                       tgB  = (XYZ.Z + b * e2_2 * Math.Sin(u0) * Math.Sin(u0)) /
                              (r - a * e_2 * Math.Cos(u0) * Math.Cos(u0)),
                       B      = Math.Atan(XYZ.Z / r + a * e_2 * tgB / r / Math.Sqrt(1 + (1 - e_2) * tgB * tgB));
                _3D_Point blh = new _3D_Point();

                //blh.X = Angle.ChangeToAngle(B).ChangeToDouble();
                blh.X = B;

                //var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
                //blh.Y = l.ChangeToDouble();
                blh.Y = Auxiliary.Get_L(XYZ.X, XYZ.Y);

                double W  = Auxiliary.GetW(e_2, B),
                       N  = a / W;
                double h1 = XYZ.Z - N * (1 - e_2) * Math.Sin(B),
                       h2 = r - N * Math.Cos(B);
                double h  = Math.Sqrt(h1 * h1 + h2 * h2);

                if (h1 / Math.Sin(B) < 0 || h2 / Math.Cos(B) < 0)
                {
                    h = -h;
                }
                blh.Z = h;
                return(new Geodetic_Point(false, blh.X, blh.Y, blh.Z));
            }
Esempio n. 2
0
            /// <summary>
            /// 获得点位精度
            /// </summary>
            /// <param name="distance">摄影距离,单位与物方坐标相同</param>
            /// <returns></returns>
            public string GetAccuracy(double distance)
            {
                if (hasKnown)
                {
                    List <double> rs = new List <double>();

                    List <_3D_Point> ddxyz = new List <_3D_Point>();

                    for (int i = 0; i < PassOList.Count; i++)
                    {
                        _3D_Point dxyz = PassOList[i].pos - KnownOList[i].pos;
                        ddxyz.Add(dxyz);
                        double r = _3D_Point.Get_Norm(dxyz, new _3D_Point());
                        rs.Add(r);
                    }
                    double sum = 0;
                    rs.ForEach(r => sum += r * r);
                    double a = Math.Sqrt(sum / rs.Count);
                    return("dr=" + a + ",点位精度:1/" + (distance / a).ToFormatString(0));
                }
                else
                {
                    return(string.Empty);
                }
            }
            private Geodetic_Point Iteration(_3D_Point XYZ)
            {
                _3D_Point blh = new _3D_Point();

                //var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
                blh.Y = Auxiliary.Get_L(XYZ.X, XYZ.Y);
                double dis = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
                double t0  = XYZ.Z / dis,
                       p   = ep.data.c * ep.data.e_2 / dis,
                       k   = 1 + ep.data.e2_2,
                       t1  = t0,
                       t2  = t0;

                do
                {
                    t1 = t2;
                    t2 = t0 + p * t1 / Math.Sqrt(k + t1 * t1);
                }while (Math.Abs(Angle.ChangeToAngle(t2 - t1).Miao) > 0.0001);
                blh.X = Angle.ChangeToAngle(t2 = Math.Atan(t2)).ChangeToRad();
                double W  = Auxiliary.GetW(ep.data.e_2, t2),
                       N  = ep.data.a / W;
                double h1 = XYZ.Z - N * (1 - ep.data.e_2) * Math.Sin(t2),
                       h2 = dis - N * Math.Cos(t2);
                double h  = Math.Sqrt(h1 * h1 + h2 * h2);

                if (h1 / Math.Sin(t2) < 0 || h2 / Math.Cos(t2) < 0)
                {
                    h = -h;
                }
                blh.Z = h;
                return(new Geodetic_Point(false, blh.X, blh.Y, blh.Z));
            }
 /// <summary>
 /// 空间直角坐标到大地坐标
 /// </summary>
 /// <param name="XYZ">直接坐标</param>
 /// <param name="mode">计算模式,默认为0(迭代法),其他为直接法</param>
 /// <returns>大地坐标</returns>
 public Geodetic_Point Xyz2Blh(_3D_Point XYZ, int mode = 0)
 {
     if (mode == 0)
     {
         return(Iteration(XYZ));
     }
     else
     {
         return(Direct(XYZ));
     }
 }
            /// <summary>
            /// 大地坐标到空间直角坐标
            /// </summary>
            /// <param name="BLH">大地坐标</param>
            /// <returns>直角坐标</returns>
            public _3D_Point Blh2Xyz(Geodetic_Point BLH)
            {
                double    W = Auxiliary.GetW(ep.data.e_2, BLH.Br);
                double    V = Auxiliary.GetV(ep.data.e2_2, BLH.Br);
                double    N = ep.data.a / W;
                _3D_Point p = new _3D_Point();

                p.X = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Cos(BLH.Lr);
                p.Y = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Sin(BLH.Lr);
                p.Z = (N * (1 - ep.data.e_2) + BLH.H) * Math.Sin(BLH.Br);
                return(p);
            }
Esempio n. 6
0
        public void ReadData(string path1, string path2)//读取像方数据文件,控制点,加密点
        {
            //StreamReader sr = new StreamReader(path1);
            var s = File.ReadAllLines(path1, Encoding.Default);

            oCount = s.Length;
            var ss = Regex.Split(s[0], " +");

            Count = (ss.Length - 4) / 2;
            for (int i = 0; i < Count; i++)
            {
                oIPoints.Add(new List <_2D_Point>());
                iPoints.Add(new List <_2D_Point>());
            }
            for (int i = 0; i < oCount; i++)
            {
                ss = Regex.Split(s[i], " +");
                pNames.Add(ss[0]);
                for (int j = 0; j < Count; j++)
                {
                    _2D_Point p = new _2D_Point();
                    p.X = double.Parse(ss[1 + 2 * j]);
                    p.Y = double.Parse(ss[2 + 2 * j]);
                    oIPoints[j].Add(p);
                }
                _3D_Point p3 = new _3D_Point();
                p3.X = double.Parse(ss[1 + 2 * Count]);
                p3.Y = double.Parse(ss[2 + 2 * Count]);
                p3.Z = double.Parse(ss[3 + 2 * Count]);
                oPoints.Add(p3);
            }

            s      = File.ReadAllLines(path2, Encoding.Default);
            pCount = s.Length;
            for (int i = 0; i < pCount; i++)
            {
                ss = Regex.Split(s[i], " +");
                pINames.Add(ss[0]);
                for (int j = 0; j < Count; j++)
                {
                    _2D_Point p = new _2D_Point();
                    p.X = double.Parse(ss[1 + 2 * j]);
                    p.Y = double.Parse(ss[2 + 2 * j]);
                    iPoints[j].Add(p);
                }
            }
        }
Esempio n. 7
0
 /// <summary>
 /// 默认构造函数(点名为空字符,坐标为原点)
 /// </summary>
 public ObjectData()
 {
     pos = new _3D_Point();
 }
Esempio n. 8
0
 /// <summary>
 /// 构造函数
 /// </summary>
 /// <param name="name">点名</param>
 /// <param name="point">坐标</param>
 public ObjectData(string name, _3D_Point point)
 {
     Name = name;
     pos  = point;
 }
Esempio n. 9
0
 /// <summary>
 /// 默认构造函数(点名为空字符,坐标为原点)
 /// </summary>
 public ObjectData()
 {
     pos = new _3D_Point();
 }
Esempio n. 10
0
 /// <summary>
 /// 构造函数
 /// </summary>
 /// <param name="name">点名</param>
 /// <param name="point">坐标</param>
 public ObjectData(string name, _3D_Point point)
 {
     Name = name;
     pos = point;
 }
Esempio n. 11
0
            /// <summary>
            /// 前方交会
            /// </summary>
            /// <returns></returns>
            public List <OData> ForwardForcus()
            {
                List <List <_3D_Point> > points = new List <List <_3D_Point> >();

                for (int i = 0; i < outE.Count - 1; i++)
                {
                    points.Add(new List <_3D_Point>());
                    var R1 = outE[i].GetR();
                    var R2 = outE[i + 1].GetR();
                    List <_3D_Point> uvw1 = new List <_3D_Point>(),
                                     uvw2 = new List <_3D_Point>();
                    foreach (var ip in PassIList[i])
                    {
                        var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
                        var uvw = R1 * xyf;
                        uvw1.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
                    }

                    foreach (var ip in PassIList[i + 1])
                    {
                        var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
                        var uvw = R2 * xyf;
                        uvw2.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
                    }

                    double Xs2 = outE[i + 1].Spos.X, Xs1 = outE[i].Spos.X,
                           Ys2 = outE[i + 1].Spos.Y, Ys1 = outE[i].Spos.Y,
                           Zs2 = outE[i + 1].Spos.Z, Zs1 = outE[i].Spos.Z,
                           Bu = Xs2 - Xs1, Bv = Ys2 - Ys1, Bw = Zs2 - Zs1;

                    for (int j = 0; j < uvw1.Count; j++)
                    {
                        double w2   = uvw2[j].Z,
                               v2   = uvw2[j].Y,
                               u2   = uvw2[j].X,
                               u1   = uvw1[j].X,
                               v1   = uvw1[j].Y,
                               w1   = uvw1[j].Z,
                               N1   = (Bu * w2 - Bw * u2) / (u1 * w2 - u2 * w1),
                               N2   = (Bu * w1 - Bw * u1) / (u1 * w2 - u2 * w1);
                        var    UVW1 = N1 * uvw1[j];
                        var    UVW2 = N2 * uvw2[j];
                        double X    = (Xs1 + Xs2 + UVW1.X + UVW2.X) / 2,
                               Y    = (Ys1 + Ys2 + UVW1.Y + UVW2.Y) / 2,
                               Z    = (Zs1 + Zs2 + UVW1.Z + UVW2.Z) / 2;
                        points[i].Add(new _3D_Point(X, Y, Z));
                    }
                }

                List <_3D_Point> pointList = new List <_3D_Point>();

                for (int i = 0; i < points[0].Count; i++)
                {
                    _3D_Point sum = new _3D_Point();
                    for (int j = 0; j < points.Count; j++)
                    {
                        sum += points[j][i];
                    }
                    pointList.Add(sum / points.Count);
                }

                //points[0].ForEach(p => p.Set_Accurate(6));
                //points[1].ForEach(p => p.Set_Accurate(6));
                //pointList.ForEach(p => p.Set_Accurate(6));


                //List<_3D_Point> tp = new List<_3D_Point>();
                //List<double> l = new List<double>();
                //StreamWriter sw = new StreamWriter("midvalue.txt"),
                //             sw1 = new StreamWriter("result.txt");
                //for (int i = 0; i < PassOList.Count; i++)
                //{
                //    l.Add(_3D_Point.Get_Norm(PassOList[i].pos, pointList[i]));
                //    l[i] = Methods.Set_Accurate(l[i], 6);
                //    sw.WriteLine(PassIList[0][i].Name + "," + points[0][i] + "," + points[1][i]);
                //    sw1.WriteLine(PassIList[0][i].Name + "," + pointList[i] + "," + l[i]);
                //}
                //sw1.Close();
                //sw.Close();
                for (int i = 0; i < pointList.Count; i++)
                {
                    PassOList.Add(new ObjectData {
                        Name = PassIList[0][i].Name, pos = pointList[i]
                    });
                }
                return(PassOList);
            }
Esempio n. 12
0
            /// <summary>
            /// 前方交会
            /// </summary>
            /// <returns></returns>
            public List<OData> ForwardForcus()
            {
                List<List<_3D_Point>> points = new List<List<_3D_Point>>();
                for (int i = 0; i < outE.Count - 1; i++)
                {
                    points.Add(new List<_3D_Point>());
                    var R1 = GetR(outE[i]);
                    var R2 = GetR(outE[i + 1]);
                    List<_3D_Point> uvw1 = new List<_3D_Point>(),
                                    uvw2 = new List<_3D_Point>();
                    foreach (var ip in PassIList[i])
                    {
                        var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
                        var uvw = R1 * xyf;
                        uvw1.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
                    }

                    foreach (var ip in PassIList[i + 1])
                    {
                        var xyf = new _3D_Point(ip.pos.X - inE.p0.X - Get_dx(ip), ip.pos.Y - inE.p0.Y - Get_dy(ip), -inE.f).ToColumnMatrix();
                        var uvw = R2 * xyf;
                        uvw2.Add(new _3D_Point(uvw[0, 0], uvw[1, 0], uvw[2, 0]));
                    }

                    double Xs2 = outE[i + 1].Spos.X, Xs1 = outE[i].Spos.X,
                           Ys2 = outE[i + 1].Spos.Y, Ys1 = outE[i].Spos.Y,
                           Zs2 = outE[i + 1].Spos.Z, Zs1 = outE[i].Spos.Z,
                           Bu = Xs2 - Xs1, Bv = Ys2 - Ys1, Bw = Zs2 - Zs1;

                    for (int j = 0; j < uvw1.Count; j++)
                    {
                        double w2 = uvw2[j].Z,
                               v2 = uvw2[j].Y,
                               u2 = uvw2[j].X,
                               u1 = uvw1[j].X,
                               v1 = uvw1[j].Y,
                               w1 = uvw1[j].Z,
                               N1 = (Bu * w2 - Bw * u2) / (u1 * w2 - u2 * w1),
                               N2 = (Bu * w1 - Bw * u1) / (u1 * w2 - u2 * w1);
                        var UVW1 = N1 * uvw1[j];
                        var UVW2 = N2 * uvw2[j];
                        double X = (Xs1 + Xs2 + UVW1.X + UVW2.X) / 2,
                               Y = (Ys1 + Ys2 + UVW1.Y + UVW2.Y) / 2,
                               Z = (Zs1 + Zs2 + UVW1.Z + UVW2.Z) / 2;
                        points[i].Add(new _3D_Point(X, Y, Z));
                    }
                }

                List<_3D_Point> pointList = new List<_3D_Point>();
                for (int i = 0; i < points[0].Count; i++)
                {
                    _3D_Point sum = new _3D_Point();
                    for (int j = 0; j < points.Count; j++)
                    {
                        sum += points[j][i];
                    }
                    pointList.Add(sum / points.Count);
                }

                //points[0].ForEach(p => p.Set_Accurate(6));
                //points[1].ForEach(p => p.Set_Accurate(6));
                //pointList.ForEach(p => p.Set_Accurate(6));


                //List<_3D_Point> tp = new List<_3D_Point>();
                //List<double> l = new List<double>();
                //StreamWriter sw = new StreamWriter("midvalue.txt"),
                //             sw1 = new StreamWriter("result.txt");
                //for (int i = 0; i < PassOList.Count; i++)
                //{
                //    l.Add(_3D_Point.Get_Norm(PassOList[i].pos, pointList[i]));
                //    l[i] = Methods.Set_Accurate(l[i], 6);
                //    sw.WriteLine(PassIList[0][i].Name + "," + points[0][i] + "," + points[1][i]);
                //    sw1.WriteLine(PassIList[0][i].Name + "," + pointList[i] + "," + l[i]);
                //}
                //sw1.Close();
                //sw.Close();
                for (int i = 0; i < pointList.Count; i++)
                {
                    PassOList.Add(new ObjectData { Name = PassIList[0][i].Name, pos = pointList[i] });
                }
                return PassOList;
            }
Esempio n. 13
0
            private Geodetic_Point Direct(_3D_Point XYZ)
            {
                double e_2 = ep.data.e_2,
                       e2_2 = ep.data.e2_2,
                       a = ep.data.a,
                       b = ep.data.b;
                double r = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y)),
                       u0 = Math.Atan(XYZ.Z / r * Math.Sqrt(1 + e2_2)),
                       tgB = (XYZ.Z + b * e2_2 * Math.Sin(u0) * Math.Sin(u0)) /
                             (r - a * e_2 * Math.Cos(u0) * Math.Cos(u0)),
                       B = Math.Atan(XYZ.Z / r + a * e_2 * tgB / r / Math.Sqrt(1 + (1 - e_2) * tgB * tgB));
                _3D_Point blh = new _3D_Point();
                //blh.X = Angle.ChangeToAngle(B).ChangeToDouble();
                blh.X = B;

                //var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
                //blh.Y = l.ChangeToDouble();
                blh.Y = Auxiliary.Get_L(XYZ.X,XYZ.Y);

                double W = Auxiliary.GetW(e_2, B),
                       N = a / W;
                double h1 = XYZ.Z - N * (1 - e_2) * Math.Sin(B),
                       h2 = r - N * Math.Cos(B);
                double h = Math.Sqrt(h1 * h1 + h2 * h2);
                if (h1 / Math.Sin(B) < 0 || h2 / Math.Cos(B) < 0)
                    h = -h;
                blh.Z = h;
                return new Geodetic_Point(false, blh.X, blh.Y, blh.Z);
            }
Esempio n. 14
0
 private Geodetic_Point Iteration(_3D_Point XYZ)
 {
     _3D_Point blh = new _3D_Point();
     //var l = _2D_Point.Get_PAngle(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
     blh.Y = Auxiliary.Get_L(XYZ.X,XYZ.Y);
     double dis = _2D_Point.Get_Norm(new _2D_Point(0, 0), new _2D_Point(XYZ.X, XYZ.Y));
     double t0 = XYZ.Z / dis,
             p = ep.data.c * ep.data.e_2 / dis,
             k = 1 + ep.data.e2_2,
             t1 = t0,
             t2 = t0;
     do
     {
         t1 = t2;
         t2 = t0 + p * t1 / Math.Sqrt(k + t1 * t1);
     }
     while (Math.Abs(Angle.ChangeToAngle(t2 - t1).Miao) > 0.0001);
     blh.X = Angle.ChangeToAngle(t2 = Math.Atan(t2)).ChangeToRad();
     double W = Auxiliary.GetW(ep.data.e_2, t2),
            N = ep.data.a / W;
     double h1 = XYZ.Z - N * (1 - ep.data.e_2) * Math.Sin(t2),
            h2 = dis - N * Math.Cos(t2);
     double h = Math.Sqrt(h1 * h1 + h2 * h2);
     if (h1 / Math.Sin(t2) < 0 || h2 / Math.Cos(t2) < 0)
         h = -h;
     blh.Z = h;
     return new Geodetic_Point(false, blh.X, blh.Y, blh.Z);
 }
Esempio n. 15
0
 /// <summary>
 /// 空间直角坐标到大地坐标
 /// </summary>
 /// <param name="XYZ">直接坐标</param>
 /// <param name="mode">计算模式,默认为0(迭代法),其他为直接法</param>
 /// <returns>大地坐标</returns>
 public Geodetic_Point Xyz2Blh(_3D_Point XYZ, int mode = 0)
 {
     if (mode == 0)
         return Iteration(XYZ);
     else
         return Direct(XYZ);
 }
Esempio n. 16
0
 /// <summary>
 /// 大地坐标到空间直角坐标
 /// </summary>
 /// <param name="BLH">大地坐标</param>
 /// <returns>直角坐标</returns>
 public _3D_Point Blh2Xyz(Geodetic_Point BLH)
 {
     double W = Auxiliary.GetW(ep.data.e_2, BLH.Br);
     double V = Auxiliary.GetV(ep.data.e2_2, BLH.Br);
     double N = ep.data.a / W;
     _3D_Point p = new _3D_Point();
     p.X = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Cos(BLH.Lr);
     p.Y = (N + BLH.H) * Math.Cos(BLH.Br) * Math.Sin(BLH.Lr);
     p.Z = (N * (1 - ep.data.e_2) + BLH.H) * Math.Sin(BLH.Br);
     return p;
 }