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