/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@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; //最適化計算の閾値を決定 double err_threshold = makeErrThreshold(i_square.sqvertex); //平行移動量計算機に、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); //回転行列を計算 NyARRotMatrix rot = this._rotmatrix; rot.initRotByPrevResult(i_prev_result); //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //現在のエラーレートを計算 double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //エラーレートの判定 if (min_err < i_prev_err + err_threshold) { //save initial result o_result.setValue(rot, trans); // System.out.println("TR:ok"); //最適化してみる。 for (int i = 0; i < 5; i++) { //変換行列の最適化 this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4); double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //System.out.println("E:"+err); if (min_err - err < err_threshold / 2) { //System.out.println("BREAK"); break; } this._transsolver.solveTransportVector(vertex_3d, trans); o_result.setValue(rot, trans); min_err = err; } //継続計算成功 if (o_param != null) { o_param.last_error = min_err; } return(true); } //継続計算失敗 return(false); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 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; double err_threshold = makeErrThreshold(i_square.sqvertex); 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; } //平行移動量計算機に、2D座標系をセット 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, err_threshold, o_result); //必要なら計算パラメータを返却 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) { Debug.Assert(num >= 4); double err0 = 0, err1; NyARIcpUtils.U u = this.__u; NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__E = new double[num]; this.__E2 = new double[num]; this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; double[] E = this.__E; double[] E2 = this.__E2; NyARDoublePoint2d[] du = this.__du; NyARDoubleMatrix44 matXw2U = this.__matXw2U; int inlierNum = (int)(num * this.getInlierProbability()) - 1; if (inlierNum < 3) { inlierNum = 3; } o_matxw2xc.setValue(initMatXw2Xc); for (int i = 0;; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); 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; du[j].x = dx; du[j].y = dy; E[j] = E2[j] = dx * dx + dy * dy; } Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE); double K2 = E2[inlierNum] * K2_FACTOR; if (K2 < 16.0) { K2 = 16.0; } err1 = 0.0; for (int j = 0; j < num; j++) { if (E2[j] > K2) { err1 += K2 / 6.0; } else { err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2)); } } err1 /= num; if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this._maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if (E[j] <= K2) { double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2); if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W)) { return false; } } } if (jus.getLength() < 3) { return false; } if (!dS.setJusArray(jus)) { return false; } dS.makeMat(o_matxw2xc); } if(o_result_param!=null){ o_result_param.last_error=err1; } return true; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@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; //最適化計算の閾値を決定 double err_threshold = makeErrThreshold(i_square.sqvertex); //平行移動量計算機に、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); //回転行列を計算 NyARRotMatrix rot = this._rotmatrix; rot.initRotByPrevResult(i_prev_result); //回転後の3D座標系から、平行移動量を計算 NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d; rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //現在のエラーレートを計算 double min_err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //エラーレートの判定 if(min_err<i_prev_err+err_threshold){ //save initial result o_result.setValue(rot,trans); // System.out.println("TR:ok"); //最適化してみる。 for (int i = 0; i < 5; i++) { //変換行列の最適化 this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4); double err = errRate(rot, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //System.out.println("E:"+err); if (min_err - err < err_threshold / 2) { //System.out.println("BREAK"); break; } this._transsolver.solveTransportVector(vertex_3d, trans); o_result.setValue(rot, trans); min_err = err; } //継続計算成功 if (o_param != null) { o_param.last_error = min_err; } return true; } //継続計算失敗 return false; }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 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; double err_threshold = makeErrThreshold(i_square.sqvertex); 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; } //平行移動量計算機に、2D座標系をセット 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, err_threshold, o_result); //必要なら計算パラメータを返却 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) { Debug.Assert(num >= 4); double err0 = 0, err1; NyARIcpUtils.U u = this.__u; NyARIcpUtils.DeltaS dS = this.__dS; //ワークオブジェクトのリセット if (this.__jus.getArraySize() < num) { this.__jus = new NyARIcpUtils.JusStack(num); this.__E = new double[num]; this.__E2 = new double[num]; this.__du = NyARDoublePoint2d.createArray(num); } NyARIcpUtils.JusStack jus = this.__jus; double[] E = this.__E; double[] E2 = this.__E2; NyARDoublePoint2d[] du = this.__du; NyARDoubleMatrix44 matXw2U = this.__matXw2U; int inlierNum = (int)(num * this.getInlierProbability()) - 1; if (inlierNum < 3) { inlierNum = 3; } o_matxw2xc.setValue(initMatXw2Xc); for (int i = 0;; i++) { matXw2U.mul(this._ref_matXc2U, o_matxw2xc); 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; du[j].x = dx; du[j].y = dy; E[j] = E2[j] = dx * dx + dy * dy; } Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE); double K2 = E2[inlierNum] * K2_FACTOR; if (K2 < 16.0) { K2 = 16.0; } err1 = 0.0; for (int j = 0; j < num; j++) { if (E2[j] > K2) { err1 += K2 / 6.0; } else { err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2)); } } err1 /= num; if (err1 < this.breakLoopErrorThresh) { break; } if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh) { break; } if (i == this._maxLoop) { break; } err0 = err1; jus.clear(); for (int j = 0; j < num; j++) { if (E[j] <= K2) { double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2); if (!jus.push(this._ref_matXc2U, o_matxw2xc, worldCoord[j], du[j], W)) { return(false); } } } if (jus.getLength() < 3) { return(false); } if (!dS.setJusArray(jus)) { return(false); } dS.makeMat(o_matxw2xc); } if (o_result_param != null) { o_result_param.last_error = err1; } return(true); }