lib/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_ARToolKit.java

   1 /* \r
   2  * PROJECT: NyARToolkit\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java edition ARToolKit class library.\r
  11  * Copyright (C)2008-2009 Ryo Iizuka\r
  12  *\r
  13  * This program is free software: you can redistribute it and/or modify\r
  14  * it under the terms of the GNU General Public License as published by\r
  15  * the Free Software Foundation, either version 3 of the License, or\r
  16  * (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  *\r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this program.  If not, see <http://www.gnu.org/licenses/>.\r
  25  * \r
  26  * For further information please contact.\r
  27  *      http://nyatla.jp/nyatoolkit/\r
  28  *      <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
  29  * \r
  30  */\r
  31 package jp.nyatla.nyartoolkit.core.transmat;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.param.*;\r
  35 import jp.nyatla.nyartoolkit.core.squaredetect.NyARSquare;\r
  36 import jp.nyatla.nyartoolkit.core.transmat.solver.*;\r
  37 import jp.nyatla.nyartoolkit.core.transmat.optimize.artoolkit.INyARRotMatrixOptimize;\r
  38 import jp.nyatla.nyartoolkit.core.transmat.optimize.artoolkit.NyARRotMatrixOptimize_O2;\r
  39 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
  40 import jp.nyatla.nyartoolkit.core.types.*;\r
  41 \r
  42 \r
  43 /**\r
  44  * このクラスは、ARToolKitと同じ方法で、二次元矩形から３次元位置姿勢を推定します。\r
  45  */\r
  46 public class NyARTransMat_ARToolKit implements INyARTransMat\r
  47 {\r
  48         private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
  49         private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
  50         private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0;\r
  51 \r
  52         /** 回転行列の計算オブジェクト*/\r
  53         protected NyARRotMatrix_ARToolKit _rotmatrix;\r
  54         /** 平行移動量の計算オブジェクト*/\r
  55         protected INyARTransportVectorSolver _transsolver;\r
  56         /** 姿勢行列最適化オブジェクト*/\r
  57         protected INyARRotMatrixOptimize _mat_optimize;\r
  58         private NyARCameraDistortionFactor _ref_dist_factor;\r
  59 \r
  60         /**\r
  61          * コンストラクタです。\r
  62          * 派生クラスで自分でメンバオブジェクトを指定したい場合はこちらを使います。\r
  63          */\r
  64         protected NyARTransMat_ARToolKit()\r
  65         {\r
  66                 //_calculator,_rotmatrix,_mat_optimizeをコンストラクタの終了後に\r
  67                 //作成して割り当ててください。\r
  68                 return;\r
  69         }\r
  70         /**\r
  71          * コンストラクタです。\r
  72          * 座標計算に必要なオブジェクトの参照値を元に、インスタンスを生成します。\r
  73          * @param i_ref_distfactor\r
  74          * 樽型歪み矯正オブジェクトの参照値です。歪み矯正が不要な時は、nullを指定します。\r
  75          * @param i_ref_projmat\r
  76          * 射影変換オブジェクトの参照値です。\r
  77          * @throws NyARException\r
  78          */\r
  79         public NyARTransMat_ARToolKit(NyARCameraDistortionFactor i_ref_distfactor,NyARPerspectiveProjectionMatrix i_ref_projmat) throws NyARException\r
  80         {\r
  81                 initInstance(i_ref_distfactor,i_ref_projmat);\r
  82                 return;\r
  83         }\r
  84         /**\r
  85          * コンストラクタです。\r
  86          * 座標計算に必要なカメラパラメータの参照値を元に、インスタンスを生成します。\r
  87          * @param i_param\r
  88          * ARToolKit形式のカメラパラメータです。\r
  89          * インスタンスは、この中から樽型歪み矯正オブジェクト、射影変換オブジェクトを参照します。\r
  90          * @throws NyARException\r
  91          */     \r
  92         public NyARTransMat_ARToolKit(NyARParam i_param) throws NyARException\r
  93         {\r
  94                 initInstance(i_param.getDistortionFactor(),i_param.getPerspectiveProjectionMatrix());\r
  95         }\r
  96         private void initInstance(NyARCameraDistortionFactor i_ref_distfactor,NyARPerspectiveProjectionMatrix i_ref_projmat) throws NyARException\r
  97         {\r
  98                 final NyARCameraDistortionFactor dist=i_ref_distfactor;\r
  99                 final NyARPerspectiveProjectionMatrix pmat=i_ref_projmat;\r
 100                 this._transsolver=new NyARTransportVectorSolver_ARToolKit(pmat);\r
 101                 //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。\r
 102                 //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。\r
 103                 this._rotmatrix = new NyARRotMatrix_ARToolKit_O2(pmat);\r
 104                 this._mat_optimize=new NyARRotMatrixOptimize_O2(pmat);\r
 105                 this._ref_dist_factor=dist;\r
 106         }\r
 107 \r
 108         private final NyARDoublePoint2d[] __transMat_vertex_2d = NyARDoublePoint2d.createArray(4);\r
 109         private final NyARDoublePoint3d[] __transMat_vertex_3d = NyARDoublePoint3d.createArray(4);\r
 110         private final NyARDoublePoint3d __transMat_trans=new NyARDoublePoint3d();\r
 111         /**\r
 112          * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。\r
 113          * ARToolKitのarGetTransMatに該当します。\r
 114          * @see INyARTransMat#transMatContinue\r
 115          */\r
 116         public final void transMat(NyARSquare i_square,NyARRectOffset i_offset, NyARTransMatResult o_result_conv) throws NyARException\r
 117         {\r
 118                 final NyARDoublePoint3d trans=this.__transMat_trans;\r
 119                 \r
 120                 //平行移動量計算機に、2D座標系をセット\r
 121                 NyARDoublePoint2d[] vertex_2d;\r
 122                 if(this._ref_dist_factor!=null){\r
 123                         //歪み復元必要\r
 124                         vertex_2d=this.__transMat_vertex_2d;\r
 125                         this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d,4);\r
 126                 }else{\r
 127                         //歪み復元は不要\r
 128                         vertex_2d=i_square.sqvertex;\r
 129                 }\r
 130                 this._transsolver.set2dVertex(vertex_2d,4);\r
 131                 //回転行列を計算\r
 132                 this._rotmatrix.initRotBySquare(i_square.line,i_square.sqvertex);\r
 133                 \r
 134                 //回転後の3D座標系から、平行移動量を計算\r
 135                 NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
 136                 this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);\r
 137                 this._transsolver.solveTransportVector(vertex_3d,trans);\r
 138                 \r
 139                 //計算結果の最適化(平行移動量と回転行列の最適化)\r
 140                 double err=this.optimize(this._rotmatrix, trans, this._transsolver,i_offset.vertex, vertex_2d);\r
 141                 \r
 142                 // マトリクスの保存\r
 143                 o_result_conv.setValue(this._rotmatrix,trans,err);\r
 144                 return;\r
 145         }\r
 146         /**\r
 147          * この関数は、理想座標系の四角系を元に、位置姿勢変換行列を求めます。\r
 148          * 計算に過去の履歴を使う点が、{@link #transMat}と異なります。\r
 149          * @see INyARTransMat#transMatContinue\r
 150          */\r
 151         public final void transMatContinue(NyARSquare i_square,NyARRectOffset i_offset,NyARTransMatResult i_prev_result,NyARTransMatResult o_result) throws NyARException\r
 152         {\r
 153                 final NyARDoublePoint3d trans=this.__transMat_trans;\r
 154 \r
 155                 // i_prev_resultが初期値なら、transMatで計算する。\r
 156                 if (!i_prev_result.has_value) {\r
 157                         this.transMat(i_square, i_offset, o_result);\r
 158                         return;\r
 159                 }\r
 160                 \r
 161                 //平行移動量計算機に、2D座標系をセット\r
 162                 NyARDoublePoint2d[] vertex_2d;\r
 163                 if(this._ref_dist_factor!=null){\r
 164                         //歪み復元必要\r
 165                         vertex_2d=this.__transMat_vertex_2d;\r
 166                         this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d,4);\r
 167                 }else{\r
 168                         //歪み復元は不要\r
 169                         vertex_2d=i_square.sqvertex;\r
 170                 }\r
 171                 this._transsolver.set2dVertex(vertex_2d,4);\r
 172 \r
 173                 NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
 174                 //回転行列を計算\r
 175                 this._rotmatrix.initRotByPrevResult(i_prev_result);\r
 176                 \r
 177                 //回転後の3D座標系から、平行移動量を計算\r
 178                 this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);\r
 179                 this._transsolver.solveTransportVector(vertex_3d,trans);\r
 180                 \r
 181                 //計算結果の最適化(平行移動量と回転行列の最適化)\r
 182                 double err=this.optimize(this._rotmatrix, trans, this._transsolver,i_offset.vertex, vertex_2d);\r
 183                 \r
 184                 // マトリクスの保存\r
 185                 o_result.setValue(this._rotmatrix,  trans, err);\r
 186                 \r
 187                 // エラー値が許容範囲でなければTransMatをやり直し\r
 188                 if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
 189                         // rotationを矩形情報で初期化\r
 190                         this._rotmatrix.initRotBySquare(i_square.line,i_square.sqvertex);\r
 191                         //回転行列の平行移動量の計算\r
 192                         this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);\r
 193                         this._transsolver.solveTransportVector(vertex_3d,trans);\r
 194                         //計算結果の最適化(this._rotmatrix,trans)\r
 195                         final double err2=this.optimize(this._rotmatrix, trans, this._transsolver,i_offset.vertex, vertex_2d);\r
 196                         //エラー値が低かったら値を差換え\r
 197                         if (err2 < err) {\r
 198                                 // 良い値が取れたら、差換え\r
 199                                 o_result.setValue(this._rotmatrix, trans, err2);\r
 200                         }\r
 201                         err=err2;\r
 202                 }\r
 203                 //エラー値保存\r
 204                 return;\r
 205         }\r
 206         private double optimize(NyARRotMatrix_ARToolKit io_rotmat,NyARDoublePoint3d io_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex) throws NyARException\r
 207         {\r
 208                 NyARDoublePoint3d[] vertex_3d=this.__transMat_vertex_3d;\r
 209                 double err = -1;\r
 210                 //System.out.println("START");\r
 211                 // ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。 \r
 212                 for (int i = 0;; i++) {\r
 213                         // <arGetTransMat3>\r
 214                         err = this._mat_optimize.modifyMatrix(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex);\r
 215                         io_rotmat.getPoint3dBatch(i_offset_3d,vertex_3d,4);\r
 216                         i_solver.solveTransportVector(vertex_3d, io_transvec);\r
 217                         \r
 218                         err = this._mat_optimize.modifyMatrix(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex);\r
 219                         //System.out.println("E:"+err*4);\r
 220                         // //</arGetTransMat3>\r
 221                         if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT - 1) {\r
 222                                 break;\r
 223                         }\r
 224                         io_rotmat.getPoint3dBatch(i_offset_3d,vertex_3d,4);\r
 225                         i_solver.solveTransportVector(vertex_3d, io_transvec);\r
 226                 }\r
 227                 //System.out.println("END");\r
 228                 return err;\r
 229         }       \r
 230 }\r