/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。 * @see INyARTransMat#transMatContinue */ public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { NyARDoublePoint3d trans = this.__transMat_trans; //平行移動量計算機に、2D座標系をセット NyARDoublePoint2d[] vertex_2d; if (this._ref_dist_factor != null) { //歪み復元必要 vertex_2d = this.__transMat_vertex_2d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); } else { //歪み復元は不要 vertex_2d = i_square.sqvertex; } this._transsolver.set2dVertex(vertex_2d, 4); NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; //回転行列を計算 this._rotmatrix.initRotByPrevResult(i_prev_result); //回転後の3D座標系から、平行移動量を計算 this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d); // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); // エラー値が許容範囲でなければTransMatをやり直し if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { return(false); } // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); //エラー値保存 if (o_param != null) { o_param.last_error = err; } return(true); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * ARToolKitのarGetTransMatに該当します。 * @see INyARTransMat#transMatContinue */ public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { NyARDoublePoint3d trans = this.__transMat_trans; //平行移動量計算機に、2D座標系をセット NyARDoublePoint2d[] vertex_2d; if (this._ref_dist_factor != null) { //歪み復元必要 vertex_2d = this.__transMat_vertex_2d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); } else { //歪み復元は不要 vertex_2d = i_square.sqvertex; } this._transsolver.set2dVertex(vertex_2d, 4); //回転行列を計算 if (!this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex)) { return(false); } //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d); // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); if (o_param != null) { o_param.last_error = err; } return(true); }
public override bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param) { double err0 = 0, err1; System.Diagnostics.Debug.Assert(num >= 4); NyARIcpUtils.DeltaS dS = this.__dS; NyARIcpUtils.U u = this.__u; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; NyARDoublePoint2d[] du = this.__du; o_matxw2xc.setValue(initMatXw2Xc); double breakLoopErrorThresh = this.getBreakLoopErrorThresh(); double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2(); double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh(); double maxLoop = this.getMaxLoop(); NyARDoubleMatrix44 matXw2U = this.__matXw2U; for (int i = 0; ; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); err1 = 0.0; for (int j = 0; j < num; j++) { if (!u.setXbyMatX2U(matXw2U, worldCoord[j])) { return false; } double dx = screenCoord[j].x - u.x; double dy = screenCoord[j].y - u.y; err1 += dx * dx + dy * dy; du[j].x = dx; du[j].y = dy; } err1 /= num; if (err1 < breakLoopErrorThresh) { break; } if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh)) { break; } if (i == maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],1.0)) { return false; } } if (!dS.setJusArray(jus)) { return false; } dS.makeMat(o_matxw2xc); } if (o_result_param != null) { o_result_param.last_error = err1; } // *err = err1; return true; }
public void getMatXc2U(NyARDoubleMatrix44 o_ret) { o_ret.setValue(this._ref_matXc2U); }
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l, NyARDoublePoint3d[] worldCoord_l, int num_l, NyARDoublePoint2d[] screenCoord_r, NyARDoublePoint3d[] worldCoord_r, int num_r, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc) { System.Diagnostics.Debug.Assert(num_l + num_r >= 3); double err0 = 0, err1; // 6*2*num? NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num_l + num_r) { this.__jus = new NyARIcpUtils.JusStack(num_l + num_r); this.__du = NyARDoublePoint2d.createArray(num_l + num_r); } NyARIcpUtils.JusStack jus = this.__jus; NyARDoublePoint2d[] du = this.__du; NyARIcpUtils.U u = this.__u; NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44(); matXw2Xc.setValue(initMatXw2Xc); matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L); matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R); for (int i = 0; ; i++) { matXw2Ul.mul(matXc2Ul, matXw2Xc); matXw2Ur.mul(matXc2Ur, matXw2Xc); err1 = 0.0; for (int j = 0; j < num_l; j++) { if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j])) { return false; } double dx = screenCoord_l[j].x - u.x; double dy = screenCoord_l[j].y - u.y; err1 += dx * dx + dy * dy; du[j].x = dx; du[j].y = dy; } for (int j = 0; j < num_r; j++) { if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j])) { return false; } double dx = screenCoord_r[j].x - u.x; double dy = screenCoord_r[j].y - u.y; err1 += dx * dx + dy * dy; du[j + num_l].x = dx; du[j + num_l].y = dy; } err1 /= (num_l + num_r); if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this.maxLoop) { break; } err0 = err1; for (int j = 0; j < num_l; j++) { if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0)) { return false; } } for (int j = 0; j < num_r; j++) { if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0)) { return false; } } if (!dS.setJusArray(jus)) { return false; } dS.makeMat(matXw2Xc); } return false; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。 * @see INyARTransMat#transMatContinue */ public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 i_prev_result, double i_prev_err, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { NyARDoublePoint3d trans = this.__transMat_trans; //平行移動量計算機に、2D座標系をセット NyARDoublePoint2d[] vertex_2d; if (this._ref_dist_factor != null) { //歪み復元必要 vertex_2d = this.__transMat_vertex_2d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); } else { //歪み復元は不要 vertex_2d = i_square.sqvertex; } this._transsolver.set2dVertex(vertex_2d, 4); NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; //回転行列を計算 this._rotmatrix.initRotByPrevResult(i_prev_result); //回転後の3D座標系から、平行移動量を計算 this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d); // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); // エラー値が許容範囲でなければTransMatをやり直し if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { return false; } // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); //エラー値保存 if (o_param != null) { o_param.last_error = err; } return true; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * ARToolKitのarGetTransMatに該当します。 * @see INyARTransMat#transMatContinue */ public bool transMat(NyARSquare i_square, NyARRectOffset i_offset, NyARDoubleMatrix44 o_result, NyARTransMatResultParam o_param) { NyARDoublePoint3d trans = this.__transMat_trans; //平行移動量計算機に、2D座標系をセット NyARDoublePoint2d[] vertex_2d; if (this._ref_dist_factor != null) { //歪み復元必要 vertex_2d = this.__transMat_vertex_2d; this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d, 4); } else { //歪み復元は不要 vertex_2d = i_square.sqvertex; } this._transsolver.set2dVertex(vertex_2d, 4); //回転行列を計算 if (!this._rotmatrix.initRotBySquare(i_square.line, i_square.sqvertex)) { return false; } //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; this._rotmatrix.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) double err = this.optimize(this._rotmatrix, trans, this._transsolver, i_offset.vertex, vertex_2d); // マトリクスの保存 o_result.setValue(this._rotmatrix, trans); if (o_param != null) { o_param.last_error = err; } return true; }
static void Main(string[] args) { NyARDoubleMatrix44 DEST_MAT = new NyARDoubleMatrix44( new double[] { 0.9832165682361184, 0.004789697223621061, -0.18237945710280384, -190.59060790299358, 0.012860184615056927, -0.9989882709616935, 0.04309419210331572, 64.04490277502563, -0.18198852802987958, -0.044716355753573425, -0.9822833548209547, 616.6427596804766, 0, 0, 0, 1 }); NyARDoubleMatrix44 SRC_MAT = new NyARDoubleMatrix44(new double[] { 0.984363556, 0.00667689135, -0.176022261, -191.179672, 0.0115975942, -0.999569774, 0.0269410834, 63.0028076, -0.175766647, -0.0285612550, -0.984017432, 611.758728, 0, 0, 0, 1 }); String img_file = "../../../../../data/testcase/test.raw"; String cparam = "../../../../../data/testcase/camera_para5.dat"; String fsetfile = "../../../../../data/testcase/pinball.fset"; String isetfile = "../../../../../data/testcase/pinball.iset5"; //カメラパラメータ NyARParam param = NyARParam.loadFromARParamFile(File.OpenRead(cparam), 640, 480, NyARParam.DISTFACTOR_LT_ARTK5); INyARGrayscaleRaster gs = NyARGrayscaleRaster.createInstance(640, 480); //試験画像の準備 { INyARRgbRaster rgb = NyARRgbRaster.createInstance(640, 480, NyARBufferType.BYTE1D_B8G8R8X8_32); Stream fs = File.OpenRead(img_file); byte[] b = (byte[])rgb.getBuffer(); fs.Read(b, 0, b.Length); INyARRgb2GsFilterRgbAve filter = (INyARRgb2GsFilterRgbAve)rgb.createInterface(typeof(INyARRgb2GsFilterRgbAve)); filter.convert(gs); } NyARNftFsetFile fset = NyARNftFsetFile.loadFromFsetFile(File.OpenRead(fsetfile)); NyARNftIsetFile iset = NyARNftIsetFile.loadFromIsetFile(File.OpenRead(isetfile)); NyARSurfaceTracker st = new NyARSurfaceTracker(param, 16, 0.5); NyARSurfaceDataSet sd = new NyARSurfaceDataSet(iset, fset); NyARDoubleMatrix44 sret = new NyARDoubleMatrix44(); NyARDoublePoint2d[] o_pos2d = NyARDoublePoint2d.createArray(16); NyARDoublePoint3d[] o_pos3d = NyARDoublePoint3d.createArray(16); NyARSurfaceTrackingTransmatUtils tmat = new NyARSurfaceTrackingTransmatUtils(param, 5.0); NyARDoubleMatrix44 tret = new NyARDoubleMatrix44(); for (int j = 0; j < 10; j++) { Stopwatch s = new Stopwatch(); s.Reset(); s.Start(); for (int i = 0; i < 3000; i++) { sret.setValue(SRC_MAT); int nop = st.tracking(gs, sd, sret, o_pos2d, o_pos3d, 16); //Transmatの試験 NyARDoublePoint3d off = NyARSurfaceTrackingTransmatUtils.centerOffset(o_pos3d, nop, new NyARDoublePoint3d()); NyARSurfaceTrackingTransmatUtils.modifyInputOffset(sret, o_pos3d, nop, off); tmat.surfaceTrackingTransmat(sret, o_pos2d, o_pos3d, nop, tret, new NyARTransMatResultParam()); NyARSurfaceTrackingTransmatUtils.restoreOutputOffset(tret, off); System.Console.WriteLine(tret.Equals(DEST_MAT)); } s.Stop(); System.Console.WriteLine(s.ElapsedMilliseconds); } return; }
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l, NyARDoublePoint3d[] worldCoord_l, int num_l, NyARDoublePoint2d[] screenCoord_r, NyARDoublePoint3d[] worldCoord_r, int num_r, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc) { System.Diagnostics.Debug.Assert(num_l + num_r >= 3); double err0 = 0, err1; // 6*2*num? NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num_l + num_r) { this.__jus = new NyARIcpUtils.JusStack(num_l + num_r); this.__du = NyARDoublePoint2d.createArray(num_l + num_r); } NyARIcpUtils.JusStack jus = this.__jus; NyARDoublePoint2d[] du = this.__du; NyARIcpUtils.U u = this.__u; NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44(); NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44(); matXw2Xc.setValue(initMatXw2Xc); matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L); matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R); for (int i = 0; ; i++) { matXw2Ul.mul(matXc2Ul, matXw2Xc); matXw2Ur.mul(matXc2Ur, matXw2Xc); err1 = 0.0; for (int j = 0; j < num_l; j++) { if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j])) { return(false); } double dx = screenCoord_l[j].x - u.x; double dy = screenCoord_l[j].y - u.y; err1 += dx * dx + dy * dy; du[j].x = dx; du[j].y = dy; } for (int j = 0; j < num_r; j++) { if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j])) { return(false); } double dx = screenCoord_r[j].x - u.x; double dy = screenCoord_r[j].y - u.y; err1 += dx * dx + dy * dy; du[j + num_l].x = dx; du[j + num_l].y = dy; } err1 /= (num_l + num_r); if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this.maxLoop) { break; } err0 = err1; for (int j = 0; j < num_l; j++) { if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0)) { return(false); } } for (int j = 0; j < num_r; j++) { if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0)) { return(false); } } if (!dS.setJusArray(jus)) { return(false); } dS.makeMat(matXw2Xc); } return(false); }
public override bool icpPoint(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int num, NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param) { double err0 = 0, err1; System.Diagnostics.Debug.Assert(num >= 4); NyARIcpUtils.DeltaS dS = this.__dS; NyARIcpUtils.U u = this.__u; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; NyARDoublePoint2d[] du = this.__du; o_matxw2xc.setValue(initMatXw2Xc); double breakLoopErrorThresh = this.getBreakLoopErrorThresh(); double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2(); double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh(); double maxLoop = this.getMaxLoop(); NyARDoubleMatrix44 matXw2U = this.__matXw2U; for (int i = 0; ; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); err1 = 0.0; for (int j = 0; j < num; j++) { if (!u.setXbyMatX2U(matXw2U, worldCoord[j])) { return(false); } double dx = screenCoord[j].x - u.x; double dy = screenCoord[j].y - u.y; err1 += dx * dx + dy * dy; du[j].x = dx; du[j].y = dy; } err1 /= num; if (err1 < breakLoopErrorThresh) { break; } if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh)) { break; } if (i == maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if (!jus.push(this._ref_matXc2U, o_matxw2xc, worldCoord[j], du[j], 1.0)) { return(false); } } if (!dS.setJusArray(jus)) { return(false); } dS.makeMat(o_matxw2xc); } if (o_result_param != null) { o_result_param.last_error = err1; } // *err = err1; return(true); }