Exemplo n.º 1
0
        ///// <summary>
        ///// 残差矩阵
        ///// </summary>
        ///// <returns></returns>
        //private double[,] GetA()
        //{
        //    double[,] M = GetM();
        //    double[,] I2 = MatrixTool.Ident(2);
        //    double[,] A = MatrixTool.Init(2, 4);

        //    MatrixTool.MutliConst(ref I2, -1);
        //    //MatrixTool.MutliConst(ref M, (1 + k));
        //    MatrixTool.CopySub(M, 0, 0, 2, 2, ref A, 0, 0);
        //    MatrixTool.CopySub(I2, 0, 0, 2, 2, ref A, 0, 2);
        //    return A;
        //}
        //private double[,] GetV(double[,] X, double[,] Y, CoordTrans4Param dpp)
        //{
        //    int rowNum = X.GetLength(0);

        //    double[,] B, F, A, B2, B3, F2, V;
        //    double[,] AT = MatrixTool.Init(4, 2);

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

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


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

            Set4Param(0, 0, 0, 0);
            //this.SetRotationParamMM(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);
            return(-1);
            //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;
        }
Exemplo n.º 2
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;
        }