/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@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); }
/** * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。 * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。 * @see INyARTransMat#transMatContinue */ public bool transMatContinue(NyARSquare i_square, NyARRectOffset i_offset, NyARTransMatResult i_prev_result, NyARTransMatResult o_result) { NyARDoublePoint3d trans = this.__transMat_trans; // io_result_convが初期値なら、transMatで計算する。 if (!i_prev_result.has_value) { this.transMat(i_square, i_offset, o_result); return(true); } //過去のエラーレートを記録(ここれやるのは、i_prev_resultとo_resultに同じインスタンスを指定できるようにするため) double last_error = i_prev_result.last_error; //最適化計算の閾値を決定 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(this._rotmatrix, trans, i_offset.vertex, vertex_2d, 4, vertex_3d); //結果をストア o_result.setValue(rot, trans, min_err); //エラーレートの判定 if (min_err < last_error + err_threshold) { // 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, err); min_err = err; } } else { // System.out.println("TR:again"); //回転行列を計算 rot.initRotBySquare(i_square.line, i_square.sqvertex); //回転後の3D座標系から、平行移動量を計算 rot.getPoint3dBatch(i_offset.vertex, vertex_3d, 4); this._transsolver.solveTransportVector(vertex_3d, trans); //計算結果の最適化(平行移動量と回転行列の最適化) this.optimize(rot, trans, this._transsolver, i_offset.vertex, vertex_2d, err_threshold, o_result); } return(true); }