branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.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.pickup;\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.NyARSquare;\r
  39 import jp.nyatla.nyartoolkit.core.raster.INyARRaster;\r
  40 \r
  41 /**\r
  42  * 24ビットカラーのマーカーを保持するために使うクラスです。\r
  43  * このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
  44  * 演算順序を含む最適化をしたもの\r
  45  *\r
  46  */\r
  47 public class NyARColorPatt_O3 implements INyColorPatt\r
  48 {\r
  49     private static final int AR_PATT_SAMPLE_NUM=64;//#define   AR_PATT_SAMPLE_NUM   64\r
  50     private int extpat[][][];\r
  51     private int width;\r
  52     private int height;\r
  53     public NyARColorPatt_O3(int i_width,int i_height)\r
  54     {\r
  55         this.width=i_width;\r
  56         this.height=i_height;\r
  57         this.extpat=new int[i_height][i_width][3];\r
  58     }\r
  59 //    public void setSize(int i_new_width,int i_new_height)\r
  60 //    {\r
  61 //      int array_w=this.extpat[0].length;\r
  62 //      int array_h=this.extpat.length;\r
  63 //      //十分なサイズのバッファがあるか確認\r
  64 //      if(array_w>=i_new_width && array_h>=i_new_height){\r
  65 //          //OK 十分だ→サイズ調整のみ\r
  66 //      }else{\r
  67 //          //足りないよ→取り直し\r
  68 //          this.extpat=new int[i_new_height][i_new_width][3];\r
  69 //      }\r
  70 //        this.width =i_new_width;\r
  71 //        this.height=i_new_height;\r
  72 //        return;\r
  73 //    }    \r
  74     public int[][][] getPatArray()\r
  75     {\r
  76         return extpat;\r
  77     }\r
  78     public int getWidth()\r
  79     {\r
  80         return width;\r
  81     }\r
  82     public int getHeight()\r
  83     {\r
  84         return height;\r
  85     }\r
  86     private final NyARMat wk_get_cpara_a=new NyARMat(8,8);\r
  87     private final NyARMat wk_get_cpara_b=new NyARMat(8,1);\r
  88 \r
  89     /**\r
  90      * @param world\r
  91      * @param vertex\r
  92      * @param o_para\r
  93      * @throws NyARException\r
  94      */\r
  95     private boolean get_cpara(int[][] vertex,NyARMat o_para) throws NyARException\r
  96     {\r
  97         int world[][]=this.wk_pickFromRaster_world;\r
  98         NyARMat a =wk_get_cpara_a;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
  99         double[][] a_array=a.getArray();\r
 100         NyARMat b =wk_get_cpara_b;//次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );\r
 101         double[][] b_array=b.getArray();\r
 102         double[] a_pt0,a_pt1;\r
 103         int[] world_pti;\r
 104 \r
 105         for(int i = 0; i < 4; i++ ) {\r
 106             a_pt0=a_array[i*2];\r
 107             a_pt1=a_array[i*2+1];\r
 108             world_pti=world[i];\r
 109 \r
 110             a_pt0[0]=(double)world_pti[0];//a->m[i*16+0]  = world[i][0];\r
 111             a_pt0[1]=(double)world_pti[1];//a->m[i*16+1]  = world[i][1];\r
 112             a_pt0[2]=1.0;//a->m[i*16+2]  = 1.0;\r
 113             a_pt0[3]=0.0;//a->m[i*16+3]  = 0.0;\r
 114             a_pt0[4]=0.0;//a->m[i*16+4]  = 0.0;\r
 115             a_pt0[5]=0.0;//a->m[i*16+5]  = 0.0;\r
 116             a_pt0[6]=(double)(-world_pti[0] * vertex[i][0]);//a->m[i*16+6]  = -world[i][0] * vertex[i][0];\r
 117             a_pt0[7]=(double)(-world_pti[1] * vertex[i][0]);//a->m[i*16+7]  = -world[i][1] * vertex[i][0];\r
 118             a_pt1[0]=0.0;//a->m[i*16+8]  = 0.0;\r
 119             a_pt1[1]=0.0;//a->m[i*16+9]  = 0.0;\r
 120             a_pt1[2]=0.0;//a->m[i*16+10] = 0.0;\r
 121             a_pt1[3]=(double)world_pti[0];//a->m[i*16+11] = world[i][0];\r
 122             a_pt1[4]=(double)world_pti[1];//a->m[i*16+12] = world[i][1];\r
 123             a_pt1[5]=1.0;//a->m[i*16+13] = 1.0;\r
 124             a_pt1[6]=(double)(-world_pti[0] * vertex[i][1]);//a->m[i*16+14] = -world[i][0] * vertex[i][1];\r
 125             a_pt1[7]=(double)(-world_pti[1] * vertex[i][1]);//a->m[i*16+15] = -world[i][1] * vertex[i][1];\r
 126             b_array[i*2+0][0]=(double)vertex[i][0];//b->m[i*2+0] = vertex[i][0];\r
 127             b_array[i*2+1][0]=(double)vertex[i][1];//b->m[i*2+1] = vertex[i][1];\r
 128         }\r
 129         if(!a.matrixSelfInv()){\r
 130             return false;\r
 131         }           \r
 132 \r
 133         o_para.matrixMul(a, b);\r
 134         return true;\r
 135     }\r
 136 \r
 137   //   private final double[] wk_pickFromRaster_para=new double[9];//[3][3];\r
 138     private final int[][] wk_pickFromRaster_world={//double    world[4][2];\r
 139             {100,   100},\r
 140             {100+10,100},\r
 141             {100+10,100 + 10},\r
 142             {100,   100 + 10}\r
 143     };\r
 144     /**\r
 145      * pickFromRaster関数から使う変数です。\r
 146      *\r
 147      */\r
 148     private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2,int i_width,int i_height)\r
 149     {\r
 150         int i,i2;\r
 151         int[][] pt2;\r
 152         int[]   pt1;\r
 153         for(i=i_height-1;i>=0;i--){\r
 154             pt2=i_ext_pat2[i];\r
 155             for(i2=i_width-1;i2>=0;i2--){\r
 156                 pt1=pt2[i2];\r
 157                 pt1[0]=0;\r
 158                 pt1[1]=0;\r
 159                 pt1[2]=0;\r
 160             }\r
 161         }\r
 162     }\r
 163     private final NyARMat wk_pickFromRaster_cpara=new NyARMat(8,1);\r
 164     /**\r
 165      * imageから、i_markerの位置にあるパターンを切り出して、保持します。\r
 166      * Optimize:STEP[769->750]\r
 167      * @param image\r
 168      * @param i_marker\r
 169      * @throws Exception\r
 170      */\r
 171     public boolean pickFromRaster(INyARRaster image, NyARSquare i_square) throws NyARException\r
 172     {\r
 173         NyARMat cpara=this.wk_pickFromRaster_cpara;\r
 174         int[][] local=i_square.imvertex;\r
 175 //      //localの計算\r
 176 //      int[] local_0=wk_pickFromRaster_local[0];//double    local[4][2];       \r
 177 //      int[] local_1=wk_pickFromRaster_local[1];//double    local[4][2];       \r
 178 //      //\r
 179 //      for(int i = 0; i < 4; i++ ) {\r
 180 //          local_0[i] = i_square.imvertex[i][0];\r
 181 //          local_1[i] = i_square.imvertex[i][1];\r
 182 //      }\r
 183         //xdiv2,ydiv2の計算\r
 184         int xdiv2, ydiv2;\r
 185         int l1,l2;\r
 186         int w1,w2;\r
 187 \r
 188         //x計算\r
 189         w1=local[0][0] - local[1][0];\r
 190         w2=local[0][1] - local[1][1];\r
 191         l1 =(w1*w1+w2*w2);\r
 192         w1=local[2][0] - local[3][0];\r
 193         w2=local[2][1] - local[3][1];\r
 194         l2 =(w1*w1+w2*w2);\r
 195         if( l2 > l1 ){\r
 196             l1 = l2;\r
 197         }\r
 198         l1=l1/4;\r
 199         xdiv2 =this.width;\r
 200         while( xdiv2*xdiv2 < l1 ){\r
 201             xdiv2*=2;\r
 202         }\r
 203         if( xdiv2 > AR_PATT_SAMPLE_NUM)\r
 204         {\r
 205             xdiv2 =AR_PATT_SAMPLE_NUM;\r
 206         }\r
 207         \r
 208         //y計算\r
 209         w1=local[1][0] - local[2][0];\r
 210         w2=local[1][1] - local[2][1];\r
 211         l1 = (w1*w1+ w2*w2);\r
 212         w1=local[3][0] - local[0][0];\r
 213         w2=local[3][1] - local[0][1];\r
 214         l2 = (w1*w1+ w2*w2);\r
 215         if( l2 > l1 ){\r
 216             l1 = l2;\r
 217         }\r
 218         ydiv2 =this.height;\r
 219         l1=l1/4;\r
 220         while( ydiv2*ydiv2 < l1 ){\r
 221             ydiv2*=2;\r
 222         }\r
 223         if( ydiv2 >AR_PATT_SAMPLE_NUM)\r
 224         {\r
 225             ydiv2 = AR_PATT_SAMPLE_NUM;\r
 226         }       \r
 227         \r
 228         //cparaの計算\r
 229         if(!get_cpara(local,cpara)){\r
 230             return false;\r
 231         }\r
 232         updateExtpat(image,cpara,xdiv2,ydiv2);\r
 233 \r
 234         return true;\r
 235     }\r
 236     //かなり大きいワークバッファを取るな…。\r
 237     private double[] wk_updateExtpat_para00_xw;\r
 238     private double[] wk_updateExtpat_para10_xw;\r
 239     private double[] wk_updateExtpat_para20_xw;\r
 240     private int[] wk_updateExtpat_rgb_buf;\r
 241     private int[] wk_updateExtpat_x_rgb_index;\r
 242     private int[] wk_updateExtpat_y_rgb_index;\r
 243     private int[] wk_updateExtpat_i_rgb_index;\r
 244     private int wk_updateExtpat_buffer_size=0;\r
 245 \r
 246     /**\r
 247      * ワークバッファを予約する\r
 248      * @param i_xdiv2\r
 249      */\r
 250     private void reservWorkBuffers(int i_xdiv2)\r
 251     {\r
 252         if(this.wk_updateExtpat_buffer_size<i_xdiv2){\r
 253             wk_updateExtpat_para00_xw=new double[i_xdiv2];\r
 254             wk_updateExtpat_para10_xw=new double[i_xdiv2];\r
 255             wk_updateExtpat_para20_xw=new double[i_xdiv2];\r
 256             wk_updateExtpat_rgb_buf=new int[i_xdiv2*3];\r
 257             wk_updateExtpat_x_rgb_index=new int[i_xdiv2];\r
 258             wk_updateExtpat_y_rgb_index=new int[i_xdiv2];\r
 259             wk_updateExtpat_i_rgb_index=new int[i_xdiv2];\r
 260             this.wk_updateExtpat_buffer_size=i_xdiv2;\r
 261         }\r
 262         //十分なら何もしない。\r
 263         return;\r
 264     }\r
 265     \r
 266     private void updateExtpat(INyARRaster image,NyARMat i_cpara,int i_xdiv2,int i_ydiv2) throws NyARException\r
 267     {\r
 268         int img_x=image.getWidth();\r
 269         int img_y=image.getHeight();\r
 270         final int[][][] L_extpat=this.extpat;\r
 271         final int L_WIDTH=this.width;\r
 272         final int L_HEIGHT=this.height;\r
 273         /*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/\r
 274         //int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32  ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
 275         int extpat_j[][],extpat_j_i[];\r
 276         //int ext_pat2_j[][],ext_pat2_j_i[];\r
 277 \r
 278         initValue_wk_pickFromRaster_ext_pat2(L_extpat,L_WIDTH,L_HEIGHT);\r
 279 \r
 280         double[][] cpara_array=i_cpara.getArray();\r
 281         double para21_x_yw,para01_x_yw,para11_x_yw;\r
 282         double para00,para01,para02,para10,para11,para12,para20,para21;\r
 283         para00 = cpara_array[0*3+0][0];//para[i][0] = c->m[i*3+0];\r
 284         para01 = cpara_array[0*3+1][0];//para[i][1] = c->m[i*3+1];\r
 285         para02 = cpara_array[0*3+2][0];//para[i][2] = c->m[i*3+2];\r
 286         para10 = cpara_array[1*3+0][0];//para[i][0] = c->m[i*3+0];\r
 287         para11 = cpara_array[1*3+1][0];//para[i][1] = c->m[i*3+1];\r
 288         para12 = cpara_array[1*3+2][0];//para[i][2] = c->m[i*3+2];\r
 289         para20 = cpara_array[2*3+0][0];//para[2][0] = c->m[2*3+0];\r
 290         para21 = cpara_array[2*3+1][0];//para[2][1] = c->m[2*3+1];\r
 291 \r
 292         \r
 293         double          d,yw;\r
 294         int             xc, yc;\r
 295         int i,j;\r
 296         //      arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );\r
 297         int xdiv = i_xdiv2/L_WIDTH;//xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
 298         int ydiv = i_ydiv2/L_HEIGHT;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
 299         \r
 300         //計算バッファを予約する\r
 301         this.reservWorkBuffers(i_xdiv2);        \r
 302         double[] para00_xw=this.wk_updateExtpat_para00_xw;\r
 303         double[] para10_xw=this.wk_updateExtpat_para10_xw;\r
 304         double[] para20_xw=this.wk_updateExtpat_para20_xw;\r
 305         int[] x_rgb_index=this.wk_updateExtpat_x_rgb_index;\r
 306         int[] y_rgb_index=this.wk_updateExtpat_y_rgb_index;\r
 307         int[] i_rgb_index=this.wk_updateExtpat_i_rgb_index;\r
 308         int[] rgb_buf=this.wk_updateExtpat_rgb_buf;\r
 309         double xw;\r
 310         for(i=0;i<i_xdiv2;i++){\r
 311             xw= 102.5 + 5.0 * ((double)i+0.5) /i_xdiv2;\r
 312             para20_xw[i]=para20*xw;\r
 313             para00_xw[i]=para00*xw;\r
 314             para10_xw[i]=para10*xw;\r
 315         }\r
 316 \r
 317         int index_num;\r
 318 \r
 319         \r
 320         for(j = 0; j < i_ydiv2; j++ ) {\r
 321             yw = 102.5 + 5.0 * ((double)j+0.5) /i_ydiv2;\r
 322             para21_x_yw=para21*yw+1.0;\r
 323             para11_x_yw=para11*yw+para12;\r
 324             para01_x_yw=para01*yw+para02;\r
 325             extpat_j=L_extpat[j/ydiv];\r
 326             index_num=0;\r
 327             //ステップ１．RGB取得用のマップを作成\r
 328             for(i = 0; i < i_xdiv2; i++ ) {\r
 329                 d = para20_xw[i] + para21_x_yw;\r
 330                 if( d == 0 ){\r
 331                     throw new NyARException();\r
 332                 }\r
 333                 xc = (int)((para00_xw[i] + para01_x_yw)/d);\r
 334                 yc = (int)((para10_xw[i] + para11_x_yw)/d);\r
 335                 //範囲外は無視\r
 336                 if(xc<0 || xc >=img_x || yc<0 || yc >=img_y){\r
 337                     continue;\r
 338                 }\r
 339 //              ピクセル値の計算\r
 340 //              image.getPixel(xc,yc,rgb_buf);\r
 341 //                ext_pat2_j_i=ext_pat2_j[i/xdiv];\r
 342 //                ext_pat2_j_i[0] += rgb_buf[0];//R\r
 343 //                ext_pat2_j_i[1] += rgb_buf[1];//G\r
 344 //                ext_pat2_j_i[2] += rgb_buf[2];//B\r
 345 \r
 346                 x_rgb_index[index_num]=xc;\r
 347                 y_rgb_index[index_num]=yc;\r
 348                 i_rgb_index[index_num]=i/xdiv;\r
 349                 index_num++;\r
 350             }\r
 351 //          //ステップ２．ピクセル配列を取得\r
 352             image.getPixelSet(x_rgb_index,y_rgb_index,index_num,rgb_buf);\r
 353 //          //ピクセル値の計算\r
 354             for(i=index_num-1;i>=0;i--){\r
 355                 extpat_j_i=extpat_j[i_rgb_index[i]];\r
 356                 extpat_j_i[0] += rgb_buf[i*3+0];//R\r
 357                 extpat_j_i[1] += rgb_buf[i*3+1];//G\r
 358                 extpat_j_i[2] += rgb_buf[i*3+2];//B\r
 359             }\r
 360         }\r
 361         /*<Optimize>*/\r
 362         int xdiv_x_ydiv=xdiv*ydiv;\r
 363         for(j =L_HEIGHT-1; j>=0; j--){\r
 364             extpat_j=L_extpat[j];\r
 365             for(i = L_WIDTH-1; i>=0; i--){                              // PRL 2006-06-08.\r
 366                 extpat_j_i=extpat_j[i];\r
 367                 extpat_j_i[0]/=(xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv));\r
 368                 extpat_j_i[1]/=(xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv));\r
 369                 extpat_j_i[2]/=(xdiv_x_ydiv);//ext_pat[j][i][2] = (byte)(ext_pat2[j][i][2] / (xdiv*ydiv));\r
 370             }\r
 371         }\r
 372         return; \r
 373     }\r
 374 }