示例#1
0
        /// <summary>
        /// 最小二乘误差矩阵
        /// [1 0  -y  x]
        /// [0 1   x  y]
        /// </summary>
        /// <param name="X"></param>
        /// <returns></returns>

        private double[,] GetB(double[,] X)
        {
            int rowNum = X.GetLength(0);

            double[,] B = MatrixTool.Init(rowNum, 4);
            //double[,] M = GetM();
            double[,] mi = MatrixTool.Ident(2);
            //double[,]  MK;

            //MK = specialMulti(M, X);

            for (int i = 0; i < rowNum; i += 2)
            {
                MatrixTool.CopySub(mi, 0, 0, 2, 2, ref B, i, 0);
            }

            for (int i = 0; i < rowNum; i += 2)
            {
                B[i, 2]     = X[i, 0];
                B[i, 3]     = -X[i + 1, 0];
                B[i + 1, 2] = X[i + 1, 0];
                B[i + 1, 3] = X[i, 0];
            }

            return(B);
        }
示例#2
0
        private double[,] GetV(double[,] X, double[,] Y, CoordTrans7Param dpp)
        {
            int rowNum = X.GetLength(0);

            double[,] B, F, A, B2, B3, F2, V;
            double[,] AT = MatrixTool.Init(6, 3);

            A = GetA();
            MatrixTool.AT(A, ref AT);
            MatrixTool.MutliConst(ref AT, 1 / (1 + (1 + k) * (1 + k)));

            F  = GetF(X, Y);
            B  = GetB(X);
            B2 = MatrixTool.Init(3, 7);
            B3 = MatrixTool.Init(3, 1);
            F2 = MatrixTool.Init(rowNum, 1);
            for (int i = 0; i < rowNum / 3; i++)
            {
                MatrixTool.CopySub(B, i * 3, 0, 3, 7, ref B2, 0, 0);
                MatrixTool.Multi(B2, dpp.values, ref B3);
                MatrixTool.CopySub(B3, 0, 0, 3, 1, ref F2, i * 3, 0);
            }
            MatrixTool.Sub(F, F2, ref F2);
            V = specialMulti(AT, F2);
            return(V);
        }
示例#3
0
        /// <summary>
        /// X矩阵如下所示
        ///
        ///
        /// </summary>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <returns></returns>
        public double CalculateTrans7Param(double[,] X, double[,] Y)
        {
            int PtNum = X.GetLength(0) / 3;

            double[,] B;
            double[,] F;
            double[,] BT  = MatrixTool.Init(7, 3 * PtNum);
            double[,] BTB = MatrixTool.Init(7, 7);
            double[,] BTF = MatrixTool.Init(7, 1);


            //init pararm
            CoordTrans7Param dpp = new CoordTrans7Param();

            Set4Param(0, 0, 0, 0);
            this.SetRotationParam(0, 0, 0);
            //debug
            //this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
            int round = 0;

            while (round++ < 20)
            {
                F = GetF(X, Y);
                B = GetB(X);
                MatrixTool.AT(B, ref BT);

                MatrixTool.Multi(BT, B, ref BTB);
                MatrixTool.Inv(BTB);
                MatrixTool.Multi(BT, F, ref BTF);
                MatrixTool.Multi(BTB, BTF, ref dpp.values);
                if (dpp.isSmall())
                {
                    break;
                }
                else
                {
                    MatrixTool.Add(this.values, dpp.values, ref this.values);
                }
            }
            Console.WriteLine(round);
            //this.TransCoord(X[0,0],X[1,0],X[2,0],out x2,out y2,out z2);
            //double[,] V = GetV(X, Y, dpp);


            double vMax = -1;

            //for (int i = 0; i < V.GetLength(0); i++)
            //{
            //    if (Math.Abs(V[i, 0]) > vMax)
            //        vMax = Math.Abs(V[i, 0]);
            //}

            return(vMax);
        }
示例#4
0
        private double[,] GetA()
        {
            double[,] M  = GetM();
            double[,] I2 = MatrixTool.Ident(3);
            double[,] A  = MatrixTool.Init(3, 6);

            MatrixTool.MutliConst(ref I2, -1);
            MatrixTool.MutliConst(ref M, (1 + k));
            MatrixTool.CopySub(M, 0, 0, 3, 3, ref A, 0, 0);
            MatrixTool.CopySub(I2, 0, 0, 3, 3, ref A, 0, 3);
            return(A);
        }
示例#5
0
        /// <summary>
        /// 获取观测方程。 -DeltaX - ((1+k)*RotateMatrix*X - Y), 或者 Y = DeltaX + (1+k)*RotateMatrix*X (布尔沙方程)
        /// </summary>
        /// <param name="X">原始点</param>
        /// <param name="Y">目标点</param>
        /// <returns></returns>
        private double[,] GetF(double[,] X, double[,] Y)
        {
            double[,] f0;
            double[,] qx = MatrixTool.Init(X.GetLength(0), 1);
            double[,] K  = { { -dx }, { -dy } };
            //double[,] S = { { 1  } };

            //MatrixTool.Multi(X, S, ref qx);
            double[,] M = GetM();
            qx          = specialMulti(M, X);
            MatrixTool.Sub(qx, Y, ref qx);
            f0 = specialSub(K, qx);
            return(f0);
        }
示例#6
0
        public void TransCoord(double x1, double y1, out double x2, out double y2)
        {
            double[,] Xi = { { x1 }, { y1 } };
            double[,] DX = { { dx }, { dy } };
            double[,] tY = new double[2, 1];
            //double[,] K = { { 1 + k } };

            double[,] M = GetM();
            //MatrixTool.Multi(Xi, K, ref tY);
            MatrixTool.Multi(M, Xi, ref tY);
            MatrixTool.Add(tY, DX, ref tY);
            x2 = tY[0, 0];
            y2 = tY[1, 0];
        }
示例#7
0
        private double[,] specialSub(double[,] m, double[,] X)
        {
            int rowNumM = m.GetLength(0);
            int colNumM = m.GetLength(1);
            int rowNumX = X.GetLength(0);
            int colNumX = X.GetLength(1);
            int lines   = rowNumX / rowNumM;

            double[,] subX = MatrixTool.Init(rowNumM, colNumX);
            double[,] res  = MatrixTool.Init(rowNumX, colNumX);

            for (int i = 0; i < rowNumX; i += rowNumM)
            {
                MatrixTool.CopySub(X, i, 0, rowNumM, colNumX, ref subX, 0, 0);
                MatrixTool.Sub(m, subX, ref subX);
                MatrixTool.CopySub(subX, 0, 0, rowNumM, colNumX, ref res, i, 0);
            }
            return(res);
        }
示例#8
0
        private double[,] GetB(double[,] X)
        {
            int rowNum = X.GetLength(0);

            double[,] B = MatrixTool.Init(rowNum, 7);
            //double[,] M = GetM();
            //double[,] Mdx = GetMdx();
            //double[,] Mdy = GetMdy();
            //double[,] Mdz = GetMdz();
            double[,] mi = MatrixTool.Ident(3);
            //double[,] MX, MY, MZ, MK;

            //MK = specialMulti(M, X);
            //MX = specialMulti(Mdx, X);
            //MY = specialMulti(Mdy, X);
            //MZ = specialMulti(Mdz, X);

            for (int i = 0; i < rowNum; i += 3)
            {
                MatrixTool.CopySub(mi, 0, 0, 3, 3, ref B, i, 0);

                B[i, 3] = 0;
                B[i, 4] = -X[i + 2, 0];
                B[i, 5] = X[i + 1, 0];
                B[i, 6] = X[i, 0];

                B[i + 1, 3] = X[i + 2, 0];
                B[i + 1, 4] = 0;
                B[i + 1, 5] = -X[i + 0, 0];
                B[i + 1, 6] = X[i + 1, 0];

                B[i + 2, 3] = -X[i + 1, 0];
                B[i + 2, 4] = X[i + 0, 0];
                B[i + 2, 5] = 0;
                B[i + 2, 6] = X[i + 2, 0];
            }
            //MatrixTool.CopySub(MX, 0, 0, rowNum, 1, ref B, 0, 3);
            //MatrixTool.CopySub(MY, 0, 0, rowNum, 1, ref B, 0, 4);
            //MatrixTool.CopySub(MZ, 0, 0, rowNum, 1, ref B, 0, 5);
            //MatrixTool.CopySub(MK, 0, 0, rowNum, 1, ref B, 0, 6);
            return(B);
        }
示例#9
0
        //private double[,] GetMdx()
        //{
        //    double[,] mt = { { 0, 0, 0 }, { 0, -Math.Sin(rx), Math.Cos(rx) }, { 0, -Math.Cos(rx), -Math.Sin(rx) } };

        //    double[,] m = new double[3, 3];

        //    MatrixTool.Multi(GetMz(), GetMy(), ref m);
        //    MatrixTool.Multi(m, mt, ref m);
        //    return m;
        //}
        //private double[,] GetMdy()
        //{
        //    double[,] mt = { { -Math.Sin(ry), 0, -Math.Cos(ry) }, { 0, 0, 0 }, { Math.Cos(ry), 0, -Math.Sin(ry) } };

        //    double[,] m = new double[3, 3];

        //    MatrixTool.Multi(GetMz(), mt, ref m);
        //    MatrixTool.Multi(m, GetMx(), ref m);
        //    return m;
        //}

        //private double[,] GetMdz()
        //{
        //    double[,] mt = { { -Math.Sin(rz), Math.Cos(rz), 0 }, { -Math.Cos(rz), -Math.Sin(rz), 0 }, { 0, 0, 0 } };

        //    double[,] m = new double[3, 3];

        //    MatrixTool.Multi(mt, GetMy(), ref m);
        //    MatrixTool.Multi(m, GetMx(), ref m);
        //    return m;
        //}
        private double[,] specialMulti(double[,] m, double[,] X)
        {
            int rowNumM = m.GetLength(0);
            int colNumM = m.GetLength(1);
            int rowNumX = X.GetLength(0);
            int colNumX = X.GetLength(1);
            int lines   = rowNumX / colNumM;

            double[,] mt   = MatrixTool.Init(rowNumM, colNumX);
            double[,] subX = MatrixTool.Init(colNumM, colNumX);
            double[,] res  = MatrixTool.Init(rowNumM * lines, colNumX);

            for (int i = 0; i < lines; i++)
            {
                MatrixTool.CopySub(X, i * colNumM, 0, colNumM, colNumX, ref subX, 0, 0);
                MatrixTool.Multi(m, subX, ref mt);
                MatrixTool.CopySub(mt, 0, 0, rowNumM, colNumX, ref res, i * rowNumM, 0);
            }
            return(res);
        }
示例#10
0
        /// <summary>
        /// 获取观测方程。 -DeltaX - ((1+k)*RotateMatrix*X - Y), 或者 Y = DeltaX + (1+k)*RotateMatrix*X (布尔沙方程)
        /// </summary>
        /// <param name="X">原始点</param>
        /// <param name="Y">目标点</param>
        /// <returns></returns>
        private double[,] GetF(double[,] X, double[,] Y)
        {
            double[,] f0 = MatrixTool.Init(X.GetLength(0), 1);
            int pn = X.GetLength(0) / 3;

            //double[,] qx = MatrixTool.Init(X.GetLength(0), 1);
            //double[,] K = { { -dx }, { -dy }, { -dz } };
            //double[,] S = { {  k } };

            //MatrixTool.Multi(X, S, ref qx);
            //double[,] M = GetM();
            //qx = specialMulti(M, qx);
            //MatrixTool.Sub(qx, Y, ref qx);
            for (int i = 0; i < pn; i++)
            {
                f0[i * 3 + 0, 0] = -(dx + (-X[i * 3 + 2, 0] * ry) + X[i * 3 + 1, 0] * rz + X[i * 3 + 0, 0] * (1 + k) - Y[i * 3 + 0, 0]);
                f0[i * 3 + 1, 0] = -(dy + (X[i * 3 + 2, 0] * rx) + (-X[i * 3 + 0, 0] * rz) + X[i * 3 + 1, 0] * (1 + k) - Y[i * 3 + 1, 0]);
                f0[i * 3 + 2, 0] = -(dz + (-X[i * 3 + 1, 0] * rx) + X[i * 3 + 0, 0] * ry + X[i * 3 + 2, 0] * (1 + k) - Y[i * 3 + 2, 0]);
            }
            //f0 = specialSub(K, qx);
            return(f0);
        }
示例#11
0
        // 117.11726427,36.67414252


        public static void MarsToEarth(double[] marsLong, double[] marsLat, double[] earthLong, double[] earthLat,
                                       double[] newMarsLong, double[] newMarsLat, out double[] newEarthLong, out double[] newEarthLat)
        {
            int ptNum = marsLong.Length;

            double[,] X = MatrixTool.Init(2 * ptNum, 1);
            double[,] Y = MatrixTool.Init(2 * ptNum, 1);


            int pN = newMarsLong.Length;

            double[] XX = new double[pN * 2];
            double[] YY = new double[pN * 2];

            newEarthLong = new double[pN];
            newEarthLat  = new double[pN];
            //double[] YYRes = new double[pN * 2];

            for (int i = 0; i < ptNum; i++)
            {
                CoordinateConverter.LatLonToUTMXY(marsLat[i], marsLong[i], out X[2 * i + 0, 0], out X[2 * i + 1, 0]);
                CoordinateConverter.LatLonToUTMXY(earthLat[i], earthLong[i], out Y[2 * i + 0, 0], out Y[2 * i + 1, 0]);
                CoordinateConverter.LatLonToUTMXY(newMarsLat[i], newMarsLong[i], out XX[2 * i + 0], out XX[2 * i + 1]);
            }

            CoordTrans4Param ct = new CoordTrans4Param();

            ct.CalculateTrans4Param(X, Y);
            for (int i = 0; i < pN; i++)
            {
                ct.TransCoord(XX[i * 2 + 0], XX[i * 2 + 1], out YY[i * 2 + 0], out YY[i * 2 + 1]);
                CoordinateConverter.UTMXYToLatLon(YY[i * 2 + 0], YY[i * 2 + 1], out newEarthLat[i], out newEarthLong[i]);
                //Console.WriteLine("准确的 x {0}, y {1}, 计算的 x {2} , y {3} ", YYRead[i * 2 + 0], YYRead[i * 2 + 1], YY84[i * 2 + 0], YY84[i * 2 + 1]);
                //Console.WriteLine("准确的 x {0}, y {1}, 计算的 x {2} , y {3} ", YYRead[i * 2 + 0], YYRead[i * 2 + 1], YY[i * 2 + 0] / sx + cx2, YY[i * 2 + 1] / sy + cy2);
            }
            //return YYRes;
        }
示例#12
0
文件: Form1.cs 项目: xuyanning/JQGis
        private void button1_Click(object sender, EventArgs e)
        {
            double x1, y1, x2, y2;

            x1 = 119;
            x2 = 120;
            y1 = y2 = 35;
            Console.WriteLine("x:" + CoordinateConverter.getUTMDistance(x1, y1, x2, y2));
            Console.WriteLine("y:" + CoordinateConverter.getUTMDistance(x1, y1, x1, y2 + 1));
            string fileName = @"D:\sevenParamsTest.xlsx";
            //if (chainList.Count > 0)
            //    chainList.Clear();
            //LoadDataTableFromExcel(fileName, "Sheet1");
            int ptNum = 10;

            double[,] X   = MatrixTool.Init(3 * ptNum, 1);
            double[,] Y   = MatrixTool.Init(3 * ptNum, 1);
            double[,] Y84 = MatrixTool.Init(3 * ptNum, 1);


            int pN = 10;

            double[] XX     = new double[pN * 3];
            double[] YYRead = new double[pN * 3];
            double[] YY     =  new double[pN * 3];
            double[] YY84   = new double[pN * 3];

            int i = 0;
            int j = 0;
            int k = 0;

            foreach (DataRow dr in ds.Tables["Sheet1"].Rows)
            {
                if (i % 20 == 0 && j < ptNum)
                {
                    X[j * 3 + 1, 0]   = Convert.ToDouble(dr["North"]);
                    X[j * 3 + 0, 0]   = Convert.ToDouble(dr["East"]);
                    X[j * 3 + 2, 0]   = Convert.ToDouble(dr["Altitude"]);
                    Y84[j * 3 + 0, 0] = Convert.ToDouble(dr["Longitude"]);
                    Y84[j * 3 + 1, 0] = Convert.ToDouble(dr["Latitude"]);
                    Y84[j * 3 + 2, 0] = Convert.ToDouble(dr["Altitude"]);
                    j++;
                }
                if (i % 20 == 10 && k < ptNum)
                {
                    XX[k * 3 + 1]     = Convert.ToDouble(dr["North"]);
                    XX[k * 3 + 0]     = Convert.ToDouble(dr["East"]);
                    XX[k * 3 + 2]     = Convert.ToDouble(dr["Altitude"]);
                    YYRead[k * 3 + 0] = Convert.ToDouble(dr["Longitude"]);
                    YYRead[k * 3 + 1] = Convert.ToDouble(dr["Latitude"]);
                    YYRead[k * 3 + 2] = Convert.ToDouble(dr["Altitude"]);
                    k++;
                }
                i++;
                if (j == ptNum && k == ptNum)
                {
                    break;
                }
            }

            for (i = 0; i < ptNum; i++)
            {
                CoordinateConverter.LatLonToUTMXY(Y84[3 * i + 1, 0], Y84[3 * i + 0, 0], out Y[3 * i + 0, 0], out Y[3 * i + 1, 0]);
            }

            CoordTrans7Param ct = new CoordTrans7Param();

            ct.CalculateTrans7Param(X, Y);
            for (i = 0; i < pN; i++)
            {
                ct.TransCoord(XX[i * 3 + 0], XX[i * 3 + 1], XX[i * 3 + 2], out YY[i * 3 + 0], out YY[i * 3 + 1], out YY[i * 3 + 2]);
                CoordinateConverter.UTMXYToLatLon(YY[i * 3 + 0], YY[i * 3 + 1], out YY84[i * 3 + 1], out YY84[i * 3 + 0]);
                Console.WriteLine("准确的 x {0}, y {1}, z{2}, 计算的 x {3} , y {4} , z {5}", YYRead[i * 3 + 0], YYRead[i * 3 + 1], YYRead[i * 3 + 2], YY84[i * 3 + 0], YY84[i * 3 + 1], YY[i * 3 + 2]);
            }
        }
示例#13
0
文件: Form1.cs 项目: xuyanning/JQGis
        private void button2_Click1(object sender, EventArgs e)
        {
            double x1, y1, x2, y2;

            x1 = 119;
            x2 = 120;
            y1 = y2 = 35;
            Console.WriteLine("x:" + CoordinateConverter.getUTMDistance(x1, y1, x2, y2));
            Console.WriteLine("y:" + CoordinateConverter.getUTMDistance(x1, y1, x1, y2 + 1));
            string fileName = @"D:\sevenParamsTest.xlsx";
            //if (chainList.Count > 0)
            //    chainList.Clear();
            //LoadDataTableFromExcel(fileName, "Sheet1");
            int ptNum = 5;

            double[,] X = MatrixTool.Init(2 * ptNum, 1);
            double[,] Y = MatrixTool.Init(2 * ptNum, 1);


            int pN = 10;

            double[] XX     = new double[pN * 2];
            double[] YYRead = new double[pN * 2];
            double[] YY     = new double[pN * 2];

            int i = 0;
            int j = 0;
            int k = 0;

            foreach (DataRow dr in ds.Tables["Sheet1"].Rows)
            {
                if (i % 40 == 0 && j < ptNum)
                {
                    X[j * 2 + 0, 0] = Convert.ToDouble(dr["North"]);
                    X[j * 2 + 1, 0] = Convert.ToDouble(dr["East"]);

                    Y[j * 2 + 0, 0] = Convert.ToDouble(dr["Longitude"]);
                    Y[j * 2 + 1, 0] = Convert.ToDouble(dr["Latitude"]);

                    j++;
                }
                if (i % 20 == 10 && k < pN)
                {
                    XX[k * 2 + 0] = Convert.ToDouble(dr["North"]);
                    XX[k * 2 + 1] = Convert.ToDouble(dr["East"]);

                    YYRead[k * 2 + 0] = Convert.ToDouble(dr["Longitude"]);
                    YYRead[k * 2 + 1] = Convert.ToDouble(dr["Latitude"]);

                    k++;
                }
                i++;
                if (j == ptNum && k == pN)
                {
                    break;
                }
            }
            double cx1, cy1, cx2, cy2;

            cx1 = cy1 = cx2 = cy2 = 0;
            double sx = 91310;
            double sy = 111000;

            for (i = 0; i < pN; i++)
            {
                cx1 += XX[2 * i];
                cy1 += XX[2 * i + 1];
                cx2 += YYRead[2 * i];
                cy2 += YYRead[2 * i + 1];
            }
            cx1 /= pN;
            cy1 /= pN;
            cx2 /= pN;
            cy2 /= pN;

            for (i = 0; i < ptNum; i++)
            {
                X[2 * i, 0]     = X[2 * i, 0] - cx1;
                X[2 * i + 1, 0] = X[2 * i + 1, 0] - cy1;
                Y[2 * i, 0]     = (Y[2 * i, 0] - cx2) * sx;
                Y[2 * i + 1, 0] = (Y[2 * i + 1, 0] - cy2) * sy;
            }

            for (i = 0; i < pN; i++)
            {
                XX[2 * i]     = XX[2 * i] - cx1;
                XX[2 * i + 1] = XX[2 * i + 1] - cy1;
            }

            CoordTrans4Param ct = new CoordTrans4Param();

            ct.CalculateTrans4Param(X, Y);
            for (i = 0; i < pN; i++)
            {
                ct.TransCoord(XX[i * 2 + 0], XX[i * 2 + 1], out YY[i * 2 + 0], out YY[i * 2 + 1]);
                Console.WriteLine("准确的 x {0}, y {1}, 计算的 x {2} , y {3} ", YYRead[i * 2 + 0], YYRead[i * 2 + 1], YY[i * 2 + 0] / sx + cx2, YY[i * 2 + 1] / sy + cy2);
            }
        }