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("参数计算完成"); }