C# Matrix.T方法代码示例

 /// <summary>
 /// 通过新旧坐标表构造，至少两个点
 /// </summary>
 /// <param name="pold">旧坐标表</param>
 /// <param name="pnew">新坐标表</param>
 /// <param name="mode">默认为0(迭代求解)，其他时候(直接求解)</param>
 public CoordinateT(List<_2D_Point> pold,List<_2D_Point> pnew,int mode=0)
 {
     this.pold = pold;
     this.pnew = pnew;
     Matrix x;
     if(mode==0)
     {
         Matrix v=new Matrix(pnew.Count*2,1);
         do
         {
             Matrix B = GetB(),
                    l = Getl();
             x = (B.T() * B).Inverse() * B.T() * l;
             Update(x);
             v = v + B * x - l;
         } while (!Terminate(x));
         xigema = Math.Sqrt((v.T() * v / (pnew.Count * 2 - 4))[0, 0]);
     }
     else
     {
         Matrix B = new Matrix(pnew.Count * 2, 4);
         Matrix l = new Matrix(pnew.Count * 2, 1);
         for (int i = 0; i < pnew.Count; i++)
         {
             B[2 * i, 0] = 1;
             B[2 * i, 1] = 0;
             B[2 * i, 2] = pold[i].X;
             B[2 * i, 3] = pold[i].Y;
             B[2 * i + 1, 0] = 0;
             B[2 * i + 1, 1] = 1;
             B[2 * i + 1, 2] = pold[i].Y;
             B[2 * i + 1, 3] = -pold[i].X;
             l[2*i, 0] = pnew[i].X;
             l[2 * i + 1,0] = pnew[i].Y;
         }
         x = (B.T() * B).Inverse() * B.T() * l;
         dx = x[0, 0];
         dy = x[1, 0];
         xita = Math.Atan(x[3, 0] / x[2, 0]);
         m = x[2, 0] / cos(xita) - 1;
         Matrix X=new Matrix(4,1);
         Matrix v = B * x - l;
         xigema = Math.Sqrt((v.T() * v / (pnew.Count * 2 - 4))[0, 0]);
     }
 }

 /// <summary>
 /// 通过新旧坐标表构造，三个点以上
 /// </summary>
 /// <param name="pold">旧坐标表</param>
 /// <param name="pnew">新坐标表</param>
 public CoordinateT3(List<_3D_Point> pold,List<_3D_Point> pnew)
 {
     this.pold = pold;
     this.pnew = pnew;
     Matrix x;
     Matrix v=new Matrix(pnew.Count*3,1);
     Matrix B = GetB(),
            l = Getl();
     x = (B.T() * B).Inverse() * B.T() * l;
     Update(x);
     v = B * x - l;
     xigema = Math.Sqrt((v.T() * v / (pnew.Count * 3 - 4))[0, 0]);
 }

            /// <summary>
            /// 严密的前方交会
            /// </summary>
            public List<OData> ForwardForcus(List<OData> originOdList)
            {
                Matrix t, x;
                List<_3D_Point> Xx = new List<_3D_Point>();
                //迭代求解加密点坐标(循环分块)
                do
                {
                    Xx.Clear();
                    List<Matrix> Bs = new List<Matrix>();
                    List<Matrix> ls = new List<Matrix>();
                    for (int i = 0; i < outE.Count; i++)
                    {
                        Bs.Add(GetFB(outE[i], PassIList[i], originOdList));
                        ls.Add(GetFl(outE[i], PassIList[i], originOdList));
                    }
                    Matrix B = new Matrix(Bs.Count * 2, 3);
                    Matrix lm = new Matrix(Bs.Count * 2, 1);
                    for (int m = 0; m < originOdList.Count; m++)
                    {
                        for (int i = 0; i < outE.Count; i++)
                        {
                            B[2 * i, 0] = Bs[i][2 * m, 0];
                            B[2 * i, 1] = Bs[i][2 * m, 1];
                            B[2 * i, 2] = Bs[i][2 * m, 2];
                            B[2 * i + 1, 0] = Bs[i][2 * m, 0];
                            B[2 * i + 1, 1] = Bs[i][2 * m + 1, 1];
                            B[2 * i + 1, 2] = Bs[i][2 * m + 1, 2];
                            lm[2 * i, 0] = ls[i][2 * m, 0];
                            lm[2 * i + 1, 0] = ls[i][2 * m + 1, 0];
                        }
                        x = (B.T() * B).Inverse() * B.T() * lm;
                        Xx.Add(new _3D_Point(x[0, 0], x[1, 0], x[2, 0]));

                    }
                    t = _3D_Point.ToColumnMatrix(Xx);
                    for (int i = 0; i < originOdList.Count; i++)
                    {
                        originOdList[i].pos += Xx[i];
                    }
                } while (!IsTerminating(0.000001, t));
                //StreamWriter sw = new StreamWriter("result.txt");
                //List<double> rs = new List<double>();

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

                //for (int i = 0; i < ps.Count; i++)
                //{
                //    _3D_Point dxyz = ps[i].pos - PassOList[i].pos;
                //    ddxyz.Add(dxyz);
                //    double r = _3D_Point.Get_Norm(dxyz, new _3D_Point());
                //    rs.Add(r);
                //    sw.WriteLine(ps[i].Name + "," + ps[i].pos + "," + dxyz + "," + r);
                //}
                //double sum = 0;
                //rs.ForEach(r => sum += r * r); double a = Math.Sqrt(sum / rs.Count);
                //sw.WriteLine("dr=" + a + ",点位精度:" + a / 5000);
                //sw.Close();
                PassOList = originOdList;
                return PassOList;
            }

            /// <summary>
            /// 光束法
            /// </summary>
            /// <param name="ie">内方位元素初值</param>
            /// <returns></returns>
            public List<OData> LightMethod(InElement ie)
            {
                BackForcus(ie);
                ForwardForcus();
                AllOList = MCOList.Concat(PassOList).ToList();
                AllIList =AllIList??new List<List<IData>>();
                for (int i = 0; i < MCIList.Count;++i )
                {
                    AllIList.Add(MCIList[i].Concat(PassIList[i]).ToList());
                }
                Matrix B = new Matrix(AllOList.Count * 2 * AllIList.Count, 7 * MCIList.Count + 3 * AllOList.Count),
                       l = new Matrix(B.Row, 1),
                       Bx = new Matrix(outE.Count, B.Column),
                       w = new Matrix(Bx.Row, 1),
                       P = Matrix.Eye(B.Column);
#region 设置控制点的权值
                for (int i = 0; i < MCOList.Count;++i )
                {
                    P[i, i] = 500;
                }
#endregion
                double[] X = new double[7 * outE.Count+PassIList[0].Count*3];
                for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7)
                {
                    X[j] = outE[i].Spos.X;
                    X[j + 1] = outE[i].Spos.Y;
                    X[j + 2] = outE[i].Spos.Z;
                    X[j + 3] = outE[i].q0;
                    X[j + 4] = outE[i].q1;
                    X[j + 5] = outE[i].q2;
                    X[j + 6] = outE[i].q3;
                }

                if (PassOList.Count == 0)
                {
                    PassIList[0].ForEach(pi => PassOList.Add(new OData(pi.Name, new _3D_Point())));
                }
                int start = 7 * outE.Count;
                for (int i = 0; i < PassIList[0].Count; ++i)
                {
                    PassOList[i].pos.X = X[start + 3 * i];
                    PassOList[i].pos.Y = X[start + 3 * i + 1];
                    PassOList[i].pos.Z = X[start + 3 * i + 2];
                }

                Matrix X0 = new Matrix(X.GetLength(0), 1, X);
                Matrix x;
                do
                {
                    SetBl_light(B, l, Bx, w);
                    var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T());
                    var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row));
                    var N = Matrix.RowCombine(N1, N2);
                    var W = Matrix.RowCombine(B.T() * l, w);
                    var Y = N.Inverse() * W;
                    x = Y.SubRMatrix(0, B.Column - 1);
                    X0 = X0 + x;
                    UpdateData_light(X0);
                } while (!IsTerminating(0.000001, x));
                UpdateData_light(X0);
                return PassOList;
            }

 /// <summary>
 /// 带有初值的后方交会
 /// </summary>
 private void BackForcus0()
 {
     Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 9 + 7 * MCIList.Count),
            l = new Matrix(B.Row, 1),
            Bx = new Matrix(outE.Count, B.Column),
            w = new Matrix(Bx.Row, 1);
     int start = 7 * outE.Count;
     double[] X = new double[9 + 7 * outE.Count];
     for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7)
     {
         X[j] = outE[i].Spos.X;
         X[j + 1] = outE[i].Spos.Y;
         X[j + 2] = outE[i].Spos.Z;
         X[j + 3] = outE[i].q0;
         X[j + 4] = outE[i].q1;
         X[j + 5] = outE[i].q2;
         X[j + 6] = outE[i].q3;
     }
     X[start] = inE.p0.X;
     X[start + 1] = inE.p0.Y;
     X[start + 2] = inE.f;
     X[start + 3] = dParams.k1;
     X[start + 4] = dParams.k2;
     X[start + 5] = dParams.p1;
     X[start + 6] = dParams.p2;
     X[start + 7] = dParams.alph;
     X[start + 8] = dParams.beta;
     Matrix X0 = new Matrix(X.GetLength(0), 1, X);
     Matrix x;
     do
     {
         SetBl0(B, l, Bx, w);
         //B.OutPut("B");
         //l.OutPut("l");
         //Bx.OutPut("Bx");
         //w.OutPut("w");
         var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T());
         var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row));
         var N = Matrix.RowCombine(N1, N2);
         var W = Matrix.RowCombine(B.T() * l, w);
         var Y = N.Inverse() * W;
         x = Y.SubRMatrix(0, B.Column - 1);
         X0 = X0 + x;
         UpdateData0(X0);
     } while (!IsTerminating(0.000001, x));
     UpdateData0(X0);
 }

            /// <summary>
            /// 已知内方位元素、畸变参数的后方交会(可求出外方位元素)
            /// 附有限制条件的间接平差
            /// </summary>
            /// <param name="ie">内方位元素初值</param>
            /// <param name="dp">畸变参数</param>
            public void BackForcus(InElement ie, DParams dp)
            {
                for (int i = 0; i < MCIList.Count; ++i)
                {
                    outE.Add(new OutElement4 { q0 = 1 });
                }

                inE = ie;
                dParams = dp;

                Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 7 * MCIList.Count),
                       l = new Matrix(B.Row, 1),
                       Bx = new Matrix(outE.Count, B.Column),
                       w = new Matrix(Bx.Row, 1);
                double[] X = new double[7 * outE.Count];
                for (int i = 0, j = 0; i < outE.Count; i++, j = j + 7)
                {
                    X[j] = outE[i].Spos.X;
                    X[j + 1] = outE[i].Spos.Y;
                    X[j + 2] = outE[i].Spos.Z;
                    X[j + 3] = outE[i].q0;
                    X[j + 4] = outE[i].q1;
                    X[j + 5] = outE[i].q2;
                    X[j + 6] = outE[i].q3;
                }
                Matrix X0 = new Matrix(X.GetLength(0), 1, X);
                Matrix x;
                do
                {
                    SetBl(B, l, Bx, w);
                    var N1 = Matrix.ColumnCombine(B.T() * B, Bx.T());
                    var N2 = Matrix.ColumnCombine(Bx, Matrix.Zeros(Bx.Row, Bx.Row));
                    var N = Matrix.RowCombine(N1, N2);
                    var W = Matrix.RowCombine(B.T() * l, w);
                    var Y = N.Inverse() * W;
                    x = Y.SubRMatrix(0, B.Column - 1);
                    X0 = X0 + x;
                    UpdateData(X0);
                } while (!IsTerminating(0.000001, x));
                UpdateData(X0);
            }

            /// <summary>
            /// 已知内方位元素、外方位元素、畸变参数初值的后方交会,应先调用SetOriginValue
            /// </summary>
            public void BackForcus()
            {
                Matrix B = new Matrix(MCOList.Count * 2 * MCIList.Count, 6 * MCIList.Count+9),
                       l = new Matrix(B.Row, 1);
                double[] X = new double[6 * outE.Count+9];
                for (int i = 0, j = 0; i < outE.Count; i++, j = j + 6)
                {
                    X[j] = outE[i].Spos.X;
                    X[j + 1] = outE[i].Spos.Y;
                    X[j + 2] = outE[i].Spos.Z;
                    X[j + 3] = outE[i].phi;
                    X[j + 4] = outE[i].omega;
                    X[j + 5] = outE[i].kappa;
                }
                int start = outE.Count * 6;
                X[start] = inE.p0.X;
                X[start + 1] = inE.p0.Y;
                X[start + 2] = inE.f;
                X[start + 3] = dParams.k1;
                X[start + 4] = dParams.k2;
                X[start + 5] = dParams.p1;
                X[start + 6] = dParams.p2;
                X[start + 7] = dParams.alph;
                X[start + 8] = dParams.beta;

                Matrix X0 = new Matrix(X.GetLength(0), 1, X);
                Matrix x;
                do
                {
                    SetBl(B, l);
                    x = (B.T() * B).Inverse() * B.T() * l;
                    X0 = X0 + x;
                    UpdateData(X0);
                } while (!IsTerminating(0.000001, x));
                UpdateData(X0);
            }

 public void TTest1()
 {
     double[,] data = null; // TODO: инициализация подходящего значения
     Matrix target = new Matrix(data); // TODO: инициализация подходящего значения
     target.T();
     Assert.Inconclusive("Невозможно проверить метод, не возвращающий значение.");
 }

        //DLT算法
        public void Adjustment()
        {
            int count=data.oCount;
            Matrix B=new Matrix(count * 2, 11);
            Matrix l=new Matrix(count * 2, 1);

            //求L系数初值
            for (int j = 0; j < data.Count;j++)
            {
                for (int i = 0; i < data.oCount;i++)
                {
                    double x = data.oIPoints[j][i].X,
                        y = data.oIPoints[j][i].Y,
                        X = data.oPoints[i].X,
                        Y = data.oPoints[i].Y,
                        Z = data.oPoints[i].Z;
                    B[2 * i,0] = X;
                    B[2 * i,1] = Y;
                    B[2 * i,2] = Z;
                    B[2 * i,3] = 1;
                    B[2 * i,8] = -x*X;
                    B[2 * i,9] = -x*Y;
                    B[2 * i,10] = -x*Z;
                    B[2 * i + 1,4] = X;
                    B[2 * i + 1,5] = Y;
                    B[2 * i + 1,6] = Z;
                    B[2 * i + 1,7] = 1;
                    B[2 * i + 1,8] = -y*X;
                    B[2 * i + 1,9] = -y*Y;
                    B[2 * i + 1,10] = -y*Z;
                    l[2 * i,0] = x;
                    l[2 * i + 1,0] =  y;
                }
                int xCount = 12;//未知数个数
                Matrix L0 = ((B.T()*B).Inverse()*(B.T()*l));
                Matrix L=new Matrix(xCount, 1);
                for (int i = 0; i < xCount-1; i++)
                    L[i,0] = L0[i,0];
                Matrix M=new Matrix(count * 2, xCount);
                Matrix W=new Matrix(count * 2, 1);
                double f = 9999;

                //迭代求解L系数
                while (abs(f - inE[j].fx) >= 0.01)
                {
                    f = inE[j].fx;
                    double x0 = (L[0,0] * L[8,0] + L[1,0] * L[9,0] + L[2,0] * L[10,0]) /
                        (pow2(L[8,0]) + pow2(L[9,0]) + pow2(L[10,0])),
                        y0 = (L[4,0] * L[8,0] + L[5,0] * L[9,0] + L[6,0] * L[10,0]) /
                        (pow2(L[8,0]) + pow2(L[9,0]) + pow2(L[10,0]));
                    for (int i = 0; i < data.oCount; i++)
                    {
                        double x = data.oIPoints[j][i].X,
                               y = data.oIPoints[j][i].Y,
                               X = data.oPoints[i].X,
                               Y = data.oPoints[i].Y,
                               Z = data.oPoints[i].Z;
                        double A = X*L[8,0] +Y*L[9,0] +Z*L[10,0] + 1;
                        double r_2 = (x - x0)*(x - x0) + (y - y0)*(y - y0);
                        M[2 * i,0] = X / A;
                        M[2 * i,1] = Y / A;
                        M[2 * i,2] = Z / A;
                        M[2 * i,3] = 1 / A;
                        M[2 * i,8] = -X*x / A;
                        M[2 * i,9] = -Y*x / A;
                        M[2 * i,10] = -Z*x / A;
                        M[2 * i,11] = -(x - x0)*r_2;
                        M[2 * i + 1,4] = X / A;
                        M[2 * i + 1,5] = Y / A;
                        M[2 * i + 1,6] = Z / A;
                        M[2 * i + 1,7] = 1 / A;
                        M[2 * i + 1,8] = -X*y / A;
                        M[2 * i + 1,9] = -Y*y / A;
                        M[2 * i + 1,10] = -Z*y / A;
                        M[2 * i + 1,11] = -(y - y0)*r_2;
                        W[2 * i,0] = x / A;
                        W[2 * i + 1,0] = y / A;
                    }
                    Matrix nL = (M.T()*M).Inverse()*M.T()*W;
                    double dbeta, ds, fx, fy,Xs,Ys,Zs,a3,b3,c3,b1,b2;
                    double gama3 = 1 / sqrt(pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0]));
                    x0 = (nL[0,0] * nL[8,0] + nL[1,0] * nL[9,0] + nL[2,0] * nL[10,0]) /
                        (pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0]));
                    y0 = (nL[4,0] * nL[8,0] + nL[5,0] * nL[9,0] + nL[6,0] * nL[10,0]) /
                        (pow2(nL[8,0]) + pow2(nL[9,0]) + pow2(nL[10,0]));
                    double At = gama3*gama3*(pow2(nL[0,0]) + pow2(nL[1,0]) + pow2(nL[2,0])) - x0*x0,
                        Bt = gama3*gama3*(pow2(nL[4,0]) + pow2(nL[5,0]) + pow2(nL[6,0])) - y0*y0,
                        Ct = gama3*gama3*(nL[0,0] * nL[4,0] + nL[1,0] * nL[5,0] + nL[2,0] * nL[6,0]) - x0*y0;
                    if (Ct >= 0)
                    {
                        dbeta = -asin(sqrt(Ct*Ct / At / Bt));
                    }
                    else
                    {
                        dbeta = asin(sqrt(Ct*Ct / At / Bt));
                    }
                    ds = sqrt(At / Bt) - 1;
                    fx = sqrt((At*Bt - Ct*Ct) / Bt);
                    fy = sqrt((At*Bt - Ct*Ct) / At);
//.........这里部分代码省略.........