///// <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; }
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]; }