\r
// NyARToolkitの準備\r
this._nya = new NyARSingleDetectMarker(this._ar_param, i_ar_code, 80.0,this._cap_image.getBufferReader().getBufferType());\r
- this._nya.setContinueMode(true);// ここをtrueにすると、transMatContinueモード（History計算）になります。\r
+ this._nya.setContinueMode(false);// ここをtrueにすると、transMatContinueモード（History計算）になります。\r
\r
// 3Dを描画するコンポーネント\r
GLCanvas canvas = new GLCanvas();\r
*/\r
package jp.nyatla.nyartoolkit.jogl.sample;\r
\r
-import jp.nyatla.nyartoolkit.nyidmarker.data.*;\r
\r
import java.awt.event.*;\r
import java.awt.*;\r
}\r
io_rot.setAngle((int)a2,(int)b2,(int)c2);\r
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
- return minerr /4;\r
+ return minerr /4;//この設定値おかしくね？16bitfixedfloatなら16で割らないと。\r
} \r
\r
\r
{\r
NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
double err = -1;\r
- /* ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。 */\r
+ // ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。 \r
for (int i = 0;; i++) {\r
// <arGetTransMat3>\r
err = this._mat_optimize.modifyMatrix(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex);\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit (Extension)\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU Lesser General Public License as \r
+ * published by the Free Software Foundation; either version 3 of the\r
+ * License, or (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful, \r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
+ * See the GNU Lesser General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU Lesser General Public \r
+ * License along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ */\r
+package jp.nyatla.nyartoolkit.core.transmat;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.squaredetect.NyARSquare;\r
+import jp.nyatla.nyartoolkit.core.transmat.solver.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.optimize.NyARPartialDifferentiationOptimize;\r
+import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+\r
+/**\r
+ * This class calculates ARMatrix from square information and holds it. --\r
+ * 変換行列を計算して、結果を保持するクラス。\r
+ * \r
+ */\r
+public class NyARTransMat_NyARToolkit implements INyARTransMat\r
+{\r
+ private final static double FIT_DIFF_THRESHOLD = 0.01;\r
+ private final static double FIT_DIFF_THRESHOLD_CONT = 1.0;\r
+\r
+ private final NyARDoublePoint2d _center=new NyARDoublePoint2d(0,0);\r
+ private final NyARTransOffset _offset=new NyARTransOffset();\r
+ private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+ protected NyARRotMatrix _rotmatrix;\r
+ protected INyARTransportVectorSolver _transsolver;\r
+ protected NyARPartialDifferentiationOptimize _mat_optimize;\r
+\r
+\r
+ private NyARCameraDistortionFactor _ref_dist_factor;\r
+\r
+ /**\r
+ * 派生クラスで自分でメンバオブジェクトを指定したい場合はこちらを使う。\r
+ *\r
+ */\r
+ protected NyARTransMat_NyARToolkit()\r
+ {\r
+ //_calculator,_rotmatrix,_mat_optimizeをコンストラクタの終了後に\r
+ //作成して割り当ててください。\r
+ return;\r
+ }\r
+ public NyARTransMat_NyARToolkit(NyARParam i_param) throws NyARException\r
+ {\r
+ final NyARCameraDistortionFactor dist=i_param.getDistortionFactor();\r
+ final NyARPerspectiveProjectionMatrix pmat=i_param.getPerspectiveProjectionMatrix();\r
+ this._transsolver=new NyARTransportVectorSolver_ARToolKit(pmat);\r
+ //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。\r
+ //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。\r
+ this._rotmatrix = new NyARRotMatrix_NyARToolKit(pmat);\r
+ this._mat_optimize=new NyARPartialDifferentiationOptimize(pmat);\r
+ this._ref_dist_factor=dist;\r
+ this._projection_mat_ref=pmat;\r
+ }\r
+\r
+ public void setCenter(double i_x, double i_y)\r
+ {\r
+ this._center.x= i_x;\r
+ this._center.y= i_y;\r
+ }\r
+\r
+\r
+\r
+\r
+ /**\r
+ * 頂点順序をi_directionに対応して並べ替えます。\r
+ * @param i_square\r
+ * @param i_direction\r
+ * @param o_sqvertex_ref\r
+ * @param o_liner_ref\r
+ */\r
+ private final void initVertexOrder(NyARSquare i_square, int i_direction, NyARDoublePoint2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref)\r
+ {\r
+ //頂点順序を考慮した矩形の頂点情報\r
+ o_sqvertex_ref[0]= i_square.sqvertex[(4 - i_direction) % 4];\r
+ o_sqvertex_ref[1]= i_square.sqvertex[(5 - i_direction) % 4];\r
+ o_sqvertex_ref[2]= i_square.sqvertex[(6 - i_direction) % 4];\r
+ o_sqvertex_ref[3]= i_square.sqvertex[(7 - i_direction) % 4]; \r
+ o_liner_ref[0]=i_square.line[(4 - i_direction) % 4];\r
+ o_liner_ref[1]=i_square.line[(5 - i_direction) % 4];\r
+ o_liner_ref[2]=i_square.line[(6 - i_direction) % 4];\r
+ o_liner_ref[3]=i_square.line[(7 - i_direction) % 4];\r
+ return;\r
+ }\r
+\r
+\r
+ private final NyARDoublePoint2d[] __transMat_sqvertex_ref = new NyARDoublePoint2d[4];\r
+ private final NyARDoublePoint2d[] __transMat_vertex_2d = NyARDoublePoint2d.createArray(4);\r
+ private final NyARDoublePoint3d[] __transMat_vertex_3d = NyARDoublePoint3d.createArray(4);\r
+ private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4];\r
+ private final NyARDoublePoint3d __transMat_trans=new NyARDoublePoint3d();\r
+ /**\r
+ * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
+ * \r
+ * @param i_square\r
+ * 計算対象のNyARSquareオブジェクト\r
+ * @param i_direction\r
+ * @param i_width\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public void transMat(final NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException\r
+ {\r
+ final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+ final NyARDoublePoint3d trans=this.__transMat_trans;\r
+ \r
+ //計算用に頂点情報を初期化（順番調整）\r
+ initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref);\r
+ \r
+ //平行移動量計算機に、2D座標系をセット\r
+ NyARDoublePoint2d[] vertex_2d=this.__transMat_vertex_2d;\r
+ NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
+ this._ref_dist_factor.ideal2ObservBatch(sqvertex_ref, vertex_2d,4); \r
+ this._transsolver.set2dVertex(vertex_2d,4);\r
+ \r
+ //基準矩形の3D座標系を作成\r
+ this._offset.setSquare(i_width,this._center);\r
+\r
+ //回転行列を計算\r
+ this._rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
+ \r
+ //回転後の3D座標系から、平行移動量を計算\r
+ this._rotmatrix.getPoint3dBatch(this._offset.vertex,vertex_3d,4);\r
+ this._transsolver.solveTransportVector(vertex_3d,trans);\r
+ \r
+ //計算結果の最適化(平行移動量と回転行列の最適化)\r
+ this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);\r
+ \r
+ // マトリクスの保存\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
+ return;\r
+ }\r
+\r
+ /*\r
+ * (non-Javadoc)\r
+ * @see jp.nyatla.nyartoolkit.core.transmat.INyARTransMat#transMatContinue(jp.nyatla.nyartoolkit.core.NyARSquare, int, double, jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult)\r
+ */\r
+ public void transMatContinue(NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException\r
+ {\r
+ final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+ final NyARDoublePoint3d trans=this.__transMat_trans;\r
+\r
+ // io_result_convが初期値なら、transMatで計算する。\r
+ if (!o_result_conv.has_value) {\r
+ this.transMat(i_square, i_direction, i_width, o_result_conv);\r
+ return;\r
+ }\r
+\r
+ //計算用に頂点情報を初期化（順番調整）\r
+ initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref);\r
+\r
+ \r
+ //平行移動量計算機に、2D座標系をセット\r
+ NyARDoublePoint2d[] vertex_2d=this.__transMat_vertex_2d;\r
+ NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
+ this._ref_dist_factor.ideal2ObservBatch(sqvertex_ref, vertex_2d,4); \r
+ this._transsolver.set2dVertex(vertex_2d,4);\r
+ \r
+ //基準矩形の3D座標系を作成\r
+ this._offset.setSquare(i_width,this._center);\r
+\r
+ //回転行列を計算\r
+ this._rotmatrix.initRotByPrevResult(o_result_conv);\r
+ \r
+ //回転後の3D座標系から、平行移動量を計算\r
+ this._rotmatrix.getPoint3dBatch(this._offset.vertex,vertex_3d,4);\r
+ this._transsolver.solveTransportVector(vertex_3d,trans);\r
+\r
+ //現在のエラーレートを計算しておく\r
+ double min_err=errRate(this._rotmatrix,trans, this._offset.vertex, vertex_2d,4,vertex_3d);\r
+ NyARDoubleMatrix33 rot=this.__rot;\r
+ //エラーレートが閾値超えてたらアゲイン\r
+ if(min_err<FIT_DIFF_THRESHOLD_CONT){\r
+ rot.setValue(this._rotmatrix);\r
+ //最適化してみる。\r
+ for (int i = 0;i<5; i++) {\r
+ //変換行列の最適化\r
+ this._mat_optimize.modifyMatrix(rot, trans, this._offset.vertex, vertex_2d, 4);\r
+ double err=errRate(rot,trans,this._offset.vertex, vertex_2d,4,vertex_3d);\r
+ //System.out.println("E:"+err);\r
+ if(min_err-err<FIT_DIFF_THRESHOLD){\r
+ //System.out.println("BREAK");\r
+ break;\r
+ }\r
+ this._transsolver.solveTransportVector(vertex_3d, trans);\r
+ this._rotmatrix.setValue(rot);\r
+ min_err=err;\r
+ }\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
+ }else{\r
+ //回転行列を計算\r
+ this._rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
+ \r
+ //回転後の3D座標系から、平行移動量を計算\r
+ this._rotmatrix.getPoint3dBatch(this._offset.vertex,vertex_3d,4);\r
+ this._transsolver.solveTransportVector(vertex_3d,trans);\r
+ \r
+ //計算結果の最適化(平行移動量と回転行列の最適化)\r
+ this.optimize(this._rotmatrix, trans, this._transsolver,this._offset.vertex, vertex_2d);\r
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);\r
+ }\r
+ return;\r
+ }\r
+ private NyARDoubleMatrix33 __rot=new NyARDoubleMatrix33();\r
+ private double optimize(NyARRotMatrix io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex) throws NyARException\r
+ {\r
+ //System.out.println("START");\r
+ NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
+ //初期のエラー値を計算\r
+ double min_err=errRate(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);\r
+ NyARDoubleMatrix33 rot=this.__rot;\r
+ rot.setValue(io_rotmat);\r
+ for (int i = 0;i<5; i++) {\r
+ //変換行列の最適化\r
+ this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);\r
+ double err=errRate(rot,io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);\r
+ //System.out.println("E:"+err);\r
+ if(min_err-err<FIT_DIFF_THRESHOLD){\r
+ //System.out.println("BREAK");\r
+ break;\r
+ }\r
+ i_solver.solveTransportVector(vertex_3d, io_transvec);\r
+ io_rotmat.setValue(rot);\r
+ min_err=err;\r
+ }\r
+ //System.out.println("END");\r
+ return min_err;\r
+ }\r
+ \r
+ //エラーレート計算機\r
+ public double errRate(NyARDoubleMatrix33 io_rot,NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d,int i_number_of_vertex,NyARDoublePoint3d[] o_rot_vertex) throws NyARException\r
+ {\r
+ NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
+ final double cp00=cp.m00;\r
+ final double cp01=cp.m01;\r
+ final double cp02=cp.m02;\r
+ final double cp11=cp.m11;\r
+ final double cp12=cp.m12;\r
+\r
+ double err=0;\r
+ for(int i=0;i<i_number_of_vertex;i++){\r
+ double x3d,y3d,z3d;\r
+ o_rot_vertex[i].x=x3d=io_rot.m00*i_vertex3d[i].x+io_rot.m01*i_vertex3d[i].y+io_rot.m02*i_vertex3d[i].z;\r
+ o_rot_vertex[i].y=y3d=io_rot.m10*i_vertex3d[i].x+io_rot.m11*i_vertex3d[i].y+io_rot.m12*i_vertex3d[i].z;\r
+ o_rot_vertex[i].z=z3d=io_rot.m20*i_vertex3d[i].x+io_rot.m21*i_vertex3d[i].y+io_rot.m22*i_vertex3d[i].z;\r
+ x3d+=i_trans.x;\r
+ y3d+=i_trans.y;\r
+ z3d+=i_trans.z;\r
+ \r
+ //射影変換\r
+ double x2d=x3d*cp00+y3d*cp01+z3d*cp02;\r
+ double y2d=y3d*cp11+z3d*cp12;\r
+ double h2d=z3d;\r
+ \r
+ //エラーレート計算\r
+ double t1=i_vertex2d[i].x-x2d/h2d;\r
+ double t2=i_vertex2d[i].y-y2d/h2d;\r
+ err+=t1*t1+t2*t2;\r
+ \r
+ }\r
+ return err/i_number_of_vertex;\r
+ } \r
+ \r
+ \r
+ \r
+ /**\r
+ * パラメータで変換行列を更新します。\r
+ * \r
+ * @param i_rot\r
+ * @param i_off\r
+ * @param i_trans\r
+ */\r
+ public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_off, NyARDoublePoint3d i_trans,NyARTransMatResult o_result)\r
+ {\r
+ o_result.m00=i_rot.m00;\r
+ o_result.m01=i_rot.m01;\r
+ o_result.m02=i_rot.m02;\r
+ o_result.m03=i_rot.m00 * i_off.x + i_rot.m01 * i_off.y + i_rot.m02 * i_off.z + i_trans.x;\r
+\r
+ o_result.m10 = i_rot.m10;\r
+ o_result.m11 = i_rot.m11;\r
+ o_result.m12 = i_rot.m12;\r
+ o_result.m13 = i_rot.m10 * i_off.x + i_rot.m11 * i_off.y + i_rot.m12 * i_off.z + i_trans.y;\r
+\r
+ o_result.m20 = i_rot.m20;\r
+ o_result.m21 = i_rot.m21;\r
+ o_result.m22 = i_rot.m22;\r
+ o_result.m23 = i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z + i_trans.z;\r
+\r
+ o_result.angle.setValue(i_rot.refAngle());\r
+ o_result.has_value = true;\r
+ return;\r
+ } \r
+}\r
public interface INyARRotMatrixOptimize\r
{\r
/**\r
- * @param io_rotmat\r
+ * @param io_rot\r
* 初期回転行列\r
- * @param io_transvec\r
+ * @param i_trans\r
* 初期並進ベクトル\r
- * @param i_solver\r
- * i_offset_3dを設定したsolver\r
- * @param i_offset_3d\r
- * 3dオフセット座標\r
- * @param i_2d_vertex\r
- * 画面上の点\r
+ * @param i_vertex3d\r
+ * 初期3次元座標\r
+ * @param i_vertex2d\r
+ * 画面上の頂点群\r
* @return\r
+ * エラーレート\r
* @throws NyARException\r
*/\r
// public double optimize(NyARRotMatrix io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex) throws NyARException;\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit (Extension)\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008-2009 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core.transmat.optimize;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core.utils.*;\r
+\r
+class TSinCosValue{\r
+ public double cos_val;\r
+ public double sin_val;\r
+ public static TSinCosValue[] createArray(int i_size)\r
+ {\r
+ TSinCosValue[] result=new TSinCosValue[i_size];\r
+ for(int i=0;i<i_size;i++){\r
+ result[i]=new TSinCosValue();\r
+ }\r
+ return result;\r
+ }\r
+}\r
+\r
+/**\r
+ * 基本姿勢と実画像を一致するように、角度を微調整→平行移動量を再計算 を繰り返して、変換行列を最適化する。\r
+ * \r
+ */\r
+public class NyARPartialDifferentiationOptimize\r
+{\r
+ private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+\r
+ public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
+ {\r
+ this._projection_mat_ref = i_projection_mat_ref;\r
+ return;\r
+ }\r
+\r
+ public final void sincos2Rotation_ZXY(TSinCosValue[] i_sincos, NyARDoubleMatrix33 i_rot_matrix)\r
+ {\r
+ final double sina = i_sincos[0].sin_val;\r
+ final double cosa = i_sincos[0].cos_val;\r
+ final double sinb = i_sincos[1].sin_val;\r
+ final double cosb = i_sincos[1].cos_val;\r
+ final double sinc = i_sincos[2].sin_val;\r
+ final double cosc = i_sincos[2].cos_val;\r
+ i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;\r
+ i_rot_matrix.m01 = -sinc * cosa;\r
+ i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;\r
+ i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;\r
+ i_rot_matrix.m11 = cosc * cosa;\r
+ i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;\r
+ i_rot_matrix.m20 = -cosa * sinb;\r
+ i_rot_matrix.m21 = sina;\r
+ i_rot_matrix.m22 = cosb * cosa;\r
+ }\r
+\r
+ private final void rotation2Sincos_ZXY(NyARDoubleMatrix33 i_rot_matrix, TSinCosValue[] o_out,NyARDoublePoint3d o_ang)\r
+ {\r
+ double x, y, z;\r
+ double sina = i_rot_matrix.m21;\r
+ if (sina >= 1.0) {\r
+ x = Math.PI / 2;\r
+ y = 0;\r
+ z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);\r
+ } else if (sina <= -1.0) {\r
+ x = -Math.PI / 2;\r
+ y = 0;\r
+ z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);\r
+ } else {\r
+ x = Math.asin(sina);\r
+ y = Math.atan2(-i_rot_matrix.m20, i_rot_matrix.m22);\r
+ z = Math.atan2(-i_rot_matrix.m01, i_rot_matrix.m11);\r
+ }\r
+ o_ang.x=x;\r
+ o_ang.y=y;\r
+ o_ang.z=z;\r
+ o_out[0].sin_val = Math.sin(x);\r
+ o_out[0].cos_val = Math.cos(x);\r
+ o_out[1].sin_val = Math.sin(y);\r
+ o_out[1].cos_val = Math.cos(y);\r
+ o_out[2].sin_val = Math.sin(z);\r
+ o_out[2].cos_val = Math.cos(z);\r
+ return;\r
+ }\r
+\r
+ /*\r
+ * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *\r
+ * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;\r
+ * \r
+ * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix\r
+ * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *\r
+ * cosa)*iz+i_trans.z;\r
+ * \r
+ * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina\r
+ * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa\r
+ * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));\r
+ * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa\r
+ * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *\r
+ * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb\r
+ * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *\r
+ * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *\r
+ * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])\r
+ * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2\r
+ * +2*M*cos(x)*sin(x)\r
+ */\r
+ private double optimizeParamX(TSinCosValue i_angle_y, TSinCosValue i_angle_z, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
+ {\r
+ NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
+ final double sinb = i_angle_y.sin_val;\r
+ final double cosb = i_angle_y.cos_val;\r
+ final double sinc = i_angle_z.sin_val;\r
+ final double cosc = i_angle_z.cos_val;\r
+ double L, J, K, M, N, O;\r
+ L = J = K = M = N = O = 0;\r
+ for (int i = 0; i < i_number_of_vertex; i++) {\r
+ double ix, iy, iz;\r
+ ix = i_vertex3d[i].x;\r
+ iy = i_vertex3d[i].y;\r
+ iz = i_vertex3d[i].z;\r
+\r
+ final double cp00 = cp.m00;\r
+ final double cp01 = cp.m01;\r
+ final double cp02 = cp.m02;\r
+ final double cp11 = cp.m11;\r
+ final double cp12 = cp.m12;\r
+\r
+ double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));\r
+ double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));\r
+ double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));\r
+ double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));\r
+ double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));\r
+ double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));\r
+ double H0 = (iy);\r
+ double H1 = (-sinb * ix + cosb * iz);\r
+ double H2 = i_trans.z;\r
+\r
+ double VX = i_vertex2d[i].x;\r
+ double VY = i_vertex2d[i].y;\r
+\r
+ double a, b, c, d, e, f;\r
+ a = (VX * H0 - X0);\r
+ b = (VX * H1 - X1);\r
+ c = (VX * H2 - X2);\r
+ d = (VY * H0 - Y0);\r
+ e = (VY * H1 - Y1);\r
+ f = (VY * H2 - Y2);\r
+\r
+ L += d * e + a * b;\r
+ N += d * d + a * a;\r
+ J += d * f + a * c;\r
+ M += e * e + b * b;\r
+ K += e * f + b * c;\r
+ O += f * f + c * c;\r
+\r
+ }\r
+ L *= 2;\r
+ J = (J + J) / L;\r
+ K = (K + K) / L;\r
+ N /= L;\r
+ M /= L;\r
+ O /= L;\r
+ return getMinimumErrorAngleFromParam(J, K, M, N, O, i_hint_angle);\r
+\r
+ }\r
+\r
+ private double optimizeParamY(TSinCosValue i_angle_x, TSinCosValue i_angle_z, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
+ {\r
+ NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
+ final double sina = i_angle_x.sin_val;\r
+ final double cosa = i_angle_x.cos_val;\r
+ final double sinc = i_angle_z.sin_val;\r
+ final double cosc = i_angle_z.cos_val;\r
+ double L, J, K, M, N, O;\r
+ L = J = K = M = N = O = 0;\r
+ for (int i = 0; i < i_number_of_vertex; i++) {\r
+ double ix, iy, iz;\r
+ ix = i_vertex3d[i].x;\r
+ iy = i_vertex3d[i].y;\r
+ iz = i_vertex3d[i].z;\r
+\r
+ final double cp00 = cp.m00;\r
+ final double cp01 = cp.m01;\r
+ final double cp02 = cp.m02;\r
+ final double cp11 = cp.m11;\r
+ final double cp12 = cp.m12;\r
+\r
+ double X0 = (cp00 * (-sinc * sina * ix + cosc * iz) + cp01 * (cosc * sina * ix + sinc * iz) + cp02 * (-cosa * ix));\r
+ double X1 = (cp01 * (sinc * ix - cosc * sina * iz) + cp00 * (cosc * ix + sinc * sina * iz) + cp02 * (cosa * iz));\r
+ double X2 = (cp00 * (i_trans.x + (-sinc * cosa) * iy) + cp01 * (i_trans.y + (cosc * cosa) * iy) + cp02 * (i_trans.z + (sina) * iy));\r
+ double Y0 = (cp11 * (cosc * sina * ix + sinc * iz) + cp12 * (-cosa * ix));\r
+ double Y1 = (cp11 * (sinc * ix - cosc * sina * iz) + cp12 * (cosa * iz));\r
+ double Y2 = (cp11 * (i_trans.y + (cosc * cosa) * iy) + cp12 * (i_trans.z + (sina) * iy));\r
+ double H0 = (-cosa * ix);\r
+ double H1 = (cosa * iz);\r
+ double H2 = i_trans.z + (sina) * iy;\r
+\r
+ double VX = i_vertex2d[i].x;\r
+ double VY = i_vertex2d[i].y;\r
+\r
+ double a, b, c, d, e, f;\r
+ a = (VX * H0 - X0);\r
+ b = (VX * H1 - X1);\r
+ c = (VX * H2 - X2);\r
+ d = (VY * H0 - Y0);\r
+ e = (VY * H1 - Y1);\r
+ f = (VY * H2 - Y2);\r
+\r
+ L += d * e + a * b;\r
+ N += d * d + a * a;\r
+ J += d * f + a * c;\r
+ M += e * e + b * b;\r
+ K += e * f + b * c;\r
+ O += f * f + c * c;\r
+\r
+ }\r
+ L *= 2;\r
+ J = (J + J) / L;\r
+ K = (K + K) / L;\r
+ N /= L;\r
+ M /= L;\r
+ O /= L;\r
+ return getMinimumErrorAngleFromParam(J, K, M, N, O, i_hint_angle);\r
+ }\r
+\r
+ private double optimizeParamZ(TSinCosValue i_angle_x, TSinCosValue i_angle_y, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
+ {\r
+ NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
+ final double sina = i_angle_x.sin_val;\r
+ final double cosa = i_angle_x.cos_val;\r
+ final double sinb = i_angle_y.sin_val;\r
+ final double cosb = i_angle_y.cos_val;\r
+ double L, J, K, M, N, O;\r
+ L = J = K = M = N = O = 0;\r
+ for (int i = 0; i < i_number_of_vertex; i++) {\r
+ double ix, iy, iz;\r
+ ix = i_vertex3d[i].x;\r
+ iy = i_vertex3d[i].y;\r
+ iz = i_vertex3d[i].z;\r
+\r
+ final double cp00 = cp.m00;\r
+ final double cp01 = cp.m01;\r
+ final double cp02 = cp.m02;\r
+ final double cp11 = cp.m11;\r
+ final double cp12 = cp.m12;\r
+\r
+ double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));\r
+ double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));\r
+ double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);\r
+ double Y0 = cp11 * (ix * cosb + sinb * iz);\r
+ double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);\r
+ double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));\r
+ double H0 = 0;\r
+ double H1 = 0;\r
+ double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);\r
+\r
+ double VX = i_vertex2d[i].x;\r
+ double VY = i_vertex2d[i].y;\r
+\r
+ double a, b, c, d, e, f;\r
+ a = (VX * H0 - X0);\r
+ b = (VX * H1 - X1);\r
+ c = (VX * H2 - X2);\r
+ d = (VY * H0 - Y0);\r
+ e = (VY * H1 - Y1);\r
+ f = (VY * H2 - Y2);\r
+\r
+ L += d * e + a * b;\r
+ N += d * d + a * a;\r
+ J += d * f + a * c;\r
+ M += e * e + b * b;\r
+ K += e * f + b * c;\r
+ O += f * f + c * c;\r
+\r
+ }\r
+ L *= 2;\r
+ J = (J + J) / L;\r
+ K = (K + K) / L;\r
+ N /= L;\r
+ M /= L;\r
+ O /= L;\r
+ return getMinimumErrorAngleFromParam(J, K, M, N, O, i_hint_angle);\r
+ }\r
+ private TSinCosValue[] __angles_in=TSinCosValue.createArray(3);\r
+ private NyARDoublePoint3d __ang=new NyARDoublePoint3d();\r
+ public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) throws NyARException\r
+ {\r
+ TSinCosValue[] angles_in = this.__angles_in;// x,y,z\r
+ NyARDoublePoint3d ang = this.__ang;\r
+\r
+ // ZXY系のsin/cos値を抽出\r
+ rotation2Sincos_ZXY(io_rot, angles_in,ang);\r
+ ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, 4, ang.x);\r
+ ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, 4, ang.y);\r
+ ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, 4, ang.z);\r
+ io_rot.setZXYAngle(ang.x, ang.y, ang.z);\r
+ return;\r
+ }\r
+ private double[] __sin_table= new double[4];\r
+ /**\r
+ * エラーレートが最小になる点を得る。\r
+ */\r
+ private double getMinimumErrorAngleFromParam(double iJ, double iK, double iM, double iN, double iO, double i_hint_angle) throws NyARException\r
+ {\r
+ double[] sin_table = this.__sin_table;\r
+\r
+ double M = iN - iM;\r
+ double J = iJ;\r
+ double K = -iK;\r
+\r
+ // パラメータからsinテーブルを作成\r
+ // (- 4*M^2-4)*x^4 + (4*K- 4*J*M)*x^3 + (4*M^2 -(K^2- 4)- J^2)*x^2 +(4*J*M- 2*K)*x + J^2-1 = 0\r
+ int number_of_sin = NyAREquationSolver.solve4Equation(-4 * M * M - 4, 4 * K - 4 * J * M, 4 * M * M - (K * K - 4) - J * J, 4 * J * M - 2 * K, J * J - 1, sin_table);\r
+\r
+\r
+ // 最小値２個を得ておく。\r
+ double min_ang_0 = Double.MAX_VALUE;\r
+ double min_ang_1 = Double.MAX_VALUE;\r
+ double min_err_0 = Double.MAX_VALUE;\r
+ double min_err_1 = Double.MAX_VALUE;\r
+ for (int i = 0; i < number_of_sin; i++) {\r
+ // +-cos_v[i]が頂点候補\r
+ double sin_rt = sin_table[i];\r
+ double cos_rt = Math.sqrt(1 - (sin_rt * sin_rt));\r
+ // cosを修復。微分式で0に近い方が正解\r
+ // 0 = 2*cos(x)*sin(x)*M - sin(x)^2 + cos(x)^2 + sin(x)*K + cos(x)*J\r
+ double a1 = 2 * cos_rt * sin_rt * M + sin_rt * (K - sin_rt) + cos_rt * (cos_rt + J);\r
+ double a2 = 2 * (-cos_rt) * sin_rt * M + sin_rt * (K - sin_rt) + (-cos_rt) * ((-cos_rt) + J);\r
+ // 絶対値になおして、真のcos値を得ておく。\r
+ a1 = a1 < 0 ? -a1 : a1;\r
+ a2 = a2 < 0 ? -a2 : a2;\r
+ cos_rt = (a1 < a2) ? cos_rt : -cos_rt;\r
+ double ang = Math.atan2(sin_rt, cos_rt);\r
+ // エラー値を計算\r
+ double err = iN * sin_rt * sin_rt + (cos_rt + iJ) * sin_rt + iM * cos_rt * cos_rt + iK * cos_rt + iO;\r
+ // 最小の２個を獲得する。\r
+ if (min_err_0 > err) {\r
+ min_err_1 = min_err_0;\r
+ min_ang_1 = min_ang_0;\r
+ min_err_0 = err;\r
+ min_ang_0 = ang;\r
+ } else if (min_err_1 > err) {\r
+ min_err_1 = err;\r
+ min_ang_1 = ang;\r
+ }\r
+ }\r
+ // [0]をテスト\r
+ double gap_0;\r
+ gap_0 = min_ang_0 - i_hint_angle;\r
+ if (gap_0 > Math.PI) {\r
+ gap_0 = (min_ang_0 - Math.PI * 2) - i_hint_angle;\r
+ } else if (gap_0 < -Math.PI) {\r
+ gap_0 = (min_ang_0 + Math.PI * 2) - i_hint_angle;\r
+ }\r
+ // [1]をテスト\r
+ double gap_1;\r
+ gap_1 = min_ang_1 - i_hint_angle;\r
+ if (gap_1 > Math.PI) {\r
+ gap_1 = (min_ang_1 - Math.PI * 2) - i_hint_angle;\r
+ } else if (gap_1 < -Math.PI) {\r
+ gap_1 = (min_ang_1 + Math.PI * 2) - i_hint_angle;\r
+ }\r
+ return Math.abs(gap_1) < Math.abs(gap_0) ? gap_1 : gap_0;\r
+ }\r
+}\r
* http://www.hitl.washington.edu/artoolkit/\r
*\r
* The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
*\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU Lesser General Public License as \r
+ * published by the Free Software Foundation; either version 3 of the\r
+ * License, or (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful, \r
* but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
+ * See the GNU Lesser General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU Lesser General Public \r
+ * License along with this program. If not, see <http://www.gnu.org/licenses/>.\r
* \r
* For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
+ * http://nyatla.jp/\r
* <airmail(at)ebony.plala.or.jp>\r
- * \r
*/\r
package jp.nyatla.nyartoolkit.core.transmat.solver;\r
\r
* --------------------------------------------------------------------------------\r
*\r
* The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
*\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU Lesser General Public License as \r
+ * published by the Free Software Foundation; either version 3 of the\r
+ * License, or (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful, \r
* but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
+ * See the GNU Lesser General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU Lesser General Public \r
+ * License along with this program. If not, see <http://www.gnu.org/licenses/>.\r
* \r
* For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
+ * http://nyatla.jp/\r
* <airmail(at)ebony.plala.or.jp>\r
- * \r
*/\r
package jp.nyatla.nyartoolkit.core.transmat.solver;\r
\r
* http://www.hitl.washington.edu/artoolkit/\r
*\r
* The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
*\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU Lesser General Public License as \r
+ * published by the Free Software Foundation; either version 3 of the\r
+ * License, or (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful, \r
* but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
+ * See the GNU Lesser General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU Lesser General Public \r
+ * License along with this program. If not, see <http://www.gnu.org/licenses/>.\r
* \r
* For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
+ * http://nyatla.jp/\r
* <airmail(at)ebony.plala.or.jp>\r
- * \r
*/\r
package jp.nyatla.nyartoolkit.core.transmat.solver;\r
\r
package jp.nyatla.nyartoolkit.core.types.matrix;\r
\r
import jp.nyatla.nyartoolkit.*;\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint3d;\r
\r
public class NyARDoubleMatrix33 implements INyARDoubleMatrix\r
{\r
this.m22=i_value[8];\r
return;\r
}\r
+ public void setValue(NyARDoubleMatrix33 i_value)\r
+ {\r
+ this.m00=i_value.m00;\r
+ this.m01=i_value.m01;\r
+ this.m02=i_value.m02;\r
+ this.m10=i_value.m10;\r
+ this.m11=i_value.m11;\r
+ this.m12=i_value.m12;\r
+ this.m20=i_value.m20;\r
+ this.m21=i_value.m21;\r
+ this.m22=i_value.m22;\r
+ return;\r
+ } \r
/**\r
* 遅いからあんまり使わないでね。\r
*/\r
this.m22=b33*det_1;\r
\r
return true;\r
- } \r
- \r
+ }\r
+ /**\r
+ * この関数は、0-PIの間で値を返します。\r
+ * @param o_out\r
+ */\r
+ public final void getZXYAngle(NyARDoublePoint3d o_out)\r
+ {\r
+ double sina = this.m21;\r
+ if (sina >= 1.0) {\r
+ o_out.x = Math.PI / 2;\r
+ o_out.y = 0;\r
+ o_out.z = Math.atan2(-this.m10, this.m00);\r
+ } else if (sina <= -1.0) {\r
+ o_out.x = -Math.PI / 2;\r
+ o_out.y = 0;\r
+ o_out.z = Math.atan2(-this.m10, this.m00);\r
+ } else {\r
+ o_out.x = Math.asin(sina);\r
+ o_out.z = Math.atan2(-this.m01, this.m11);\r
+ o_out.y = Math.atan2(-this.m20, this.m22);\r
+ }\r
+ }\r
+ public final void setZXYAngle(final double i_x, final double i_y, final double i_z)\r
+ {\r
+ final double sina = Math.sin(i_x);\r
+ final double cosa = Math.cos(i_x);\r
+ final double sinb = Math.sin(i_y);\r
+ final double cosb = Math.cos(i_y);\r
+ final double sinc = Math.sin(i_z);\r
+ final double cosc = Math.cos(i_z);\r
+ this.m00 = cosc * cosb - sinc * sina * sinb;\r
+ this.m01 = -sinc * cosa;\r
+ this.m02 = cosc * sinb + sinc * sina * cosb;\r
+ this.m10 = sinc * cosb + cosc * sina * sinb;\r
+ this.m11 = cosc * cosa;\r
+ this.m12 = sinc * sinb - cosc * sina * cosb;\r
+ this.m20 = -cosa * sinb;\r
+ this.m21 = sina;\r
+ this.m22 = cosb * cosa;\r
+ } \r
}\r
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit(Extension)\r
+ * --------------------------------------------------------------------------------\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008-2009 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU Lesser General Public License as \r
+ * published by the Free Software Foundation; either version 3 of the\r
+ * License, or (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful, \r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
+ * See the GNU Lesser General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU Lesser General Public \r
+ * License along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ */\r
+package jp.nyatla.nyartoolkit.core.utils;\r
+\r
+import jp.nyatla.nyartoolkit.*;\r
+/**\r
+ * 方程式を解く関数を定義します。\r
+ *\r
+ */\r
+public class NyAREquationSolver\r
+{\r
+ public static int solve2Equation(double i_a, double i_b, double i_c,double[] o_result)\r
+ {\r
+ assert i_a!=0;\r
+ return solve2Equation(i_b/i_a,i_c/i_a,o_result,0);\r
+ }\r
+ \r
+ public static int solve2Equation(double i_b, double i_c,double[] o_result)\r
+ {\r
+ return solve2Equation(i_b,i_c,o_result,0);\r
+ }\r
+ \r
+ public static int solve2Equation(double i_b, double i_c,double[] o_result,int i_result_st)\r
+ {\r
+ double t=i_b*i_b-4*i_c;\r
+ if(t<0){\r
+ //虚数根\r
+ return 0;\r
+ }\r
+ if(t==0){\r
+ //重根\r
+ o_result[i_result_st+0]=-i_b/(2);\r
+ return 1;\r
+ }\r
+ //実根２個\r
+ t=Math.sqrt(t);\r
+ o_result[i_result_st+0]=(-i_b+t)/(2);\r
+ o_result[i_result_st+1]=(-i_b-t)/(2);\r
+ return 2;\r
+ }\r
+\r
+ /**\r
+ * ３次方程式 a*x^3+b*x^2+c*x+d=0の実根を求める。 \r
+ * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
+ * のコードを基にしてます。\r
+ * @param i_a\r
+ * X^3の係数\r
+ * @param i_b\r
+ * X^2の係数\r
+ * @param i_c\r
+ * X^1の係数\r
+ * @param i_d\r
+ * X^0の係数\r
+ * @param o_result\r
+ * 実根。double[3]を指定すること。\r
+ * @return\r
+ */\r
+ public static int solve3Equation(double i_a, double i_b, double i_c, double i_d,double[] o_result)\r
+ {\r
+ assert (i_a != 0);\r
+ return solve3Equation(i_b/i_a,i_c/i_a,i_d/i_a,o_result);\r
+ }\r
+ \r
+ /**\r
+ * ３次方程式 x^3+b*x^2+c*x+d=0の実根を求める。\r
+ * だけを求める。\r
+ * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
+ * のコードを基にしてます。\r
+ * @param i_b\r
+ * X^2の係数\r
+ * @param i_c\r
+ * X^1の係数\r
+ * @param i_d\r
+ * X^0の係数\r
+ * @param o_result\r
+ * 実根。double[1]以上を指定すること。\r
+ * @return\r
+ */\r
+ public static int solve3Equation(double i_b, double i_c, double i_d,double[] o_result)\r
+ {\r
+ double tmp,b, p, q;\r
+ b = i_b/(3);\r
+ p = b * b - i_c / 3;\r
+ q = (b * (i_c - 2 * b * b) - i_d) / 2;\r
+ if ((tmp = q * q - p * p * p) == 0) {\r
+ // 重根\r
+ q = Math.cbrt(q);\r
+ o_result[0] = 2 * q - b;\r
+ o_result[1] = -q - b;\r
+ return 2;\r
+ } else if (tmp > 0) {\r
+ // 実根1,虚根2\r
+ double a3 = Math.cbrt(q + ((q > 0) ? 1 : -1) * Math.sqrt(tmp));\r
+ double b3 = p / a3;\r
+ o_result[0] = a3 + b3 - b;\r
+ // 虚根:-0.5*(a3+b3)-b,Math.abs(a3-b3)*Math.sqrt(3.0)/2\r
+ return 1;\r
+ } else {\r
+ // 実根3\r
+ tmp = 2 * Math.sqrt(p);\r
+ double t = Math.acos(q / (p * tmp / 2));\r
+ o_result[0] = tmp * Math.cos(t / 3) - b;\r
+ o_result[1] = tmp * Math.cos((t + 2 * Math.PI) / 3) - b;\r
+ o_result[2] = tmp * Math.cos((t + 4 * Math.PI) / 3) - b;\r
+ return 3;\r
+ }\r
+ }\r
+\r
+ \r
+ \r
+ /**\r
+ * ４次方程式の実根だけを求める。\r
+ * @param i_a\r
+ * X^3の係数\r
+ * @param i_b\r
+ * X^2の係数\r
+ * @param i_c\r
+ * X^1の係数\r
+ * @param i_d\r
+ * X^0の係数\r
+ * @param o_result\r
+ * 実根。double[3]を指定すること。\r
+ * @return\r
+ */\r
+ public static int solve4Equation(double i_a, double i_b, double i_c, double i_d,double i_e,double[] o_result) throws NyARException\r
+ {\r
+ assert (i_a != 0);\r
+ double A3,A2,A1,A0,B3;\r
+ A3=i_b/i_a;\r
+ A2=i_c/i_a;\r
+ A1=i_d/i_a;\r
+ A0=i_e/i_a;\r
+ B3=A3/4;\r
+ double p,q,r;\r
+ double B3_2=B3*B3;\r
+ p=A2-6*B3_2;//A2-6*B3*B3;\r
+ q=A1+B3*(-2*A2+8*B3_2);//A1-2*A2*B3+8*B3*B3*B3;\r
+ r=A0+B3*(-A1+A2*B3)-3*B3_2*B3_2;//A0-A1*B3+A2*B3*B3-3*B3*B3*B3*B3;\r
+ if(q==0){\r
+ double result_0,result_1;\r
+ //複二次式\r
+ int res=solve2Equation(p,r,o_result,0);\r
+ switch(res){\r
+ case 0:\r
+ //全て虚数解\r
+ return 0;\r
+ case 1:\r
+ //重根\r
+ //解は0,1,2の何れか。\r
+ result_0=o_result[0];\r
+ if(result_0<0){\r
+ //全て虚数解\r
+ return 0;\r
+ }\r
+ //実根1個\r
+ if(result_0==0){\r
+ //NC\r
+ o_result[0]=0-B3;\r
+ return 1;\r
+ }\r
+ //実根2個\r
+ result_0=Math.sqrt(result_0);\r
+ o_result[0]=result_0-B3;\r
+ o_result[1]=-result_0-B3;\r
+ return 2;\r
+ case 2:\r
+ //実根２個だからt==t2==0はありえない。(case1)\r
+ //解は、0,2,4の何れか。\r
+ result_0=o_result[0];\r
+ result_1=o_result[1];\r
+ int number_of_result=0;\r
+ if(result_0>0){\r
+ //NC\r
+ result_0=Math.sqrt(result_0);\r
+ o_result[0]= result_0-B3;\r
+ o_result[1]=-result_0-B3;\r
+ number_of_result+=2;\r
+ }\r
+ if(result_1>0)\r
+ {\r
+ //NC\r
+ result_1=Math.sqrt(result_1);\r
+ o_result[number_of_result+0]= result_1-B3;\r
+ o_result[number_of_result+1]=-result_1-B3;\r
+ number_of_result+=2;\r
+ }\r
+ return number_of_result;\r
+ default:\r
+ throw new NyARException();\r
+ }\r
+ }else{\r
+ //それ以外\r
+ //最適化ポイント:\r
+ //u^3 + (2*p)*u^2 +((- 4*r)+(p^2))*u -q^2= 0\r
+ double u=solve3Equation_1((2*p),(- 4*r)+(p*p),-q*q);\r
+ if(u<0){\r
+ //全て虚数解\r
+ return 0;\r
+ }\r
+ double ru=Math.sqrt(u);\r
+ //2次方程式を解いてyを計算(最適化ポイント)\r
+ int result_1st,result_2nd;\r
+ result_1st=solve2Equation(-ru,(p+u)/2+ru*q/(2*u),o_result,0);\r
+ //配列使い回しのために、変数に退避\r
+ switch(result_1st){\r
+ case 0:\r
+ break;\r
+ case 1:\r
+ o_result[0]=o_result[0]-B3;\r
+ break;\r
+ case 2:\r
+ o_result[0]=o_result[0]-B3;\r
+ o_result[1]=o_result[1]-B3;\r
+ break;\r
+ default:\r
+ throw new NyARException();\r
+ }\r
+ result_2nd=solve2Equation(ru,(p+u)/2-ru*q/(2*u),o_result,result_1st);\r
+ //0,1番目に格納\r
+ switch(result_2nd){\r
+ case 0:\r
+ break;\r
+ case 1:\r
+ o_result[result_1st+0]=o_result[result_1st+0]-B3;\r
+ break;\r
+ case 2:\r
+ o_result[result_1st+0]=o_result[result_1st+0]-B3;\r
+ o_result[result_1st+1]=o_result[result_1st+1]-B3;\r
+ break;\r
+ default:\r
+ throw new NyARException();\r
+ }\r
+ return result_1st+result_2nd;\r
+ }\r
+ }\r
+ /**\r
+ * 3乗根を求められないシステムで、３乗根を求めます。\r
+ * http://aoki2.si.gunma-u.ac.jp/JavaScript/src/3jisiki.html\r
+ * @param i_in\r
+ * @return\r
+ */\r
+ private double cuberoot(double i_in) {\r
+ double res = Math.pow(Math.abs(i_in), 1.0 / 3.0);\r
+ return (i_in >= 0) ? res : -res;\r
+ }\r
+ /**\r
+ * 3次方程式の実根を１個だけ求める。\r
+ * 4字方程式で使う。\r
+ * @param i_b\r
+ * @param i_c\r
+ * @param i_d\r
+ * @param o_result\r
+ * @return\r
+ */\r
+ private static double solve3Equation_1(double i_b, double i_c, double i_d)\r
+ {\r
+ double tmp,b, p, q;\r
+ b = i_b/(3);\r
+ p = b * b - i_c / 3;\r
+ q = (b * (i_c - 2 * b * b) - i_d) / 2;\r
+ if ((tmp = q * q - p * p * p) == 0) {\r
+ // 重根\r
+ q = Math.cbrt(q);\r
+ return 2 * q - b;\r
+ } else if (tmp > 0) {\r
+ // 実根1,虚根2\r
+ double a3 = Math.cbrt(q + ((q > 0) ? 1 : -1) * Math.sqrt(tmp));\r
+ double b3 = p / a3;\r
+ return a3 + b3 - b;\r
+ } else {\r
+ // 実根3\r
+ tmp = 2 * Math.sqrt(p);\r
+ double t = Math.acos(q / (p * tmp / 2));\r
+ return tmp * Math.cos(t / 3) - b;\r
+ }\r
+ } \r
+\r
+ public static void main(String[] args)\r
+ {\r
+ NyAREquationSolver n = new NyAREquationSolver();\r
+ int l=0;\r
+ double[] r = new double[10];\r
+ try{\r
+ l=n.solve4Equation(1, 9, -18, -68, 120, r);\r
+ }catch(Exception e){\r
+ e.printStackTrace();\r
+ }\r
+ System.out.println(l);\r
+ }\r
+}\r
protected void initInstance(\r
INyARColorPatt i_patt_inst,\r
INyARSquareDetector i_sqdetect_inst,\r
+ INyARTransMat i_transmat_inst,\r
INyARRasterFilter_RgbToBin i_filter,\r
NyARParam i_ref_param,\r
NyARCode i_ref_code,\r
final NyARIntSize scr_size=i_ref_param.getScreenSize(); \r
// 解析オブジェクトを作る\r
this._square_detect = i_sqdetect_inst;\r
- this._transmat = new NyARTransMat(i_ref_param);\r
+ this._transmat = i_transmat_inst;\r
this._tobin_filter=i_filter;\r
// 比較コードを保存\r
this._marker_width = i_marker_width;\r
final NyARIntSize scr_size=i_ref_param.getScreenSize();\r
// 解析オブジェクトを作る\r
\r
- this._transmat = new NyARTransMat(i_ref_param);\r
+ this._transmat = new NyARTransMat_NyARToolkit(i_ref_param);\r
//各コード用の比較器を作る。\r
this._match_patt=new NyARMatchPatt_Color_WITHOUT_PCA[i_number_of_code];\r
final int cw = i_ref_code[0].getWidth();\r
import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2bin.NyARRasterFilter_ARToolkitThreshold;\r
import jp.nyatla.nyartoolkit.core.squaredetect.*;\r
import jp.nyatla.nyartoolkit.core.pickup.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.*;\r
/**\r
* 画像からARCodeに最も一致するマーカーを1個検出し、その変換行列を計算するクラスです。\r
* \r
{\r
public final static int PF_ARTOOLKIT_COMPATIBLE=1;\r
public final static int PF_NYARTOOLKIT=2;\r
+ public final static int PF_NYARTOOLKIT_ARTOOLKIT_FITTING=100;\r
+ public final static int PF_TEST2=201;\r
\r
/**\r
* 検出するARCodeとカメラパラメータから、1個のARCodeを検出するNyARSingleDetectMarkerインスタンスを作ります。\r
final NyARRasterFilter_ARToolkitThreshold th=new NyARRasterFilter_ARToolkitThreshold(100,i_input_raster_type);\r
INyARColorPatt patt_inst;\r
INyARSquareDetector sqdetect_inst;\r
+ INyARTransMat transmat_inst;\r
\r
switch(i_profile_id){\r
case PF_ARTOOLKIT_COMPATIBLE:\r
patt_inst=new NyARColorPatt_O3(i_ref_code.getWidth(), i_ref_code.getHeight());\r
sqdetect_inst=new NyARSquareDetector_ARToolKit(i_ref_param.getDistortionFactor(),i_ref_param.getScreenSize());\r
+ transmat_inst=new NyARTransMat(i_ref_param);\r
+ break;\r
+ case PF_NYARTOOLKIT_ARTOOLKIT_FITTING:\r
+ patt_inst=new NyARColorPatt_Perspective_O2(i_ref_code.getWidth(), i_ref_code.getHeight(),4,25);\r
+ sqdetect_inst=new NyARSquareDetector_Rle(i_ref_param.getDistortionFactor(),i_ref_param.getScreenSize());\r
+ transmat_inst=new NyARTransMat(i_ref_param);\r
break;\r
case PF_NYARTOOLKIT:\r
patt_inst=new NyARColorPatt_Perspective_O2(i_ref_code.getWidth(), i_ref_code.getHeight(),4,25);\r
- sqdetect_inst=new NyARSquareDetector_Rle(i_ref_param.getDistortionFactor(),i_ref_param.getScreenSize()); \r
+ sqdetect_inst=new NyARSquareDetector_Rle(i_ref_param.getDistortionFactor(),i_ref_param.getScreenSize());\r
+ transmat_inst=new NyARTransMat_NyARToolkit(i_ref_param);\r
break;\r
default:\r
throw new NyARException();\r
}\r
- super.initInstance(patt_inst,sqdetect_inst,th,i_ref_param,i_ref_code,i_marker_width);\r
+ super.initInstance(patt_inst,sqdetect_inst,transmat_inst,th,i_ref_param,i_ref_code,i_marker_width);\r
\r
}\r
\r
\r
private INyARSquareDetector _square_detect;\r
\r
- protected NyARTransMat _transmat;\r
+ protected INyARTransMat _transmat;\r
\r
private double _marker_width;\r
\r
NyARIntSize scr_size = i_param.getScreenSize();\r
// 解析オブジェクトを作る\r
this._square_detect = new NyARSquareDetector_Rle(i_param.getDistortionFactor(), scr_size);\r
- this._transmat = new NyARTransMat(i_param);\r
+ this._transmat = new NyARTransMat_NyARToolkit(i_param);\r
this._tobin_filter=new NyARRasterFilter_ARToolkitThreshold(110,i_raster_type);\r
\r
// ２値画像バッファを作る\r
private int _lost_delay = 5;\r
\r
private NyARSquareDetector_Rle _square_detect;\r
- protected NyARTransMat _transmat;\r
+ protected INyARTransMat _transmat;\r
private double _marker_width=100;\r
\r
private NyARSquareStack _square_list = new NyARSquareStack(100);\r
NyARIntSize scr_size = i_param.getScreenSize();\r
// 解析オブジェクトを作る\r
this._square_detect = new NyARSquareDetector_Rle(i_param.getDistortionFactor(), scr_size);\r
- this._transmat = new NyARTransMat(i_param);\r
+ this._transmat = new NyARTransMat_NyARToolkit(i_param);\r
this._encoder=i_encoder;\r
\r
// ２値画像バッファを作る\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.dev;\r
-\r
-import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.nyartoolkit.core.raster.rgb.*;\r
-import jp.nyatla.nyartoolkit.core.rasterreader.*;\r
-import jp.nyatla.nyartoolkit.core.squaredetect.NyARSquare;\r
-import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.utils.*;\r
-import jp.nyatla.nyartoolkit.core.labeling.artoolkit.NyARLabelingLabel;\r
-import jp.nyatla.nyartoolkit.core.pickup.*;\r
-import jp.nyatla.nyartoolkit.nyidmarker.MarkerPattEncoder;\r
-import jp.nyatla.nyartoolkit.nyidmarker.PerspectivePixelReader;\r
-\r
-\r
-\r
-/**\r
- * 1区間にある1個のエッジ位置を推定するクラスです。\r
- *\r
- */\r
-class NyARSingleEdgeFinder2\r
-{\r
- public static class TEdgeInfo\r
- {\r
- double point; //検出したエッジの位置\r
- int sub; //エッジの差分\r
- } \r
- private int[] _work;\r
- private int _width;\r
- private int _height;\r
- public NyARSingleEdgeFinder2(int i_width,int i_height)\r
- {\r
- this._work=new int[(i_width>i_height?i_width:i_height)+1];\r
- this._work[this._work.length-1]=0;\r
- this._width=i_width;\r
- this._height=i_height;\r
- return;\r
- }\r
- /**\r
- * この関数は、一区間に１個のエッジが含まれていると仮定して、その位置を推定します。\r
- * [n]個の配列を与えた場合、[n+2]～[n-1]の間にあるエッジを検出します。\r
- * 検出方向は左→右の順です\r
- * @param i_pixcel\r
- * @param i_start\r
- * @param i_length\r
- * @param o_out\r
- * @return\r
- */\r
- public boolean scanSingleEdgeLeftToRight(int[] i_pixcel,int i_y,TEdgeInfo o_out)\r
- {\r
- final int[] temp=this._work;\r
- //1回微分(0-8)\r
- final int length=this._width-1;\r
- int p=i_y*this._width;\r
- for(int i2=0;i2<length;i2++){\r
- temp[i2]=i_pixcel[p+1]-i_pixcel[p];\r
- p++;\r
- }\r
- return scanSingleEdge(temp,length,o_out);\r
- }\r
- /**\r
- * 線分内のエッジを検出します。\r
- * この関数は、1区間に1個のエッジが含まれていると仮定して、その位置を推定します。\r
- * [n]個の配列を与えた場合、[n+2]～[n-1]の間にあるエッジを検出します。\r
- * 検出方向は右→左の順です\r
- * @param i_pixcel\r
- * @param i_start\r
- * @param i_length\r
- * @param o_out\r
- * @return\r
- */\r
- public boolean scanSingleEdgeRightToLeft(int[] i_pixcel,int i_y,TEdgeInfo o_out)\r
- {\r
- final int[] temp=this._work;\r
- //1回微分(0-8)\r
- final int length=this._width-1;\r
- int p=(i_y+1)*this._width-1;\r
- for(int i2=0;i2<length;i2++){\r
- temp[i2]=i_pixcel[p-1]-i_pixcel[p];\r
- p--;\r
- }\r
- return scanSingleEdge(temp,length,o_out);\r
- } \r
- public boolean scanSingleEdgeTopToBottom(int[] i_pixcel,int i_x,TEdgeInfo o_out)\r
- {\r
- final int[] temp=this._work;\r
- //1回微分(0-8)\r
- final int step=this._width;\r
- final int length=this._height-1;\r
- int p=i_x;\r
- for(int i2=0;i2<length;i2++){\r
- temp[i2]=i_pixcel[p+step]-i_pixcel[p];\r
- p+=step;\r
- }\r
- return scanSingleEdge(temp,length,o_out);\r
- }\r
- public boolean scanSingleEdgeBottomToTop(int[] i_pixcel,int i_x,TEdgeInfo o_out)\r
- {\r
- final int[] temp=this._work;\r
- //1回微分(0-8)\r
- final int step=this._width;\r
- final int length=this._height-1;\r
- int p=i_x+step*length;\r
- for(int i2=0;i2<length;i2++){\r
- temp[i2]=i_pixcel[p-step]-i_pixcel[p];\r
- p-=step;\r
- }\r
- return scanSingleEdge(temp,length,o_out);\r
- } \r
- private boolean scanSingleEdge(int[] i_pixels,int i_length,TEdgeInfo o_out)\r
- {\r
- //微分(2回目)して、極値２か所を得る\r
- int max_index,min_index;\r
- int length=i_length-1;\r
- max_index=min_index=0;\r
- int max_value,min_value;\r
- max_value=min_value=0;\r
- for(int i2=0;i2<length;i2++){\r
- int t=i_pixels[i2+1]-i_pixels[i2];\r
- if(t>max_value){\r
- max_index=i2;\r
- max_value=t;\r
- }\r
- if(t<min_value){\r
- min_index=i2;\r
- min_value=t;\r
- }\r
- }\r
- //同符号である場合、範囲内にエッジはない\r
- if(max_value*min_value>=0){\r
- return false;\r
- } \r
- o_out.point=(max_index+min_index)/2.0;\r
- o_out.sub=max_value-min_value;\r
- return true;\r
- }\r
- public int scanEdgeLeftToRight(int[] i_pixel,int i_y,int i_noise_th,double[] o_edge_index)\r
- {\r
- final int[] temp=this._work;\r
- //1回微分(0-8)\r
- final int length=this._width-1;\r
- int p=i_y*this._width;\r
- for(int i2=0;i2<length;i2++){\r
- temp[i2]=i_pixel[p+1]-i_pixel[p];\r
- p++;\r
- }\r
- //0終端させるために1要素を後続に追加\r
- return scanEdge(temp,length+1,i_noise_th,o_edge_index);\r
- }\r
- private int scanEdge(int[] i_pixels,int i_length,int i_noise_th,double[] o_out)\r
- {\r
- int points=0;\r
- final int length=i_length;\r
- //エッジ1区間を抜き出す\r
- for(int i2=0;i2<length;i2++){\r
- int t=i_pixels[i2];\r
- if(t>i_noise_th){\r
- int st=i2;\r
- i2++;\r
- for(;i2<length;i2++){\r
- t=i_pixels[i2];\r
- if(t<=0){\r
- //(st - i2で1区間)\r
- //エッジ位置は区間の中央にする。\r
- o_out[points]=(st+i2)/2.0;\r
- points++;\r
- if(t<0){\r
- //マイナスであれば、0を補完する\r
- i2--;\r
- i_pixels[i2]=0;\r
- }\r
- break;\r
- }\r
-\r
- }\r
- }else if(t<-i_noise_th){\r
- int st=i2;\r
- i2++;\r
- for(;i2<length;i2++){\r
- t=i_pixels[i2];\r
- if(t>=0){\r
- //(st - i2で1区間)\r
- //エッジ位置は区間の中央にする。\r
- o_out[points]=(st+i2)/2.0;\r
- points++;\r
- if(t>0){\r
- //プラスであれば、0を補完する\r
- i2--;\r
- i_pixels[i2]=0;\r
- }\r
- break;\r
- }\r
- }\r
- }\r
- }\r
- return points;\r
- } \r
- /**\r
- * 指定した配列をノイズパターンとして、ノイズ値を計算します。\r
- * このノイズ値は、scanEdgeのノイズ値として使用できます。\r
- * @param i_pixels\r
- * @param i_length\r
- * @return\r
- */\r
- public int getNoiseValue(int[] i_pixels,int i_length)\r
- {\r
- //1回微分して、その最大値と最小値を計算\r
- int length=i_length-1;\r
- int max_value,min_value;\r
- max_value=min_value=0;\r
- for(int i2=0;i2<length;i2++){\r
- int t=i_pixels[i2+1]-i_pixels[i2];\r
- if(t>max_value){\r
- max_value=t;\r
- }\r
- if(t<min_value){\r
- min_value=t;\r
- }\r
- }\r
- return (-min_value>max_value)?-min_value:max_value;\r
- } \r
-}\r
-\r
-class MarkerEncoder2\r
-{\r
- private final static int[] _bit_table_3={\r
- 25, 26, 27, 28, 29, 30, 31,\r
- 48, 9, 10, 11, 12, 13, 32,\r
- 47, 24, 1, 2, 3, 14, 33,\r
- 46, 23, 8, 0, 4, 15, 34,\r
- 45, 22, 7, 6, 5, 16, 35,\r
- 44, 21, 20, 19, 18, 17, 36,\r
- 43, 42, 41, 40, 39, 38, 37\r
- }; \r
- private final static int[] _bit_table_2={\r
- 9, 10, 11, 12, 13,\r
- 24, 1, 2, 3, 14,\r
- 23, 8, 0, 4, 15,\r
- 22, 7, 6, 5, 16,\r
- 21, 20, 19, 18, 17};\r
- private final static int[][] _bit_tables={\r
- _bit_table_2,_bit_table_3\r
- };\r
- /**\r
- * RECT(0):[0]=(0)\r
- * RECT(1):[1]=(1-8)\r
- * RECT(2):[2]=(9-16),[3]=(17-24)\r
- * RECT(3):[4]=(25-32),[5]=(33-40),[6]=(41-48)\r
- */\r
- int[] _bit_table;\r
- int[] _bits=new int[16];\r
- int[] _work=new int[16];\r
- int _model;\r
- public void setBitByBitIndex(int i_index_no,int i_value)\r
- {\r
- assert i_value==0 || i_value==1;\r
- final int bit_no=this._bit_table[i_index_no];\r
- if(bit_no==0){\r
- this._bits[0]=i_value;\r
- }else{\r
- int bidx=(bit_no-1)/8+1;\r
- int sidx=(bit_no-1)%8;\r
- this._bits[bidx]=(this._bits[bidx]&(~(0x01<<sidx)))|(i_value<<sidx);\r
- }\r
- return;\r
- }\r
- \r
- public void setBit(int i_bit_no,int i_value)\r
- {\r
- assert i_value==0 || i_value==1;\r
- if(i_bit_no==0){\r
- this._bits[0]=i_value;\r
- }else{\r
- int bidx=(i_bit_no-1)/8+1;\r
- int sidx=(i_bit_no-1)%8;\r
- this._bits[bidx]=(this._bits[bidx]&(~(0x01<<sidx)))|(i_value<<sidx);\r
- }\r
- return;\r
- }\r
- public int getBit(int i_bit_no)\r
- {\r
- if(i_bit_no==0){\r
- return this._bits[0];\r
- }else{\r
- int bidx=(i_bit_no-1)/8+1;\r
- int sidx=(i_bit_no-1)%8;\r
- return (this._bits[bidx]>>(sidx))&(0x01);\r
- }\r
- }\r
- public boolean initEncoder(int i_model)\r
- {\r
- if(i_model>=4 || i_model<2){\r
- //Lv4以降に対応する時は、この制限を変える。\r
- return false;\r
- }\r
- this._bit_table=_bit_tables[i_model-2];\r
- this._model=i_model;\r
- return true;\r
- }\r
- private int getDirection()\r
- {\r
- int l,t,r,b;\r
- switch(this._model){\r
- case 2:\r
- //トラッキングセルを得る\r
- t=this._bits[2] & 0x07;\r
- r=(this._bits[2] & 0x70)>>4;\r
- b=this._bits[3] & 0x07;\r
- l=(this._bits[3] & 0x70)>>4;\r
- break;\r
- case 3:\r
- default:\r
- return -3;\r
- }\r
- //101であるかを確認\r
- if(t==0x05){\r
- if(r==0x05){\r
- return (b!=0x05 && l!=0x05)?2:-2;\r
- }else if(l==0x05){\r
- return (b!=0x05 && r!=0x05)?3:-2;\r
- }\r
- }else if(b==0x05){\r
- if(r==0x05){\r
- return (t!=0x05 && l!=0x05)?1:-2;\r
- }else if(l==0x05){\r
- return (t!=0x05 && r!=0x05)?0:-2;\r
- }\r
- }\r
- return -1;\r
- }\r
-\r
- public boolean encode()\r
- {\r
- int d=getDirection();\r
- if(d<0){\r
- return false;\r
- }\r
- //回転操作\r
- \r
- rotateDirection(d);\r
- //デコード\r
- \r
- return true;\r
- }\r
- private void rotateDirection(int i_direction)\r
- {\r
- int sl=i_direction*2;\r
- int sr=8-sl;\r
-\r
- int w1;\r
- //RECT1\r
- w1=this._bits[1];\r
- this._bits[1]=(w1<<sl)|(w1>>sr);\r
- \r
- //RECT2\r
- sl=i_direction*4;\r
- sr=16-sl;\r
- w1=this._bits[2]|(this._bits[3]<<8);\r
- w1=(w1<<sl)|(w1>>sr);\r
- this._bits[2]=w1 & 0xff;\r
- this._bits[3]=(w1>>8) & 0xff;\r
-\r
- if(this._model<2){\r
- return;\r
- }\r
-\r
- //RECT3\r
- sl=i_direction*6;\r
- sr=24-sl; \r
- w1=this._bits[4]|(this._bits[5]<<8)|(this._bits[6]<<16);\r
- w1=(w1<<sl)|(w1>>sr);\r
- this._bits[4]=w1 & 0xff;\r
- this._bits[5]=(w1>>8) & 0xff;\r
- this._bits[6]=(w1>>16) & 0xff;\r
- \r
- if(this._model<3){\r
- return;\r
- }\r
- //RECT4(Lv4以降はここの制限を変える)\r
-// shiftLeft(this._bits,7,3,i_direction*);\r
-\r
- return;\r
- }\r
- public void shiftLeft(int[] i_pack,int i_start,int i_length,int i_ls)\r
- {\r
- int[] work=this._work;\r
- //端数シフト\r
- final int mod_shift=i_ls%8;\r
- for(int i=i_length-1;i>=1;i--){\r
- work[i]=(i_pack[i+i_start]<<mod_shift)|(0xff&(i_pack[i+i_start-1]>>(8-mod_shift)));\r
- }\r
- work[0]=(i_pack[i_start]<<mod_shift)|(0xff&(i_pack[i_start+i_length-1]>>(8-mod_shift)));\r
- //バイトシフト\r
- final int byte_shift=(i_ls/8)%i_length;\r
- for(int i=i_length-1;i>=0;i--){\r
- i_pack[(byte_shift+i)%i_length+i_start]=0xff & work[i];\r
- }\r
- return;\r
- }\r
-\r
- \r
-}\r
-\r
-\r
-/**\r
- * NyARColorPatt_NyIdMarkerがラスタからPerspective変換して読みだすためのクラス\r
- *\r
- */\r
-class PerspectivePixelReader2 extends NyARPerspectiveParamGenerator_O1\r
-{\r
- private double[] _cparam=new double[8];\r
-\r
- public PerspectivePixelReader2(int i_local_x,int i_local_y,int i_width, int i_height)\r
- {\r
- super(i_local_x,i_local_y,i_width,i_height);\r
- this._temp1=new int[i_width];\r
- this._temp2=new int[i_width];\r
- this._pixcel_temp=new int[i_width*3];\r
-\r
- return;\r
- }\r
- private int[] _temp1;\r
- private int[] _temp2;\r
- private int[] _pixcel_temp;\r
- private INyARRgbRaster _raster;\r
- public void setSourceRaster(INyARRgbRaster i_raster)\r
- {\r
- this._raster=i_raster;\r
- return;\r
- }\r
- public boolean setSourceSquare(NyARIntPoint2d[] i_vertex)throws NyARException\r
- {\r
- return this.getParam(i_vertex, this._cparam);\r
- }\r
- public void rectPixels(int i_lt_x,int i_lt_y,int i_width,int i_height,int[] o_pixcel)throws NyARException\r
- {\r
- final double[] cpara=this._cparam;\r
- final INyARRgbPixelReader reader=this._raster.getRgbPixelReader();\r
- final int[] ref_x=this._temp1;\r
- final int[] ref_y=this._temp2;\r
- final int[] pixcel_temp=this._pixcel_temp;\r
- int out_index=0;\r
-\r
- for(int i=0;i<i_height;i++){\r
- //1列分のピクセルのインデックス値を計算する。\r
- int cy0=1+i+i_lt_y;\r
- double cpy0_12=cpara[1]*cy0+cpara[2];\r
- double cpy0_45=cpara[4]*cy0+cpara[5];\r
- double cpy0_7=cpara[7]*cy0+1.0; \r
- int pt=0;\r
- for(int i2=0;i2<i_width;i2++)\r
- {\r
- int cx0=1+i2+i_lt_x;\r
-\r
- double cp6_0=cpara[6]*cx0;\r
- double cpx0_0=cpara[0]*cx0;\r
- double cpx3_0=cpara[3]*cx0;\r
- \r
- final double d=cp6_0+cpy0_7;\r
- ref_x[pt]=(int)((cpx0_0+cpy0_12)/d);\r
- ref_y[pt]=(int)((cpx3_0+cpy0_45)/d);\r
- pt++;\r
- }\r
- //1行分のピクセルを取得(場合によっては専用アクセサを書いた方がいい)\r
- reader.getPixelSet(ref_x,ref_y,i_width,pixcel_temp);\r
- //グレースケールにしながら、line→mapへの転写\r
- for(int i2=0;i2<i_width;i2++){\r
- int index=i2*3;\r
- o_pixcel[out_index]=(pixcel_temp[index+0]+pixcel_temp[index+1]+pixcel_temp[index+2])/3;\r
- out_index++;\r
- } \r
- }\r
- return;\r
- }\r
-\r
-\r
- \r
- \r
- //タイミングパターン用のパラメタ(FRQ_POINTS*FRQ_STEPが100を超えないようにすること)\r
- private static int FRQ_STEP=2;\r
- private static int FRQ_POINTS=45;\r
- public static int FRQ_EDGE=5;\r
-\r
- \r
- /**\r
- * i_y1行目とi_y2行目を平均して、タイミングパターンの周波数を得ます。\r
- * LHLを1周期として、たとえばLHLHLの場合は2を返します。LHLHやHLHL等の始端と終端のレベルが異なるパターンを\r
- * 検出した場合、関数は失敗します。\r
- * \r
- * @param i_y1\r
- * @param i_y2\r
- * @param i_th_h\r
- * @param i_th_l\r
- * @param o_edge_index\r
- * 検出したエッジ位置(H->L,L->H)のインデクスを受け取る配列です。\r
- * [FRQ_POINTS]以上の配列を指定してください。\r
- * @return\r
- * @throws NyARException\r
- */\r
- public int getRowFrequency(int i_y1,int i_th_h,int i_th_l,int[] o_edge_index)throws NyARException\r
- {\r
- final double[] cpara=this._cparam;\r
- final INyARRgbPixelReader reader=this._raster.getRgbPixelReader();\r
- final int[] ref_x=this._temp1;\r
- final int[] ref_y=this._temp2;\r
- final int[] pixcel_temp=this._pixcel_temp;\r
-\r
- //2行分のピクセルインデックスを計算\r
- int cy0=1+i_y1;\r
- double cpy0_12=cpara[1]*cy0+cpara[2];\r
- double cpy0_45=cpara[4]*cy0+cpara[5];\r
- double cpy0_7=cpara[7]*cy0+1.0; \r
-\r
-\r
- int pt=0;\r
- for(int i2=0;i2<FRQ_POINTS;i2++)\r
- {\r
- double d;\r
- final int cx0=1+i2*FRQ_STEP+FRQ_EDGE; \r
- d=(cpara[6]*cx0)+cpy0_7;\r
- ref_x[pt]=(int)((cpara[0]*cx0+cpy0_12)/d);\r
- ref_y[pt]=(int)((cpara[3]*cx0+cpy0_45)/d);\r
- pt++;\r
- }\r
- \r
- //ピクセルを取得(入力画像を多様化するならここから先を調整すること)\r
- reader.getPixelSet(ref_x,ref_y,FRQ_POINTS,pixcel_temp);\r
- return getFreqInfo(pixcel_temp,i_th_h,i_th_l,o_edge_index);\r
- }\r
- public int getColFrequency(int i_x1,int i_th_h,int i_th_l,int[] o_edge_index)throws NyARException\r
- {\r
- final double[] cpara=this._cparam;\r
- final INyARRgbPixelReader reader=this._raster.getRgbPixelReader();\r
- final int[] ref_x=this._temp1;\r
- final int[] ref_y=this._temp2;\r
- final int[] pixcel_temp=this._pixcel_temp;\r
-\r
- final int cx0=1+i_x1;\r
- final double cp6_0=cpara[6]*cx0;\r
- final double cpx0_0=cpara[0]*cx0;\r
- final double cpx3_0=cpara[3]*cx0;\r
-\r
- int pt=0;\r
- for(int i2=0;i2<FRQ_POINTS;i2++)\r
- {\r
- double d;\r
- int cy=1+i2*FRQ_STEP+FRQ_EDGE;\r
- double cpy_12=cpara[1]*cy+cpara[2];\r
- double cpy_45=cpara[4]*cy+cpara[5];\r
- double cpy_7=cpara[7]*cy+1.0; \r
- \r
- d=cp6_0+cpy_7;\r
- ref_x[pt]=(int)((cpx0_0+cpy_12)/d);\r
- ref_y[pt]=(int)((cpx3_0+cpy_45)/d);\r
- pt++;\r
- \r
- } \r
- \r
- //ピクセルを取得(入力画像を多様化するならここから先を調整すること)\r
- reader.getPixelSet(ref_x,ref_y,FRQ_POINTS,pixcel_temp);\r
- return getFreqInfo(pixcel_temp,i_th_h,i_th_l,o_edge_index);\r
- }\r
-\r
- /**\r
- * デバックすんだらstaticにしておｋ\r
- * @param i_pixcels\r
- * @param i_th_h\r
- * @param i_th_l\r
- * @param o_edge_index\r
- * @return\r
- */\r
- private int getFreqInfo(int[] i_pixcels,int i_th_h,int i_th_l,int[] o_edge_index)\r
- {\r
- //トークンを解析して、周波数を計算\r
- int i=0;\r
- int frq_l2h=0;\r
- int frq_h2l=0;\r
- while(i<FRQ_POINTS){\r
- //L->Hトークンを検出する\r
- while(i<FRQ_POINTS){\r
- final int index=i*3;\r
- final int pix=(i_pixcels[index+0]+i_pixcels[index+1]+i_pixcels[index+2])/3;\r
- if(pix>i_th_h){\r
- //トークン発見\r
- o_edge_index[frq_l2h+frq_h2l]=i;\r
- frq_l2h++;\r
- break;\r
- }\r
- i++;\r
- }\r
- i++;\r
- //L->Hトークンを検出する\r
- while(i<FRQ_POINTS){\r
- final int index=i*3;\r
- final int pix=(i_pixcels[index+0]+i_pixcels[index+1]+i_pixcels[index+2])/3;\r
- if(pix<=i_th_l){\r
- //トークン発見\r
- o_edge_index[frq_l2h+frq_h2l]=i;\r
- frq_h2l++;\r
- break;\r
- }\r
- i++;\r
- }\r
- i++;\r
- }\r
- return frq_l2h==frq_h2l?frq_l2h:-1; \r
- }\r
-\r
-\r
-\r
- public boolean readDataBits(double[] i_index_x,double[] i_index_y,int i_size,int i_th,MarkerPattEncoder o_bitbuffer)throws NyARException\r
- {\r
- \r
- assert i_size*4<this._width;\r
- \r
- final double[] cpara=this._cparam;\r
-\r
- final int[] ref_x=this._temp1;\r
- final int[] ref_y=this._temp2;\r
- final INyARRgbPixelReader reader=this._raster.getRgbPixelReader();\r
- final int[] pixcel_temp=this._pixcel_temp;\r
- \r
- int p=0;\r
- for(int i=0;i<i_size;i++){\r
- //1列分のピクセルのインデックス値を計算する。\r
- double cy0=1+i_index_y[i*2+0];\r
- double cy1=1+i_index_y[i*2+1]; \r
- double cpy0_12=cpara[1]*cy0+cpara[2];\r
- double cpy0_45=cpara[4]*cy0+cpara[5];\r
- double cpy0_7=cpara[7]*cy0+1.0;\r
- double cpy1_12=cpara[1]*cy1+cpara[2];\r
- double cpy1_45=cpara[4]*cy1+cpara[5];\r
- double cpy1_7=cpara[7]*cy1+1.0;\r
- \r
- int pt=0;\r
- for(int i2=0;i2<i_size;i2++)\r
- { \r
-\r
- double d;\r
- double cx0=1+i_index_x[i2*2+0];\r
- double cx1=1+i_index_x[i2*2+1];\r
-\r
- double cp6_0=cpara[6]*cx0;\r
- double cp6_1=cpara[6]*cx1;\r
- double cpx0_0=cpara[0]*cx0;\r
- double cpx3_0=cpara[3]*cx0;\r
- \r
- d=cp6_0+cpy0_7;\r
- ref_x[pt]=(int)((cpx0_0+cpy0_12)/d);\r
- ref_y[pt]=(int)((cpx3_0+cpy0_45)/d);\r
- pt++;\r
-\r
- d=cp6_0+cpy1_7;\r
- ref_x[pt]=(int)((cpx0_0+cpy1_12)/d);\r
- ref_y[pt]=(int)((cpx3_0+cpy1_45)/d);\r
- pt++;\r
-\r
- d=cp6_1+cpy0_7;\r
- ref_x[pt]=(int)((cpara[0]*cx1+cpy0_12)/d);\r
- ref_y[pt]=(int)((cpara[3]*cx1+cpy0_45)/d);\r
- pt++;\r
-\r
- d=cp6_1+cpy1_7;\r
- ref_x[pt]=(int)((cpara[0]*cx1+cpy1_12)/d);\r
- ref_y[pt]=(int)((cpara[3]*cx1+cpy1_45)/d);\r
- pt++;\r
- }\r
- //1行分のピクセルを取得(場合によっては専用アクセサを書いた方がいい)\r
- reader.getPixelSet(ref_x,ref_y,i_size*4,pixcel_temp);\r
- //グレースケールにしながら、line→mapへの転写\r
- for(int i2=0;i2<i_size;i2++){\r
- int index=i2*3*4;\r
- int pixel=( pixcel_temp[index+0]+pixcel_temp[index+1]+pixcel_temp[index+2]+\r
- pixcel_temp[index+3]+pixcel_temp[index+4]+pixcel_temp[index+5]+\r
- pixcel_temp[index+6]+pixcel_temp[index+7]+pixcel_temp[index+8]+\r
- pixcel_temp[index+9]+pixcel_temp[index+10]+pixcel_temp[index+11])/(4*3);\r
- o_bitbuffer.setBitByBitIndex(p,pixel>i_th?1:0);\r
- p++;\r
- }\r
- }\r
- return true;\r
- }\r
- \r
- public boolean setSquare(NyARIntPoint2d[] i_vertex) throws NyARException\r
- {\r
- if (!getParam(i_vertex,this._cparam)) {\r
- return false;\r
- }\r
- return true;\r
- }\r
-}\r
-\r
-/**\r
- * 遠近法を使ったパースペクティブ補正付きのINyARColorPatt\r
- *\r
- */\r
-public class CopyOfNyARColorPatt_NyIdMarker implements INyARColorPatt\r
-{\r
- private int[] _patdata;\r
- private NyARBufferReader _buf_reader;\r
- private NyARRgbPixelReader_INT1D_GRAY_8 _pixelreader;\r
- private NyARIntSize _size;\r
- PerspectivePixelReader2 _perspective_gen;\r
- private static final int LOCAL_LT=1;\r
- /**\r
- * 例えば、64\r
- * @param i_width\r
- * 取得画像の解像度幅\r
- * @param i_height\r
- * 取得画像の解像度高さ\r
- * @param i_edge_percentage\r
- * エッジ幅の割合(ARToolKit標準と同じなら、25)\r
- */\r
- public CopyOfNyARColorPatt_NyIdMarker(int i_model)\r
- {\r
- //入力制限\r
- assert i_model==2;\r
- int resolution=i_model*2+1;\r
- this._size=new NyARIntSize(resolution,resolution);\r
- this._patdata = new int[resolution*resolution];\r
- this._buf_reader=new NyARBufferReader(this._patdata,NyARBufferReader.BUFFERFORMAT_INT1D_GLAY_8);\r
- this._pixelreader=new NyARRgbPixelReader_INT1D_GRAY_8(this._patdata,this._size);\r
- this._perspective_gen=new PerspectivePixelReader2(LOCAL_LT,LOCAL_LT,100,100);\r
- return;\r
- } \r
- \r
- public final int getWidth()\r
- {\r
- return this._size.w;\r
- }\r
- public final int getHeight()\r
- {\r
- return this._size.h;\r
- }\r
- public final NyARIntSize getSize()\r
- {\r
- return this._size;\r
- }\r
- public final INyARBufferReader getBufferReader()\r
- {\r
- return this._buf_reader;\r
- }\r
- public final INyARRgbPixelReader getRgbPixelReader()\r
- {\r
- return this._pixelreader;\r
- }\r
- \r
-\r
- public int[] vertex_x=new int[49*4];\r
- public int[] vertex_y=new int[49*4];\r
- public int[] vertex2_x=new int[400];\r
- public int[] vertex2_y=new int[400];\r
- public int[] line1=new int[45];\r
- public int[] line2=new int[45];\r
- public int[] sv=new int[4];\r
- private int[] temp_pixcel=new int[144];\r
-// private NyARSingleEdgeFinder _edge_finder=new NyARSingleEdgeFinder(12,12);\r
-\r
-\r
- /**\r
- * マーカパラメータを格納する構造体\r
- */\r
- private class TMarkerParam{\r
- int threshold;\r
- int model;\r
- private double[] index_x=new double[25*2];\r
- private double[] index_y=new double[25*2];\r
- }\r
- /**\r
- * 周期が等間隔か調べる。\r
- * 次段半周期が、前段の80%より大きく、120%未満であるものを、等間隔周期であるとみなす。\r
- * @param i_freq\r
- * @param i_width\r
- */\r
- private boolean checkFreqWidth(int[] i_freq,int i_width)\r
- {\r
- int c=i_freq[1]-i_freq[0];\r
- for(int i=1;i<i_width*2-1;i++){\r
- final int n=i_freq[i+1]-i_freq[i];\r
- final int v=n*100/c;\r
- if(v>150 || v<50){\r
- return false;\r
- }\r
- c=n;\r
- }\r
- return true;\r
- }\r
- private static int MIN_FREQ=3;\r
- private static int MAX_FREQ=10;\r
- \r
- private int getRowFreq(int i_y,int i_th_h,int i_th_l,int[] o_freq_index)throws NyARException\r
- {\r
- //3,4,5,6,7,8,9,10\r
- int freq_index1[]=new int[45];\r
- int freq_count_table[]=new int[10];\r
- //0,2,4,6,8,10,12,14,16,18,20の要素を持つ配列\r
- int freq_table[][]=new int[10][20];\r
- //初期化\r
- \r
- //10-20ピクセル目からタイミングパターンを検出\r
- for(int i=i_y;i<i_y+10;i++){\r
- final int freq_t=this._perspective_gen.getRowFrequency(i,i_th_h,i_th_l,freq_index1);\r
- //周期は3-10であること\r
- if(freq_t<MIN_FREQ || freq_t>MAX_FREQ){\r
- continue;\r
- }\r
- //周期は等間隔であること\r
- if(!checkFreqWidth(freq_index1,freq_t)){\r
- continue;\r
- }\r
- //検出カウンタを追加\r
- freq_count_table[freq_t]++;\r
- for(int i2=0;i2<freq_t*2;i2++){\r
- freq_table[freq_t][i2]+=freq_index1[i2];\r
- }\r
- }\r
- //一番成分の大きいものを得る\r
- int index=-1;\r
- int max=0;\r
- for(int i=0;i<MAX_FREQ;i++){\r
- if(max<freq_count_table[i]){\r
- index=i;\r
- max=freq_count_table[i];\r
- }\r
- }\r
- /*周波数インデクスを計算*/\r
- for(int i=0;i<index*2;i++)\r
- {\r
- o_freq_index[i]=freq_table[index][i]/max;\r
- }\r
- return index;\r
- }\r
- private int getColFreq(int i_y,int i_th_h,int i_th_l,int[] o_freq_index)throws NyARException\r
- {\r
- //3,4,5,6,7,8,9,10\r
- int freq_index1[]=new int[45];\r
- int freq_count_table[]=new int[10];\r
- int freq_table[][]=new int[10][20];\r
- //初期化\r
- \r
- //10-20ピクセル目からタイミングパターンを検出\r
- for(int i=i_y;i<i_y+10;i++){\r
- final int freq_t=this._perspective_gen.getColFrequency(i,i_th_h,i_th_l,freq_index1);\r
- //周期は3-10であること\r
- if(freq_t<MIN_FREQ || freq_t>MAX_FREQ){\r
- continue;\r
- }\r
- //周期は等間隔であること\r
- if(!checkFreqWidth(freq_index1,freq_t)){\r
- continue;\r
- }\r
- //検出カウンタを追加\r
- freq_count_table[freq_t]++;\r
- for(int i2=0;i2<freq_t*2;i2++){\r
- freq_table[freq_t][i2]+=freq_index1[i2];\r
- }\r
- }\r
- //一番成分の大きいものを得る\r
- int index=-1;\r
- int max=0;\r
- for(int i=0;i<MAX_FREQ;i++){\r
- if(max<freq_count_table[i]){\r
- index=i;\r
- max=freq_count_table[i];\r
- }\r
- }\r
- for(int i=0;i<10;i++){\r
- System.out.print(freq_count_table[i]+",");\r
- }\r
- System.out.println();\r
- if(index==-1){\r
- return -1;\r
- }\r
- /*周波数インデクスを計算*/\r
- for(int i=0;i<index*2;i++)\r
- {\r
- o_freq_index[i]=freq_table[index][i]/max;\r
- }\r
- return index;\r
- }\r
- private boolean detectMarkerParam(INyARRgbPixelReader i_reader,TMarkerParam o_param)throws NyARException\r
- {\r
- TThreshold th=new TThreshold();\r
- \r
- detectThresholdValue(i_reader,10,10,th);\r
- //周波数測定(最も正しい周波数を検出する)\r
-\r
- \r
-\r
-\r
- //左上,右下から、12x12(抽出範囲は10x10)の調査用矩形を抽出\r
-\r
-\r
- \r
- //周波数を測定\r
- int freq_index1[]=new int[45];\r
- int freq_index2[]=new int[45];\r
- int frq_t=getRowFreq(10,th.th_h,th.th_l,freq_index1);\r
- int frq_b=getRowFreq(80,th.th_h,th.th_l,freq_index2);\r
- //周波数はまとも？\r
- if((frq_t<0 && frq_b<0) || frq_t==frq_b){\r
- System.out.print("FRQ_R");\r
- return false;\r
- }\r
- //タイミングパターンからインデクスを作成\r
- int freq;\r
- int[] index;\r
- if(frq_t>frq_b){\r
- freq=frq_t;\r
- index=freq_index1;\r
- }else{\r
- freq=frq_b;\r
- index=freq_index2;\r
- }\r
- for(int i=0;i<freq*2-1;i++){\r
- o_param.index_x[i*2]=((index[i+1]-index[i])*2/5+index[i])*2+PerspectivePixelReader.FRQ_EDGE;\r
- o_param.index_x[i*2+1]=((index[i+1]-index[i])*3/5+index[i])*2+PerspectivePixelReader.FRQ_EDGE;\r
- }\r
- int frq_l=getColFreq(10,th.th_h,th.th_l,freq_index1);\r
- int frq_r=getColFreq(80,th.th_h,th.th_l,freq_index2); \r
- //周波数はまとも？\r
- if((frq_l<0 && frq_r<0) || frq_l==frq_r){\r
- System.out.print("FRQ_C");\r
- return false;\r
- }\r
- //タイミングパターンからインデクスを作成\r
- if(frq_l>frq_r){\r
- freq=frq_l;\r
- index=freq_index1;\r
- }else{\r
- freq=frq_r;\r
- index=freq_index2;\r
- }\r
- for(int i=0;i<freq*2-1;i++){\r
- o_param.index_y[i*2]=((index[i+1]-index[i])*2/5+index[i])*2+PerspectivePixelReader.FRQ_EDGE;\r
- o_param.index_y[i*2+1]=((index[i+1]-index[i])*3/5+index[i])*2+PerspectivePixelReader.FRQ_EDGE;\r
- }\r
-\r
- \r
- \r
- //Lv4以上は無理\r
- if(freq>4){\r
- System.out.print("LVL_4");\r
- return false;\r
- }\r
- o_param.model=freq-1;\r
- o_param.threshold=th.th;\r
- return true;\r
- \r
- }\r
- private class TThreshold{\r
- public int th_h;\r
- public int th_l;\r
- public int th;\r
- }\r
- /**\r
- * 指定した場所のピクセル値を調査して、閾値を計算して返します。\r
- * @param i_reader\r
- * @param i_x\r
- * @param i_y\r
- * @return\r
- * @throws NyARException\r
- */\r
- private void detectThresholdValue(INyARRgbPixelReader i_reader,int i_x,int i_y,TThreshold o_threshold)throws NyARException\r
- {\r
- final int[] temp_pixel=this.temp_pixcel;\r
- //特定エリアから画素を得る\r
- this._perspective_gen.rectPixels(i_x,i_y,10,10,temp_pixel);\r
- //ソート\r
- int len=100;\r
- int h = len *13/10;\r
- for(;;){\r
- int swaps = 0;\r
- for (int i = 0; i + h < len; i++) {\r
- if (temp_pixel[i + h] > temp_pixel[i]) {\r
- final int temp = temp_pixel[i + h];\r
- temp_pixel[i + h] = temp_pixel[i];\r
- temp_pixel[i] = temp;\r
- swaps++;\r
- }\r
- }\r
- if (h == 1) {\r
- if (swaps == 0){\r
- break;\r
- }\r
- }else{\r
- h=h*10/13;\r
- }\r
- }\r
- //値の大きい方と小さい方の各4点を取得\r
- //4点の中間を閾値中心とする\r
- final int th_l=temp_pixel[99]+temp_pixel[98]+temp_pixel[97]+temp_pixel[96];\r
- final int th_h=temp_pixel[0]+temp_pixel[1]+temp_pixel[2]+temp_pixel[3];\r
- int th_sub=(th_h-th_l)/(4*5);//ヒステリシス(20%)\r
- int th=(th_h+th_l)/8;\r
- o_threshold.th=th;\r
- o_threshold.th_h=th+th_sub;//ヒステリシス付き閾値\r
- o_threshold.th_l=th-th_sub;//ヒステリシス付き閾値\r
- System.out.println(o_threshold.th+","+o_threshold.th_h+","+o_threshold.th_l);\r
- return;\r
- } \r
- \r
-\r
- /**\r
- * \r
- * @param image\r
- * @param i_marker\r
- * @return 切り出しに失敗した\r
- * @throws Exception\r
- */\r
- public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException\r
- {\r
- this._perspective_gen.setSourceRaster(image);\r
- i_square.imvertex[0].x=149;\r
- i_square.imvertex[0].y=179;\r
- i_square.imvertex[0].x=27;\r
- i_square.imvertex[0].y=96;\r
- i_square.imvertex[0].x=152;\r
- i_square.imvertex[0].y=29;\r
- i_square.imvertex[0].x=280;\r
- i_square.imvertex[0].y=72;\r
- \r
- //遠近法のパラメータを計算\r
- if(!this._perspective_gen.setSourceSquare(i_square.imvertex)){\r
- return false;\r
- };\r
- \r
- final INyARRgbPixelReader reader=image.getRgbPixelReader();\r
-\r
-\r
- TMarkerParam param=new TMarkerParam();\r
- MarkerPattEncoder encoder=new MarkerPattEncoder();\r
- //マーカパラメータを取得\r
- if(!detectMarkerParam(reader,param))\r
- {\r
- System.out.println("E");\r
- return false;\r
- } \r
- final int resolution=(param.model*2)+1;\r
- //Threshold検出\r
-\r
- if(!encoder.initEncoder(param.model)){\r
- return false;\r
- }\r
- //int i_stx,int i_sty,int size_x,int size_y,int i_resolution,int i_th,\r
- //this._perspective_gen.readDataBits(st_x,st_y,size_x,size_y,resolution, param.threshold, encoder);\r
- this._perspective_gen.readDataBits(param.index_x,param.index_y,resolution, param.threshold, encoder);\r
- encoder.encode();\r
-\r
- for(int i=0;i<49*4;i++){\r
- this.vertex_x[i]=0;\r
- this.vertex_y[i]=0;\r
- }\r
- for(int i=0;i<resolution*2;i++){\r
- for(int i2=0;i2<resolution*2;i2++){\r
- this.vertex_x[i*resolution*2+i2]=(int)param.index_x[i2];\r
- this.vertex_y[i*resolution*2+i2]=(int)param.index_y[i];\r
- \r
- }\r
- }\r
-\r
- System.out.println(param.model);\r
-/*\r
- //マップを作りなおす\r
- for(int i=0;i<size*size;i++){\r
- this._patdata[i]=encoder.getBit(_role_table[i])==0?0:255;\r
- }\r
-*/ \r
- \r
- \r
- return true;\r
- }\r
- public static void main(String[] args)\r
- {\r
- try {\r
- double[] a=new double[10];\r
- int[] d={\r
- // 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9\r
- 0,0,0,0,0,0,0,1,5,1,2,0,0,0,1,2,5,2,1,0,\r
- 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};\r
- NyARSingleEdgeFinder px=new NyARSingleEdgeFinder(20,2);\r
- System.out.print(px.scanEdgeLeftToRight(d,0,0,a));\r
- System.out.print("");\r
- } catch (Exception e) {\r
- e.printStackTrace();\r
- }\r
-\r
- } \r
-}
\ No newline at end of file
--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit JOGL sample program.\r
+ * --------------------------------------------------------------------------------\r
+ * The MIT License\r
+ * Copyright (c) 2008 nyatla\r
+ * airmail(at)ebony.plala.or.jp\r
+ * http://nyatla.jp/nyartoolkit/\r
+ * \r
+ * Permission is hereby granted, free of charge, to any person obtaining a copy\r
+ * of this software and associated documentation files (the "Software"), to deal\r
+ * in the Software without restriction, including without limitation the rights\r
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r
+ * copies of the Software, and to permit persons to whom the Software is\r
+ * furnished to do so, subject to the following conditions:\r
+ * The above copyright notice and this permission notice shall be included in\r
+ * all copies or substantial portions of the Software.\r
+ * \r
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\r
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\r
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\r
+ * THE SOFTWARE.\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.dev;\r
+\r
+import java.awt.event.*;\r
+import java.awt.*;\r
+import javax.media.Buffer;\r
+import javax.media.opengl.*;\r
+import com.sun.opengl.util.*;\r
+import jp.nyatla.nyartoolkit.*;\r
+import jp.nyatla.nyartoolkit.core.*;\r
+import jp.nyatla.nyartoolkit.core.param.*;\r
+import jp.nyatla.nyartoolkit.core.transmat.*;\r
+import jp.nyatla.nyartoolkit.detector.*;\r
+import jp.nyatla.nyartoolkit.jmf.utils.*;\r
+import jp.nyatla.nyartoolkit.jogl.utils.*;\r
+class Program implements JmfCaptureListener\r
+{\r
+ private OptimizeView _view1;\r
+ private OptimizeView _view2;\r
+ public Object _sync_object=new Object(); \r
+ public NyARParam _ar_param;\r
+ public NyARCode _ar_code;\r
+ private final static int SCREEN_X = 320;\r
+ private final static int SCREEN_Y = 240;\r
+ private JmfCaptureDevice _capture;\r
+ public GLNyARRaster_RGB _cap_image;\r
+ public Program(NyARParam i_param, NyARCode i_ar_code) throws NyARException\r
+ {\r
+ // キャプチャの準備\r
+ JmfCaptureDeviceList devlist = new JmfCaptureDeviceList();\r
+ this._capture = devlist.getDevice(0);\r
+ if (!this._capture.setCaptureFormat(SCREEN_X, SCREEN_Y, 30.0f)) {\r
+ throw new NyARException();\r
+ }\r
+ this._ar_param=i_param;\r
+ this._ar_code=i_ar_code;\r
+ this._capture.setOnCapture(this);\r
+ // GL対応のRGBラスタオブジェクト\r
+ this._cap_image = new GLNyARRaster_RGB(i_param, this._capture.getCaptureFormat()); \r
+ this._view1=new OptimizeView(this,NyARSingleDetectMarker.PF_NYARTOOLKIT);\r
+ this._view2=new OptimizeView(this,NyARSingleDetectMarker.PF_TEST1);\r
+ this._capture.start();\r
+ return;\r
+ }\r
+ public void onUpdateBuffer(Buffer i_buffer)\r
+ {\r
+ try {\r
+ synchronized (this._sync_object) {\r
+ this._cap_image.setBuffer(i_buffer);\r
+ this._view1.updateCapture(this._cap_image);\r
+ this._view2.updateCapture(this._cap_image);\r
+ }\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ }\r
+ //GL API\r
+ void glDrawCube(GL i_gl)\r
+ {\r
+ // Colour cube data.\r
+ int polyList = 0;\r
+ float fSize = 0.5f;// マーカーサイズに対して0.5倍なので、4cmの立方体\r
+ int f, i;\r
+ float[][] cube_vertices = new float[][] { { 1.0f, 1.0f, 1.0f }, { 1.0f, -1.0f, 1.0f }, { -1.0f, -1.0f, 1.0f }, { -1.0f, 1.0f, 1.0f }, { 1.0f, 1.0f, -1.0f }, { 1.0f, -1.0f, -1.0f }, { -1.0f, -1.0f, -1.0f }, { -1.0f, 1.0f, -1.0f } };\r
+ float[][] cube_vertex_colors = new float[][] { { 1.0f, 1.0f, 1.0f }, { 1.0f, 1.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, { 0.0f, 1.0f, 1.0f }, { 1.0f, 0.0f, 1.0f }, { 1.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 1.0f } };\r
+ int cube_num_faces = 6;\r
+ short[][] cube_faces = new short[][] { { 3, 2, 1, 0 }, { 2, 3, 7, 6 }, { 0, 1, 5, 4 }, { 3, 0, 4, 7 }, { 1, 2, 6, 5 }, { 4, 5, 6, 7 } };\r
+\r
+ if (polyList == 0) {\r
+ polyList = i_gl.glGenLists(1);\r
+ i_gl.glNewList(polyList, GL.GL_COMPILE);\r
+ i_gl.glBegin(GL.GL_QUADS);\r
+ for (f = 0; f < cube_num_faces; f++)\r
+ for (i = 0; i < 4; i++) {\r
+ i_gl.glColor3f(cube_vertex_colors[cube_faces[f][i]][0], cube_vertex_colors[cube_faces[f][i]][1], cube_vertex_colors[cube_faces[f][i]][2]);\r
+ i_gl.glVertex3f(cube_vertices[cube_faces[f][i]][0] * fSize, cube_vertices[cube_faces[f][i]][1] * fSize, cube_vertices[cube_faces[f][i]][2] * fSize);\r
+ }\r
+ i_gl.glEnd();\r
+ i_gl.glColor3f(0.0f, 0.0f, 0.0f);\r
+ for (f = 0; f < cube_num_faces; f++) {\r
+ i_gl.glBegin(GL.GL_LINE_LOOP);\r
+ for (i = 0; i < 4; i++)\r
+ i_gl.glVertex3f(cube_vertices[cube_faces[f][i]][0] * fSize, cube_vertices[cube_faces[f][i]][1] * fSize, cube_vertices[cube_faces[f][i]][2] * fSize);\r
+ i_gl.glEnd();\r
+ }\r
+ i_gl.glEndList();\r
+ }\r
+\r
+ i_gl.glPushMatrix(); // Save world coordinate system.\r
+ i_gl.glTranslatef(0.0f, 0.0f, 0.5f); // Place base of cube on marker surface.\r
+ i_gl.glRotatef(0.0f, 0.0f, 0.0f, 1.0f); // Rotate about z axis.\r
+ i_gl.glDisable(GL.GL_LIGHTING); // Just use colours.\r
+ i_gl.glCallList(polyList); // Draw the cube.\r
+ i_gl.glPopMatrix(); // Restore world coordinate system.\r
+ return;\r
+ }\r
+ \r
+}\r
+/**\r
+ * simpleLiteと同じようなテストプログラム 出来る限りARToolKitのサンプルと似せて作ってあります。 最も一致する"Hiro"マーカーを一つ選択して、その上に立方体を表示します。\r
+ * \r
+ */\r
+public class OptimizeView implements GLEventListener\r
+{\r
+ private final static int SCREEN_X = 320;\r
+\r
+ private final static int SCREEN_Y = 240;\r
+\r
+ private Animator _animator;\r
+ \r
+ private Program _parent;\r
+\r
+\r
+// private JmfCaptureDevice _capture;\r
+\r
+ private GL _gl;\r
+ private NyARGLUtil _glnya;\r
+\r
+ // NyARToolkit関係\r
+ private NyARSingleDetectMarker _nya;\r
+ private NyARParam _ar_param;\r
+\r
+ private double[] _camera_projection = new double[16];\r
+\r
+\r
+ public OptimizeView(Program i_program,int i_pf) throws NyARException\r
+ {\r
+ this._parent=i_program;\r
+ this._ar_param = i_program._ar_param;\r
+\r
+ Frame frame = new Frame("["+i_pf+"]");\r
+\r
+ // NyARToolkitの準備\r
+ this._nya = new NyARSingleDetectMarker(this._ar_param, i_program._ar_code, 80.0,i_program._cap_image.getBufferReader().getBufferType(),i_pf);\r
+ this._nya.setContinueMode(true);// ここをtrueにすると、transMatContinueモード（History計算）になります。\r
+ \r
+ // 3Dを描画するコンポーネント\r
+ GLCanvas canvas = new GLCanvas();\r
+ frame.add(canvas);\r
+ canvas.addGLEventListener(this);\r
+ frame.addWindowListener(new WindowAdapter() {\r
+ public void windowClosing(WindowEvent e)\r
+ {\r
+ System.exit(0);\r
+ }\r
+ });\r
+\r
+ frame.setVisible(true);\r
+ Insets ins = frame.getInsets();\r
+ frame.setSize(SCREEN_X + ins.left + ins.right, SCREEN_Y + ins.top + ins.bottom);\r
+ canvas.setBounds(ins.left, ins.top, SCREEN_X, SCREEN_Y);\r
+ }\r
+\r
+ public void init(GLAutoDrawable drawable)\r
+ {\r
+ this._gl = drawable.getGL();\r
+ this._gl.glEnable(GL.GL_DEPTH_TEST);\r
+ this._gl.glClearColor(0.0f, 0.0f, 0.0f, 0.0f);\r
+ // NyARToolkitの準備\r
+ try {\r
+ // NyARToolkit用の支援クラス\r
+ _glnya = new NyARGLUtil(_gl);\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ // カメラパラメータの計算\r
+ this._glnya.toCameraFrustumRH(this._ar_param,this._camera_projection);\r
+ this._animator = new Animator(drawable);\r
+ this._animator.start();\r
+ return;\r
+ }\r
+\r
+ public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height)\r
+ {\r
+ _gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);\r
+ _gl.glViewport(0, 0, width, height);\r
+\r
+ // 視体積の設定\r
+ _gl.glMatrixMode(GL.GL_PROJECTION);\r
+ _gl.glLoadIdentity();\r
+ // 見る位置\r
+ _gl.glMatrixMode(GL.GL_MODELVIEW);\r
+ _gl.glLoadIdentity();\r
+ }\r
+\r
+ private boolean _is_marker_exist=false;\r
+ private NyARTransMatResult __display_transmat_result = new NyARTransMatResult();\r
+\r
+ private double[] __display_wk = new double[16];\r
+\r
+ public void display(GLAutoDrawable drawable)\r
+ {\r
+ NyARTransMatResult transmat_result = __display_transmat_result;\r
+ if (!this._parent._cap_image.hasData()) {\r
+ return;\r
+ }\r
+ // 背景を書く\r
+ this._gl.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT); // Clear the buffers for new frame.\r
+ this._glnya.drawBackGround(this._parent._cap_image, 1.0); \r
+ try{\r
+ synchronized(this._parent._sync_object){\r
+ // マーカーがあれば、立方体を描画\r
+ if (this._is_marker_exist){\r
+ // マーカーの一致度を調査するならば、ここでnya.getConfidence()で一致度を調べて下さい。\r
+ // Projection transformation.\r
+ _gl.glMatrixMode(GL.GL_PROJECTION);\r
+ _gl.glLoadMatrixd(_camera_projection, 0);\r
+ _gl.glMatrixMode(GL.GL_MODELVIEW);\r
+ // Viewing transformation.\r
+ _gl.glLoadIdentity();\r
+ // 変換行列を取得\r
+ _nya.getTransmationMatrix(transmat_result);\r
+ // 変換行列をOpenGL形式に変換\r
+ _glnya.toCameraViewRH(transmat_result, __display_wk);\r
+ _gl.glLoadMatrixd(__display_wk, 0);\r
+ \r
+ // All other lighting and geometry goes here.\r
+ this._parent.glDrawCube(_gl);\r
+ }\r
+ }\r
+ Thread.sleep(1);// タスク実行権限を一旦渡す\r
+ }catch(Exception e){\r
+ e.printStackTrace();\r
+ }\r
+\r
+ }\r
+\r
+ public void updateCapture(GLNyARRaster_RGB i_img)\r
+ {\r
+ try {\r
+ synchronized (this._parent._sync_object) {\r
+ this._is_marker_exist =this._nya.detectMarkerLite(i_img, 110);\r
+ }\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ }\r
+\r
+ public void displayChanged(GLAutoDrawable drawable, boolean modeChanged, boolean deviceChanged)\r
+ {\r
+ }\r
+\r
+ private final static String CARCODE_FILE = "../Data/patt.hiro";\r
+\r
+ private final static String PARAM_FILE = "../Data/camera_para.dat";\r
+\r
+ public static void main(String[] args)\r
+ {\r
+ try {\r
+ NyARParam param = new NyARParam();\r
+ param.loadARParamFromFile(PARAM_FILE);\r
+ param.changeScreenSize(SCREEN_X, SCREEN_Y);\r
+\r
+ NyARCode code = new NyARCode(16, 16);\r
+ code.loadARPattFromFile(CARCODE_FILE);\r
+\r
+ new Program(param, code);\r
+ } catch (Exception e) {\r
+ e.printStackTrace();\r
+ }\r
+ return;\r
+ }\r
+}\r
OSDN Git Service
