branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O2.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 version ARToolkit class library.\r
  11  * Copyright (C)2008 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (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 framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core.transmat;\r
  33 \r
  34 \r
  35 \r
  36 import jp.nyatla.nyartoolkit.NyARException;\r
  37 import jp.nyatla.nyartoolkit.core.NyARMat;\r
  38 import jp.nyatla.nyartoolkit.core.NyARParam;\r
  39 import jp.nyatla.nyartoolkit.core.NyARSquare;\r
  40 import jp.nyatla.util.DoubleValue;\r
  41 \r
  42 \r
  43 \r
  44 \r
  45 \r
  46 /**\r
  47  * This class calculates ARMatrix from square information and holds it.\r
  48  * --\r
  49  * 変換行列を計算して、結果を保持するクラス。\r
  50  *\r
  51  */\r
  52 public class NyARTransMat_O2 implements INyARTransMat\r
  53 {\r
  54     private final static int AR_FITTING_TO_IDEAL=0;//#define  AR_FITTING_TO_IDEAL          0\r
  55     private final static int AR_FITTING_TO_INPUT=1;//#define  AR_FITTING_TO_INPUT          1\r
  56     private final static int    arFittingMode   =AR_FITTING_TO_INPUT;\r
  57 \r
  58     private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define   AR_GET_TRANS_MAT_MAX_LOOP_COUNT         5\r
  59     private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define   AR_GET_TRANS_MAT_MAX_FIT_ERROR          1.0\r
  60     private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR=1.0;\r
  61     private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX       500\r
  62     private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
  63     private final NyARTransRot transrot;\r
  64     private final double[] center={0.0,0.0};\r
  65     private final NyARParam param;\r
  66     private final NyARMat result_mat=new NyARMat(3,4);\r
  67     public NyARTransMat_O2(NyARParam i_param)throws NyARException\r
  68     {\r
  69         param=i_param;\r
  70         transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
  71 \r
  72     }\r
  73     public void setCenter(double i_x,double i_y)\r
  74     {\r
  75         center[0]=i_x;\r
  76         center[1]=i_x;\r
  77     }\r
  78     public NyARMat getTransformationMatrix()\r
  79     {\r
  80         return result_mat;\r
  81     }\r
  82 \r
  83     /**\r
  84      * transMat関数の初期化関数を分離したものです。\r
  85      * @param square\r
  86      * @param i_direction\r
  87      * @param i_width\r
  88      * @param o_ppos2d\r
  89      * @param o_ppos3d\r
  90      */\r
  91     private final void init_transMat_ppos(NyARSquare square,int i_direction,double i_width,double[][] o_ppos2d,double[][] o_ppos3d)\r
  92     {   \r
  93         o_ppos2d[0][0] = square.sqvertex[(4-i_direction)%4][0];\r
  94         o_ppos2d[0][1] = square.sqvertex[(4-i_direction)%4][1];\r
  95         o_ppos2d[1][0] = square.sqvertex[(5-i_direction)%4][0];\r
  96         o_ppos2d[1][1] = square.sqvertex[(5-i_direction)%4][1];\r
  97         o_ppos2d[2][0] = square.sqvertex[(6-i_direction)%4][0];\r
  98         o_ppos2d[2][1] = square.sqvertex[(6-i_direction)%4][1];\r
  99         o_ppos2d[3][0] = square.sqvertex[(7-i_direction)%4][0];\r
 100         o_ppos2d[3][1] = square.sqvertex[(7-i_direction)%4][1];\r
 101         \r
 102         double c0,c1,w_2;\r
 103         c0=center[0];\r
 104         c1=center[1];\r
 105         w_2 =i_width/2.0;\r
 106         \r
 107         o_ppos3d[0][0] = c0 - w_2;//center[0] - w/2.0;\r
 108         o_ppos3d[0][1] = c1 + w_2;//center[1] + w/2.0;\r
 109         o_ppos3d[1][0] = c0 + w_2;//center[0] + w/2.0;\r
 110         o_ppos3d[1][1] = c1 + w_2;//center[1] + w/2.0;\r
 111         o_ppos3d[2][0] = c0 + w_2;//center[0] + w/2.0;\r
 112         o_ppos3d[2][1] = c1 - w_2;//center[1] - w/2.0;\r
 113         o_ppos3d[3][0] = c0 - w_2;//center[0] - w/2.0;\r
 114         o_ppos3d[3][1] = c1 - w_2;//center[1] - w/2.0;\r
 115         return;\r
 116     }\r
 117     \r
 118     private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];\r
 119     private final double[][] wk_transMat_ppos2d=new double[4][2];\r
 120     private final double[][] wk_transMat_ppos3d=new double[4][2];\r
 121     private final double[] wk_transMat_off=new double[3];\r
 122     private final double[][] wk_transMat_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];\r
 123     private final NyARMat wk_transMat_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
 124     private final NyARMat wk_transMat_mat_d=new NyARMat( 3, 3 );    \r
 125     private final double[] wk_transMat_mat_trans=new double[3];\r
 126 \r
 127     /**\r
 128      * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
 129      * 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。\r
 130      * 保持している変換行列を更新する。\r
 131      * @param square\r
 132      * 計算対象のNyARSquareオブジェクト\r
 133      * @param i_direction\r
 134      * @param width\r
 135      * @return\r
 136      * @throws NyARException\r
 137      */\r
 138     public double transMat(NyARSquare square,int i_direction, double width,NyARTransMatResult o_result_conv)throws NyARException\r
 139     {\r
 140         double[][]  ppos2d=wk_transMat_ppos2d;\r
 141         double[][]  ppos3d=wk_transMat_ppos3d;\r
 142         double[]    off   =wk_transMat_off;\r
 143         double[][]  pos3d =wk_transMat_pos3d;\r
 144         \r
 145         //rotationの初期化\r
 146         transrot.initRot(square,i_direction);\r
 147 \r
 148         //ppos2dとppos3dの初期化\r
 149         init_transMat_ppos(square,i_direction,width,ppos2d,ppos3d);\r
 150         \r
 151         //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
 152         double[][] pos2d=this.wk_transMat_pos2d;\r
 153         final NyARMat mat_b =this.wk_transMat_mat_b;\r
 154         final NyARMat mat_d =this.wk_transMat_mat_d;\r
 155 \r
 156         arGetTransMat3_initTransMat(ppos3d,ppos2d,pos2d,pos3d,off,mat_b,mat_d);\r
 157         \r
 158         double  err=-1;\r
 159         double[] trans=this.wk_transMat_mat_trans;\r
 160         for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){\r
 161             //<arGetTransMat3>\r
 162             err = arGetTransMatSub(pos2d, pos3d,mat_b,mat_d,trans);\r
 163 //          //</arGetTransMat3>\r
 164             if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 165                 break;\r
 166             }\r
 167         }\r
 168         //マトリクスの保存\r
 169         o_result_conv.updateMatrixValue(this.transrot,off,trans);\r
 170         return err;\r
 171     }\r
 172     private final NyARTransMatResult wk_transMatContinue_result=new NyARTransMatResult();\r
 173 \r
 174     /**\r
 175      * double arGetTransMatCont( ARMarkerInfo *marker_info, double prev_conv[3][4],double center[2], double width, double conv[3][4] )\r
 176      *     \r
 177      * @param i_square\r
 178      * @param i_direction\r
 179      * マーカーの方位を指定する。\r
 180      * @param i_width\r
 181      * @param io_result_conv\r
 182      * 計算履歴を持つNyARTransMatResultオブジェクトを指定する。\r
 183      * 履歴を持たない場合は、transMatと同じ処理を行う。\r
 184      * @return\r
 185      * @throws NyARException\r
 186      */\r
 187     public double transMatContinue(NyARSquare i_square,int i_direction, double i_width,NyARTransMatResult io_result_conv)throws NyARException\r
 188     {\r
 189         //io_result_convが初期値なら、transMatで計算する。\r
 190         if(!io_result_conv.hasValue()){\r
 191             return this.transMat(i_square, i_direction, i_width, io_result_conv);\r
 192         }\r
 193 \r
 194         double[][]  ppos2d=wk_transMat_ppos2d;\r
 195         double[][]  ppos3d=wk_transMat_ppos3d;\r
 196         double[]    off   =wk_transMat_off;\r
 197         double[][]  pos3d =wk_transMat_pos3d;\r
 198         \r
 199         //      arGetTransMatContSub計算部分\r
 200         transrot.initRotByPrevResult(io_result_conv);\r
 201 \r
 202         //ppos2dとppos3dの初期化\r
 203         init_transMat_ppos(i_square,i_direction,i_width,ppos2d,ppos3d);\r
 204         \r
 205         //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
 206         double[][] pos2d=this.wk_transMat_pos2d;\r
 207         final NyARMat mat_b =this.wk_transMat_mat_b;\r
 208         final NyARMat mat_d =this.wk_transMat_mat_d;\r
 209 \r
 210         //transMatに必要な初期値を計算\r
 211         arGetTransMat3_initTransMat(ppos3d,ppos2d,pos2d,pos3d,off,mat_b,mat_d);\r
 212         \r
 213         double  err1,err2;\r
 214         int     i;\r
 215 \r
 216         err1=err2=-1;\r
 217         double[] trans=this.wk_transMat_mat_trans;\r
 218         for( i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){\r
 219             err1 = arGetTransMatSub(pos2d, pos3d,mat_b,mat_d,trans);\r
 220             if( err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
 221                 //十分な精度を達成できたらブレーク\r
 222                 break;\r
 223             }\r
 224         }\r
 225         //値を保存\r
 226         io_result_conv.updateMatrixValue(this.transrot,off,trans);\r
 227         \r
 228         //エラー値が許容範囲でなければTransMatをやり直し\r
 229         if(err1>AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
 230             NyARTransMatResult result2=this.wk_transMatContinue_result;\r
 231             //transMatを実行(初期化値は共用)\r
 232             transrot.initRot(i_square,i_direction);\r
 233             err2 = transMat(i_square,i_direction,i_width,result2);\r
 234             //transmMatここまで\r
 235             if(err2<err1){\r
 236                 //良い値が取れたら、差換え\r
 237                 io_result_conv.copyFrom(result2);\r
 238                 err1 = err2;\r
 239             }\r
 240         }\r
 241         return err1;\r
 242     }    \r
 243     \r
 244     \r
 245     \r
 246 \r
 247     private final NyARMat wk_arGetTransMat3_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
 248 \r
 249     /**\r
 250      * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
 251      * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
 252      * @param i_ppos3d\r
 253      * 入力配列[num][3]\r
 254      * @param o_pos3d\r
 255      * 出力配列[P_MAX][3]\r
 256      * @param o_off\r
 257      * [3]\r
 258      * @throws NyARException\r
 259      */\r
 260     private final void arGetTransMat3_initTransMat(double[][] i_ppos3d,double[][] i_ppos2d,double[][] o_pos2d,double[][] o_pos3d,double[] o_off,NyARMat o_mat_b,NyARMat o_mat_d)throws NyARException\r
 261     {\r
 262         double pmax0,pmax1,pmax2,pmin0,pmin1,pmin2;\r
 263         int i;\r
 264         pmax0=pmax1=pmax2 = -10000000000.0;\r
 265         pmin0=pmin1=pmin2 =  10000000000.0;\r
 266         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 267             if( i_ppos3d[i][0] > pmax0 ){\r
 268                 pmax0 = i_ppos3d[i][0];\r
 269             }\r
 270             if( i_ppos3d[i][0] < pmin0 ){\r
 271                 pmin0 = i_ppos3d[i][0];\r
 272             }\r
 273             if( i_ppos3d[i][1] > pmax1 ){\r
 274                 pmax1 = i_ppos3d[i][1];\r
 275             }\r
 276             if( i_ppos3d[i][1] < pmin1 ){\r
 277                 pmin1 = i_ppos3d[i][1];\r
 278             }\r
 279             /*  オリジナルでもコメントアウト\r
 280                 if( ppos3d[i][2] > pmax[2] ) pmax[2] = ppos3d[i][2];\r
 281                 if( ppos3d[i][2] < pmin[2] ) pmin[2] = ppos3d[i][2];\r
 282              */  \r
 283         }\r
 284         o_off[0] = -(pmax0 + pmin0) / 2.0;\r
 285         o_off[1] = -(pmax1 + pmin1) / 2.0;\r
 286         o_off[2] = -(pmax2 + pmin2) / 2.0;\r
 287 \r
 288 \r
 289         double[] o_pos3d_pt;\r
 290         double[] i_pos_pd_pt;   \r
 291         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 292             o_pos3d_pt =o_pos3d[i];\r
 293             i_pos_pd_pt=i_ppos3d[i];\r
 294             o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
 295             o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
 296             o_pos3d_pt[2] = 0.0;\r
 297         }\r
 298         //ココから先でarGetTransMatSubの初期化処理\r
 299         //arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。\r
 300         \r
 301         if(arFittingMode == AR_FITTING_TO_INPUT ){\r
 302             //arParamIdeal2Observをバッチ処理\r
 303             param.ideal2ObservBatch(i_ppos2d,o_pos2d,NUMBER_OF_VERTEX);\r
 304         }else{\r
 305             for(i = 0; i < NUMBER_OF_VERTEX; i++ ){\r
 306                 o_pos2d[i][0] = i_ppos2d[i][0];\r
 307                 o_pos2d[i][1] = i_ppos2d[i][1];\r
 308             }\r
 309         }\r
 310 \r
 311         //変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
 312         final double cpara[]=param.get34Array();\r
 313         final NyARMat mat_a =this.wk_arGetTransMat3_mat_a;\r
 314         final double[][] a_array=mat_a.getArray();\r
 315 \r
 316         //mat_bの設定\r
 317         final double[][] b_array=o_mat_b.getArray();\r
 318 \r
 319         int x2;\r
 320         for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 321             x2=i*2;\r
 322             //</Optimize>\r
 323             a_array[x2  ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
 324             a_array[x2  ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
 325             a_array[x2  ][2]=b_array[2][x2]=cpara[0*4+2]-o_pos2d[i][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
 326             a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
 327             a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
 328             a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2]-o_pos2d[i][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
 329         }\r
 330         \r
 331         //mat_d\r
 332         o_mat_d.matrixMul(o_mat_b,mat_a);\r
 333         o_mat_d.matrixSelfInv();        \r
 334     }    \r
 335 \r
 336     private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
 337     private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
 338     private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
 339 \r
 340     /**\r
 341      * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
 342      * Optimize:2008.04.20:STEP[1033→1004]\r
 343      * @param i_ppos2d\r
 344      * @param i_pos3d\r
 345      * @param i_mat_b\r
 346      * 演算用行列b\r
 347      * @param i_mat_d\r
 348      * 演算用行列d\r
 349      * @return\r
 350      * @throws NyARException\r
 351      */\r
 352     private final double arGetTransMatSub(double i_pos2d[][],double i_pos3d[][],NyARMat i_mat_b,NyARMat i_mat_d,double[] o_trans) throws NyARException\r
 353     {\r
 354         double cpara[]=param.get34Array();\r
 355         NyARMat mat_c,mat_e,mat_f;//ARMat   *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
 356 \r
 357         double  wx, wy, wz;\r
 358         double  ret;\r
 359         int     i;\r
 360 \r
 361         mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
 362         double[][] c_array=mat_c.getArray();\r
 363         double[] rot=transrot.getArray();\r
 364         double[] i_pos3d_pt;\r
 365         int x2;\r
 366         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 367             x2=i*2;\r
 368             i_pos3d_pt=i_pos3d[i];\r
 369             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 370             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 371             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 372             c_array[x2][0]  =wz * i_pos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
 373             c_array[x2+1][0]=wz * i_pos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
 374         }\r
 375         mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
 376         mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
 377         double[][] f_array=mat_f.getArray();\r
 378 \r
 379         mat_e.matrixMul(i_mat_b, mat_c );\r
 380         mat_f.matrixMul(i_mat_d, mat_e );\r
 381 \r
 382 //      double[] trans=wk_arGetTransMatSub_trans;//double  trans[3];    \r
 383         o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 384         o_trans[1] = f_array[1][0];\r
 385         o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 386         ret =transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d);\r
 387         for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
 388             x2=i*2;\r
 389             i_pos3d_pt=i_pos3d[i];\r
 390             wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
 391             wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
 392             wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
 393             c_array[x2][0]  =wz * i_pos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
 394             c_array[x2+1][0]=wz * i_pos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
 395         }\r
 396 \r
 397         mat_e.matrixMul(i_mat_b, mat_c );\r
 398         mat_f.matrixMul(i_mat_d, mat_e );\r
 399         o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
 400         o_trans[1] = f_array[1][0];\r
 401         o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
 402         ret = transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d);\r
 403         return ret;\r
 404     }\r
 405 }\r