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);
}
/// <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.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);
}
}
//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);
}
private void simpleButton1_Click(object sender, EventArgs e)
{
DataTable posDt = this.ucPosPair1.GetPosPair();
if (posDt == null || posDt.Rows.Count < 3)
{
MessageBox.Show("请输入至少三组数据");
return;
}
CoordTrans7Param calparam = new CoordTrans7Param();
//double[,] souPos = new double[9, 1] { { 111.11},{ 39.12}, {12 }, { 118.345}, {40.123},{ 4 }, { 123.111}, {24.334},{ 2 } };
//double[,] tarPos = new double[9, 1] { { 111.22}, {39.16}, {12 }, { 118.123}, {40.345},{ 6 }, { 123.104}, {24.304},{ 8 } };
double[,] souPos = new double[posDt.Rows.Count * 3, 1];
double[,] tarPos = new double[posDt.Rows.Count * 3, 1];
double sx, sy, sz, tx, ty, tz;
for (int i = 0; i < posDt.Rows.Count; i++)
{
if (!double.TryParse(posDt.Rows[i]["SOU_X"].ToString(), out sx))
{
sx = 0;
}
if (!double.TryParse(posDt.Rows[i]["SOU_Y"].ToString(), out sy))
{
sy = 0;
}
if (!double.TryParse(posDt.Rows[i]["SOU_Z"].ToString(), out sz))
{
sz = 0;
}
if (!double.TryParse(posDt.Rows[i]["TAR_X"].ToString(), out tx))
{
tx = 0;
}
if (!double.TryParse(posDt.Rows[i]["TAR_Y"].ToString(), out ty))
{
ty = 0;
}
if (!double.TryParse(posDt.Rows[i]["TAR_Z"].ToString(), out tz))
{
tz = 0;
}
souPos[i * 3, 0] = sx;
souPos[i * 3 + 1, 0] = sy;
souPos[i * 3 + 2, 0] = sz;
tarPos[i * 3, 0] = tx;
tarPos[i * 3 + 1, 0] = ty;
tarPos[i * 3 + 2, 0] = tz;
}
double result = calparam.CalculateTrans7Param(souPos, tarPos);
this._trancParamClass = new CoordTrancParamClass();
this._trancParamClass.DX = calparam.dx;
this._trancParamClass.DY = calparam.dy;
this._trancParamClass.DZ = calparam.dz;
this._trancParamClass.DS = calparam.k;
this._trancParamClass.RX = calparam.rx;
this._trancParamClass.RY = calparam.ry;
this._trancParamClass.RZ = calparam.rz;
this._trancParamClass.Accuracy = result;
this.memoEdit1.Text = string.Format("dx:{0} dy:{1} dz:{2}\r\nrx:{3} ry:{4} rz:{5}\r\nds:{6}"
, calparam.dx, calparam.dy, calparam.dz, calparam.rx, calparam.ry, calparam.rz, calparam.k);
MessageBox.Show("参数计算完成");
}
