+/* \r
+ * PROJECT: NyARToolkit(Extension)\r
+ * --------------------------------------------------------------------------------\r
+ * The NyARToolkit is Java edition ARToolKit class library.\r
+ * Copyright (C)2008-2009 Ryo Iizuka\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (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 program. If not, see <http://www.gnu.org/licenses/>.\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp> or <nyatla(at)nyatla.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.nyidmarker;\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
+\r
+\r
+\r
+\r
+\r
+\r
+\r
+/**\r
+ * NyARColorPatt_NyIdMarkerがラスタからPerspective変換して読みだすためのクラス\r
+ *\r
+ */\r
+class PerspectivePixelReader\r
+{\r
+ private NyARPerspectiveParamGenerator _param_gen=new NyARPerspectiveParamGenerator_O1(1,1,100,100);\r
+ private double[] _cparam=new double[8];\r
+\r
+\r
+ public PerspectivePixelReader()\r
+ {\r
+ return;\r
+ }\r
+\r
+ public boolean setSourceSquare(NyARIntPoint2d[] i_vertex)throws NyARException\r
+ {\r
+ return this._param_gen.getParam(i_vertex, this._cparam);\r
+ }\r
+\r
+ /**\r
+ * 矩形からピクセルを切り出します\r
+ * @param i_lt_x\r
+ * @param i_lt_y\r
+ * @param i_step_x\r
+ * @param i_step_y\r
+ * @param i_width\r
+ * @param i_height\r
+ * @param i_out_st\r
+ * o_pixelへの格納場所の先頭インデクス\r
+ * @param o_pixel\r
+ * @throws NyARException\r
+ */\r
+ private boolean rectPixels(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,int i_lt_x,int i_lt_y,int i_step_x,int i_step_y,int i_width,int i_height,int i_out_st,int[] o_pixel)throws NyARException\r
+ {\r
+ final double[] cpara=this._cparam;\r
+ final int[] ref_x=this._ref_x;\r
+ final int[] ref_y=this._ref_y;\r
+ final int[] pixcel_temp=this._pixcel_temp;\r
+ final int raster_width=i_raster_size.w;\r
+ final int raster_height=i_raster_size.h;\r
+\r
+ int out_index=i_out_st;\r
+ final double cpara_6=cpara[6];\r
+ final double cpara_0=cpara[0];\r
+ final double cpara_3=cpara[3];\r
+\r
+ for(int i=0;i<i_height;i++){\r
+ //1列分のピクセルのインデックス値を計算する。\r
+ int cy0=1+i*i_step_y+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
+ final int cx0=1+i2*i_step_x+i_lt_x; \r
+ final double d=cpara_6*cx0+cpy0_7;\r
+ final int x=(int)((cpara_0*cx0+cpy0_12)/d);\r
+ final int y=(int)((cpara_3*cx0+cpy0_45)/d);\r
+ if(x<0||y<0||x>=raster_width||y>=raster_height)\r
+ {\r
+ return false;\r
+ }\r
+ ref_x[pt]=x;\r
+ ref_y[pt]=y;\r
+ pt++;\r
+ }\r
+ //1行分のピクセルを取得(場合によっては専用アクセサを書いた方がいい)\r
+ i_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_pixel[out_index]=(pixcel_temp[index+0]+pixcel_temp[index+1]+pixcel_temp[index+2])/3;\r
+ out_index++;\r
+ } \r
+ }\r
+ return true;\r
+ }\r
+ /**\r
+ * i_freqにあるゼロクロス点の周期が、等間隔か調べます。\r
+ * 次段半周期が、前段の80%より大きく、120%未満であるものを、等間隔周期であるとみなします。\r
+ * @param i_freq\r
+ * @param i_width\r
+ */\r
+ private static boolean checkFreqWidth(int[] i_freq,int i_width)\r
+ {\r
+ int c=i_freq[1]-i_freq[0];\r
+ final int count=i_width*2-1;\r
+ for(int i=1;i<count;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
+ /**\r
+ * i_freq_count_tableとi_freq_tableの内容を調査し、最も大きな周波数成分を返します。\r
+ * @param i_freq_count_table\r
+ * @param i_freq_table\r
+ * @param o_freq_table\r
+ * @return\r
+ * 見つかれば0以上、密辛ければ0未満\r
+ */\r
+ private static int getMaxFreq(int[] i_freq_count_table,int[] i_freq_table,int[] o_freq_table)\r
+ {\r
+ //一番成分の大きいものを得る\r
+ int index=-1;\r
+ int max=0;\r
+ for(int i=0;i<MAX_FREQ;i++){\r
+ if(max<i_freq_count_table[i]){\r
+ index=i;\r
+ max=i_freq_count_table[i];\r
+ }\r
+ } \r
+ if(index==-1){\r
+ return -1;\r
+ }\r
+ /*周波数インデクスを計算*/\r
+ final int st=(index-1)*index;\r
+ for(int i=0;i<index*2;i++)\r
+ {\r
+ o_freq_table[i]=i_freq_table[st+i]*FRQ_STEP/max;\r
+ }\r
+ return index;\r
+ }\r
+ \r
+ \r
+ //タイミングパターン用のパラメタ(FRQ_POINTS*FRQ_STEPが100を超えないようにすること)\r
+ private static final int FRQ_EDGE=5;\r
+ private static final int FRQ_STEP=2;\r
+ private static final int FRQ_POINTS=(100-(FRQ_EDGE*2))/FRQ_STEP;\r
+ \r
+\r
+ private static final int MIN_FREQ=3;\r
+ private static final int MAX_FREQ=10;\r
+ private static final int FREQ_SAMPLE_NUM=4;\r
+ private static final int MAX_DATA_BITS=MAX_FREQ+MAX_FREQ-1;\r
+\r
+ private final int[] _ref_x=new int[108];\r
+ private final int[] _ref_y=new int[108];\r
+ //(model+1)*4*3とTHRESHOLD_PIXEL*3のどちらか大きい方\r
+ private int[] _pixcel_temp=new int[108*3];\r
+ \r
+ private final int[] _freq_count_table=new int[MAX_FREQ];\r
+ private final int[] _freq_table=new int[(MAX_FREQ*2-1)*MAX_FREQ*2/2];\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(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,int i_y1,int i_th_h,int i_th_l,int[] o_edge_index)throws NyARException\r
+ {\r
+ //3,4,5,6,7,8,9,10\r
+ final int[] freq_count_table=this._freq_count_table;\r
+ //0,2,4,6,8,10,12,14,16,18,20の要素を持つ配列\r
+ final int freq_table[]=this._freq_table;\r
+ //初期化\r
+ final double[] cpara=this._cparam;\r
+// final INyARRgbPixelReader reader=this._raster.getRgbPixelReader();\r
+ final int[] ref_x=this._ref_x;\r
+ final int[] ref_y=this._ref_y;\r
+ final int[] pixcel_temp=this._pixcel_temp;\r
+ for(int i=0;i<10;i++){\r
+ freq_count_table[i]=0;\r
+ }\r
+ for(int i=0;i<110;i++){\r
+ freq_table[i]=0;\r
+ }\r
+ final int raster_width=i_raster_size.w;\r
+ final int raster_height=i_raster_size.h;\r
+\r
+ final double cpara_0=cpara[0];\r
+ final double cpara_3=cpara[3];\r
+ final double cpara_6=cpara[6]; \r
+ \r
+ //10-20ピクセル目からタイミングパターンを検出\r
+ for(int i=0;i<FREQ_SAMPLE_NUM;i++){\r
+ //2行分のピクセルインデックスを計算\r
+ final double cy0=1+i_y1+i;\r
+ final double cpy0_12=cpara[1]*cy0+cpara[2];\r
+ final double cpy0_45=cpara[4]*cy0+cpara[5];\r
+ final double cpy0_7=cpara[7]*cy0+1.0;\r
+\r
+ int pt=0;\r
+ for(int i2=0;i2<FRQ_POINTS;i2++)\r
+ {\r
+ final double cx0=1+i2*FRQ_STEP+FRQ_EDGE; \r
+ final double d=(cpara_6*cx0)+cpy0_7;\r
+ final int x=(int)((cpara_0*cx0+cpy0_12)/d);\r
+ final int y=(int)((cpara_3*cx0+cpy0_45)/d);\r
+ if(x<0||y<0||x>=raster_width||y>=raster_height)\r
+ {\r
+ return -1;\r
+ }\r
+ ref_x[pt]=x;\r
+ ref_y[pt]=y;\r
+ pt++;\r
+ }\r
+ \r
+ //ピクセルを取得(入力画像を多様化するならここから先を調整すること)\r
+ i_reader.getPixelSet(ref_x,ref_y,FRQ_POINTS,pixcel_temp);\r
+\r
+ //o_edge_indexを一時的に破壊して調査する\r
+ final int freq_t=getFreqInfo(pixcel_temp,i_th_h,i_th_l,o_edge_index); \r
+ \r
+ //周期は3-10であること\r
+ if(freq_t<MIN_FREQ || freq_t>MAX_FREQ){\r
+ continue;\r
+ }\r
+ //周期は等間隔であること\r
+ if(!checkFreqWidth(o_edge_index,freq_t)){\r
+ continue;\r
+ }\r
+ //検出カウンタを追加\r
+ freq_count_table[freq_t]++;\r
+ final int table_st=(freq_t-1)*freq_t;\r
+ for(int i2=0;i2<freq_t*2;i2++){\r
+ freq_table[table_st+i2]+=o_edge_index[i2];\r
+ }\r
+ }\r
+ return getMaxFreq(freq_count_table,freq_table,o_edge_index);\r
+ }\r
+ \r
+ public int getColFrequency(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,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._ref_x;\r
+ final int[] ref_y=this._ref_y;\r
+ final int[] pixcel_temp=this._pixcel_temp;\r
+ //0,2,4,6,8,10,12,14,16,18,20=(11*20)/2=110\r
+ //初期化\r
+ final int[] freq_count_table=this._freq_count_table;\r
+ for(int i=0;i<10;i++){\r
+ freq_count_table[i]=0;\r
+ }\r
+ final int[] freq_table=this._freq_table;\r
+ for(int i=0;i<110;i++){\r
+ freq_table[i]=0;\r
+ }\r
+ final int raster_width=i_raster_size.w;\r
+ final int raster_height=i_raster_size.h;\r
+ \r
+ \r
+ final double cpara7=cpara[7];\r
+ final double cpara4=cpara[4];\r
+ final double cpara1=cpara[1];\r
+ //基準点から4ピクセルを参照パターンとして抽出\r
+ for(int i=0;i<FREQ_SAMPLE_NUM;i++){\r
+\r
+ int cx0=1+i+i_x1;\r
+ final double cp6_0=cpara[6]*cx0;\r
+ final double cpx0_0=cpara[0]*cx0+cpara[2];\r
+ final double cpx3_0=cpara[3]*cx0+cpara[5];\r
+ \r
+ int pt=0;\r
+ for(int i2=0;i2<FRQ_POINTS;i2++)\r
+ {\r
+ int cy=1+i2*FRQ_STEP+FRQ_EDGE;\r
+ \r
+ final double d=cp6_0+cpara7*cy+1.0;\r
+ final int x=(int)((cpx0_0+cpara1*cy)/d);\r
+ final int y=(int)((cpx3_0+cpara4*cy)/d);\r
+ if(x<0||y<0||x>=raster_width||y>=raster_height)\r
+ {\r
+ return -1;\r
+ }\r
+ ref_x[pt]=x;\r
+ ref_y[pt]=y; \r
+ pt++;\r
+ } \r
+ \r
+ //ピクセルを取得(入力画像を多様化するならここを調整すること)\r
+ i_reader.getPixelSet(ref_x,ref_y,FRQ_POINTS,pixcel_temp);\r
+ \r
+ final int freq_t=getFreqInfo(pixcel_temp,i_th_h,i_th_l,o_edge_index);\r
+ //周期は3-10であること\r
+ if(freq_t<MIN_FREQ || freq_t>MAX_FREQ){\r
+ continue;\r
+ }\r
+ //周期は等間隔であること\r
+ if(!checkFreqWidth(o_edge_index,freq_t)){\r
+ continue;\r
+ }\r
+ //検出カウンタを追加\r
+ freq_count_table[freq_t]++;\r
+ final int table_st=(freq_t-1)*freq_t;\r
+ for(int i2=0;i2<freq_t*2;i2++){\r
+ freq_table[table_st+i2]+=o_edge_index[i2];\r
+ }\r
+ }\r
+ return getMaxFreq(freq_count_table,freq_table,o_edge_index); \r
+ }\r
+\r
+ /**\r
+ * デバックすんだらstaticにしておk\r
+ * @param i_pixcels\r
+ * @param i_th_h\r
+ * @param i_th_l\r
+ * @param o_edge_index\r
+ * @return\r
+ */\r
+ private static 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
+ private final static int THRESHOLD_EDGE=10;\r
+ private final static int THRESHOLD_STEP=2;\r
+ private final static int THRESHOLD_WIDTH=10;\r
+ private final static int THRESHOLD_PIXEL=THRESHOLD_WIDTH/THRESHOLD_STEP;\r
+ private final static int THRESHOLD_SAMPLE=THRESHOLD_PIXEL*THRESHOLD_PIXEL;\r
+ private final static int THRESHOLD_SAMPLE_LT=THRESHOLD_EDGE;\r
+ private final static int THRESHOLD_SAMPLE_RB=100-THRESHOLD_WIDTH-THRESHOLD_EDGE;\r
+ \r
+ public static class TThreshold{\r
+ public int th_h;\r
+ public int th_l;\r
+ public int th;\r
+ public int lt_x;\r
+ public int lt_y;\r
+ public int rb_x;\r
+ public int rb_y;\r
+ } \r
+\r
+ class THighAndLow{\r
+ public int h;\r
+ public int l;\r
+ }\r
+ /**\r
+ * ピクセル配列の上位、下位の4ピクセルのピクセル値平均を求めます。\r
+ * この関数は、(4/i_pixcel.length)の領域を占有するPtail法で双方向の閾値を求めることになります。\r
+ * @param i_pixcel\r
+ * @param i_initial\r
+ * @param i_out\r
+ */\r
+ private void getPtailHighAndLow(int[] i_pixcel,THighAndLow i_out)\r
+ {\r
+ int h3,h2,h1,h0,l3,l2,l1,l0;\r
+ h3=h2=h1=h0=l3=l2=l1=l0=i_pixcel[0];\r
+ \r
+ for(int i=i_pixcel.length-1;i>=1;i--){\r
+ final int pix=i_pixcel[i];\r
+ if(h0<pix){\r
+ if(h1<pix){\r
+ if(h2<pix){\r
+ if(h3<pix){\r
+ h0=h1;\r
+ h1=h2;\r
+ h2=h3;\r
+ h3=pix;\r
+ }else{\r
+ h0=h1;\r
+ h1=h2;\r
+ h2=pix;\r
+ }\r
+ }else{\r
+ h0=h1;\r
+ h1=pix;\r
+ }\r
+ }else{\r
+ h0=pix;\r
+ }\r
+ }\r
+ if(l0>pix){\r
+ if(l1>pix){\r
+ if(l2>pix){\r
+ if(l3>pix){\r
+ l0=l1;\r
+ l1=l2;\r
+ l2=l3;\r
+ l3=pix;\r
+ }else{\r
+ l0=l1;\r
+ l1=l2;\r
+ l2=pix;\r
+ }\r
+ }else{\r
+ l0=l1;\r
+ l1=pix;\r
+ }\r
+ }else{\r
+ l0=pix;\r
+ }\r
+ }\r
+ }\r
+ i_out.l=(l0+l1+l2+l3)/4;\r
+ i_out.h=(h0+h1+h2+h3)/4;\r
+ return;\r
+ }\r
+ private THighAndLow __detectThresholdValue_hl=new THighAndLow();\r
+ private NyARIntPoint2d __detectThresholdValue_tpt=new NyARIntPoint2d();\r
+ private int[] _th_pixels=new int[THRESHOLD_SAMPLE*4];\r
+ /**\r
+ * 指定した場所のピクセル値を調査して、閾値を計算して返します。\r
+ * @param i_reader\r
+ * @param i_x\r
+ * @param i_y\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public void detectThresholdValue(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,TThreshold o_threshold)throws NyARException\r
+ {\r
+ final int[] th_pixels=this._th_pixels;\r
+\r
+ //左上のピックアップ領域からピクセルを得る(00-24)\r
+ rectPixels(i_reader,i_raster_size,THRESHOLD_SAMPLE_LT,THRESHOLD_SAMPLE_LT,THRESHOLD_STEP,THRESHOLD_STEP,THRESHOLD_PIXEL,THRESHOLD_PIXEL,0,th_pixels);\r
+ \r
+ //左下のピックアップ領域からピクセルを得る(25-49)\r
+ rectPixels(i_reader,i_raster_size,THRESHOLD_SAMPLE_LT,THRESHOLD_SAMPLE_RB,THRESHOLD_STEP,THRESHOLD_STEP,THRESHOLD_PIXEL,THRESHOLD_PIXEL,THRESHOLD_SAMPLE,th_pixels);\r
+ \r
+ //右上のピックアップ領域からピクセルを得る(50-74)\r
+ rectPixels(i_reader,i_raster_size,THRESHOLD_SAMPLE_RB,THRESHOLD_SAMPLE_LT,THRESHOLD_STEP,THRESHOLD_STEP,THRESHOLD_PIXEL,THRESHOLD_PIXEL,THRESHOLD_SAMPLE*2,th_pixels);\r
+\r
+ //右下のピックアップ領域からピクセルを得る(75-99)\r
+ rectPixels(i_reader,i_raster_size,THRESHOLD_SAMPLE_RB,THRESHOLD_SAMPLE_RB,THRESHOLD_STEP,THRESHOLD_STEP,THRESHOLD_PIXEL,THRESHOLD_PIXEL,THRESHOLD_SAMPLE*3,th_pixels);\r
+\r
+ final THighAndLow hl=this.__detectThresholdValue_hl;\r
+ //Ptailで求めたピクセル平均\r
+ getPtailHighAndLow(th_pixels,hl);\r
+\r
+\r
+ \r
+ //閾値中心\r
+ int th=(hl.h+hl.l)/2;\r
+ //ヒステリシス(差分の20%)\r
+ int th_sub=(hl.h-hl.l)/5;\r
+ \r
+ o_threshold.th=th;\r
+ o_threshold.th_h=th+th_sub;//ヒステリシス付き閾値\r
+ o_threshold.th_l=th-th_sub;//ヒステリシス付き閾値\r
+\r
+ //エッジを計算(明点重心)\r
+ int lt_x,lt_y,lb_x,lb_y,rt_x,rt_y,rb_x,rb_y;\r
+ final NyARIntPoint2d tpt=this.__detectThresholdValue_tpt;\r
+ //LT\r
+ if(getHighPixelCenter(0,th_pixels,THRESHOLD_PIXEL,THRESHOLD_PIXEL,th,tpt)){\r
+ lt_x=tpt.x*THRESHOLD_STEP;\r
+ lt_y=tpt.y*THRESHOLD_STEP;\r
+ }else{\r
+ lt_x=11;\r
+ lt_y=11;\r
+ }\r
+ //LB\r
+ if(getHighPixelCenter(THRESHOLD_SAMPLE*1,th_pixels,THRESHOLD_PIXEL,THRESHOLD_PIXEL,th,tpt)){\r
+ lb_x=tpt.x*THRESHOLD_STEP;\r
+ lb_y=tpt.y*THRESHOLD_STEP;\r
+ }else{\r
+ lb_x=11;\r
+ lb_y=-1;\r
+ }\r
+ //RT\r
+ if(getHighPixelCenter(THRESHOLD_SAMPLE*2,th_pixels,THRESHOLD_PIXEL,THRESHOLD_PIXEL,th,tpt)){\r
+ rt_x=tpt.x*THRESHOLD_STEP;\r
+ rt_y=tpt.y*THRESHOLD_STEP;\r
+ }else{\r
+ rt_x=-1;\r
+ rt_y=11;\r
+ }\r
+ //RB\r
+ if(getHighPixelCenter(THRESHOLD_SAMPLE*3,th_pixels,THRESHOLD_PIXEL,THRESHOLD_PIXEL,th,tpt)){\r
+ rb_x=tpt.x*THRESHOLD_STEP;\r
+ rb_y=tpt.y*THRESHOLD_STEP;\r
+ }else{\r
+ rb_x=-1;\r
+ rb_y=-1;\r
+ }\r
+ //トラッキング開始位置の決定\r
+ o_threshold.lt_x=(lt_x+lb_x)/2+THRESHOLD_SAMPLE_LT-1;\r
+ o_threshold.rb_x=(rt_x+rb_x)/2+THRESHOLD_SAMPLE_RB+1;\r
+ o_threshold.lt_y=(lt_y+rt_y)/2+THRESHOLD_SAMPLE_LT-1;\r
+ o_threshold.rb_y=(lb_y+rb_y)/2+THRESHOLD_SAMPLE_RB+1;\r
+ return;\r
+ }\r
+\r
+ private boolean getHighPixelCenter(int i_st,final int[] i_pixels,int i_width,int i_height,int i_th,NyARIntPoint2d o_point)\r
+ {\r
+ int rp=i_st;\r
+ int pos_x=0;\r
+ int pos_y=0;\r
+ int number_of_pos=0;\r
+ for(int i=0;i<i_height;i++){\r
+ for(int i2=0;i2<i_width;i2++){\r
+ if(i_pixels[rp++]>i_th){\r
+ pos_x+=i2;\r
+ pos_y+=i;\r
+ number_of_pos++;\r
+ }\r
+ }\r
+ }\r
+ if(number_of_pos>0){\r
+ pos_x/=number_of_pos;\r
+ pos_y/=number_of_pos;\r
+ }else{\r
+ return false;\r
+ }\r
+ o_point.x=pos_x;\r
+ o_point.y=pos_y;\r
+ return true;\r
+ }\r
+ private int[] __detectDataBitsIndex_freq_index1=new int[FRQ_POINTS];\r
+ private int[] __detectDataBitsIndex_freq_index2=new int[FRQ_POINTS];\r
+ private int detectDataBitsIndex(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,PerspectivePixelReader.TThreshold i_th,double[] o_index_row,double[] o_index_col) throws NyARException\r
+ {\r
+ //周波数を測定\r
+ final int[] freq_index1=this.__detectDataBitsIndex_freq_index1;\r
+ final int[] freq_index2=this.__detectDataBitsIndex_freq_index2;\r
+ \r
+ int frq_t=getRowFrequency(i_reader,i_raster_size,i_th.lt_y,i_th.th_h,i_th.th_l,freq_index1);\r
+ int frq_b=getRowFrequency(i_reader,i_raster_size,i_th.rb_y,i_th.th_h,i_th.th_l,freq_index2);\r
+ //周波数はまとも?\r
+ if((frq_t<0 && frq_b<0) || frq_t==frq_b){\r
+ return -1;\r
+ }\r
+ //タイミングパターンからインデクスを作成\r
+ int freq_h,freq_v;\r
+ int[] index;\r
+ if(frq_t>frq_b){\r
+ freq_h=frq_t;\r
+ index=freq_index1;\r
+ }else{\r
+ freq_h=frq_b;\r
+ index=freq_index2;\r
+ }\r
+ for(int i=0;i<freq_h+freq_h-1;i++){\r
+ o_index_row[i*2]=((index[i+1]-index[i])*2/5+index[i])+FRQ_EDGE;\r
+ o_index_row[i*2+1]=((index[i+1]-index[i])*3/5+index[i])+FRQ_EDGE;\r
+ } \r
+ \r
+ \r
+ final int frq_l=getColFrequency(i_reader,i_raster_size,i_th.lt_x,i_th.th_h,i_th.th_l,freq_index1);\r
+ final int frq_r=getColFrequency(i_reader,i_raster_size,i_th.rb_x,i_th.th_h,i_th.th_l,freq_index2);\r
+ //周波数はまとも?\r
+ if((frq_l<0 && frq_r<0) || frq_l==frq_r){\r
+ return -1;\r
+ }\r
+ //タイミングパターンからインデクスを作成\r
+ if(frq_l>frq_r){\r
+ freq_v=frq_l;\r
+ index=freq_index1;\r
+ }else{\r
+ freq_v=frq_r;\r
+ index=freq_index2;\r
+ }\r
+ //同じ周期?\r
+ if(freq_v!=freq_h){\r
+ return -1;\r
+ }\r
+ \r
+ for(int i=0;i<freq_v+freq_v-1;i++){\r
+ final int w=index[i];\r
+ final int w2=index[i+1]-w;\r
+ o_index_col[i*2]=((w2)*2/5+w)+FRQ_EDGE;\r
+ o_index_col[i*2+1]=((w2)*3/5+w)+FRQ_EDGE;\r
+ } \r
+ //Lv4以上は無理\r
+ if(freq_v>MAX_FREQ){\r
+ return -1;\r
+ }\r
+ return freq_v;\r
+ \r
+ }\r
+ private double[] __readDataBits_index_bit_x=new double[MAX_DATA_BITS*2];\r
+ private double[] __readDataBits_index_bit_y=new double[MAX_DATA_BITS*2];\r
+ \r
+ public boolean readDataBits(INyARRgbPixelReader i_reader,NyARIntSize i_raster_size,PerspectivePixelReader.TThreshold i_th,MarkerPattEncoder o_bitbuffer)throws NyARException\r
+ {\r
+ final double[] index_x=this.__readDataBits_index_bit_x;\r
+ final double[] index_y=this.__readDataBits_index_bit_y;\r
+ \r
+\r
+ //読み出し位置を取得\r
+ final int size=detectDataBitsIndex(i_reader,i_raster_size,i_th,index_x,index_y);\r
+ final int resolution=size+size-1;\r
+ if(size<0){\r
+ return false;\r
+ }\r
+ if(!o_bitbuffer.initEncoder(size-1)){\r
+ return false;\r
+ } \r
+ \r
+ final double[] cpara=this._cparam;\r
+ final int[] ref_x=this._ref_x;\r
+ final int[] ref_y=this._ref_y;\r
+ final int[] pixcel_temp=this._pixcel_temp;\r
+ \r
+ final double cpara_0=cpara[0];\r
+ final double cpara_1=cpara[1];\r
+ final double cpara_3=cpara[3];\r
+ final double cpara_6=cpara[6];\r
+ \r
+ \r
+ final int th=i_th.th;\r
+ int p=0;\r
+ for(int i=0;i<resolution;i++){\r
+ //1列分のピクセルのインデックス値を計算する。\r
+ double cy0=1+index_y[i*2+0];\r
+ double cy1=1+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<resolution;i2++)\r
+ { \r
+\r
+ double d;\r
+ double cx0=1+index_x[i2*2+0];\r
+ double cx1=1+index_x[i2*2+1];\r
+\r
+ double cp6_0=cpara_6*cx0;\r
+ double cpx0_0=cpara_0*cx0;\r
+ double cpx3_0=cpara_3*cx0;\r
+\r
+ double cp6_1=cpara_6*cx1;\r
+ double cpx0_1=cpara_0*cx1;\r
+ double cpx3_1=cpara_3*cx1;\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)((cpx0_1+cpy0_12)/d);\r
+ ref_y[pt]=(int)((cpx3_1+cpy0_45)/d);\r
+ pt++;\r
+\r
+ d=cp6_1+cpy1_7;\r
+ ref_x[pt]=(int)((cpx0_1+cpy1_12)/d);\r
+ ref_y[pt]=(int)((cpx3_1+cpy1_45)/d);\r
+ pt++;\r
+ }\r
+ //1行分のピクセルを取得(場合によっては専用アクセサを書いた方がいい)\r
+ i_reader.getPixelSet(ref_x,ref_y,resolution*4,pixcel_temp);\r
+ //グレースケールにしながら、line→mapへの転写\r
+ for(int i2=0;i2<resolution;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
+ //暗点を1、明点を0で表現します。\r
+ o_bitbuffer.setBitByBitIndex(p,pixel>th?0:1);\r
+ p++;\r
+ }\r
+ }\r
+/* \r
+ for(int i=0;i<225*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)index_x[i2];\r
+ this.vertex_y[i*(resolution)*2+i2]=(int)index_y[i];\r
+ \r
+ }\r
+ }\r
+*/ return true;\r
+ }\r
+ public boolean setSquare(NyARIntPoint2d[] i_vertex) throws NyARException\r
+ {\r
+ if (!this._param_gen.getParam(i_vertex,this._cparam)) {\r
+ return false;\r
+ }\r
+ return true;\r
+ }\r
+\r
+}\r
+class MarkerPattDecoder\r
+{\r
+ public void decode(int model,int domain,int mask)\r
+ {\r
+ \r
+ }\r
+}\r
+/**\r
+ * マーカパターンのエンコーダです。\r
+ *\r
+ */\r
+class MarkerPattEncoder\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,null,null,null,null,null,\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
+ private int[] _bit_table;\r
+ private int[] _bits=new int[16];\r
+ private int[] _work=new int[16];\r
+ private 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 int getModel()\r
+ {\r
+ return this._model;\r
+ }\r
+ private static int getControlValue(int i_model,int[] i_data)\r
+ {\r
+ int v;\r
+ switch(i_model){\r
+ case 2:\r
+ v=(i_data[2] & 0x0e)>>1;\r
+ return v>=5?v-1:v;\r
+ case 3:\r
+ v=(i_data[4] & 0x3e)>>1;\r
+ return v>=21?v-1:v;\r
+ case 4:\r
+ case 5:\r
+ case 6:\r
+ case 7:\r
+ default:\r
+ break;\r
+ }\r
+ return -1;\r
+ }\r
+ public static int getCheckValue(int i_model,int[] i_data)\r
+ {\r
+ int v;\r
+ switch(i_model){\r
+ case 2:\r
+ v=(i_data[2] & 0xe0)>>5;\r
+ return v>5?v-1:v;\r
+ case 3:\r
+ v=((i_data[4] & 0x80)>>7) |((i_data[5] & 0x0f)<<1);\r
+ return v>21?v-1:v;\r
+ case 4:\r
+ case 5:\r
+ case 6:\r
+ case 7:\r
+ default:\r
+ break;\r
+ }\r
+ return -1;\r
+ }\r
+ public boolean initEncoder(int i_model)\r
+ {\r
+ if(i_model>3 || 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
+ int timing_pat;\r
+ switch(this._model){\r
+ case 2:\r
+ //トラッキングセルを得る\r
+ t=this._bits[2] & 0x1f;\r
+ r=((this._bits[2] & 0xf0)>>4)|((this._bits[3]&0x01)<<4);\r
+ b=this._bits[3] & 0x1f;\r
+ l=((this._bits[3] & 0xf0)>>4)|((this._bits[2]&0x01)<<4);\r
+ timing_pat=0x0a;\r
+ break;\r
+ case 3:\r
+ t=this._bits[4] & 0x7f;\r
+ r=((this._bits[4] & 0xc0)>>6)|((this._bits[5] & 0x1f)<<2);\r
+ b=((this._bits[5] & 0xf0)>>4)|((this._bits[6] & 0x07)<<4);\r
+ l=((this._bits[6] & 0xfc)>>2)|((this._bits[4] & 0x01)<<6);\r
+ timing_pat=0x2a;\r
+ break;\r
+ default:\r
+ return -3;\r
+ }\r
+ //タイミングパターンの比較\r
+ if(t==timing_pat){\r
+ if(r==timing_pat){\r
+ return (b!=timing_pat && l!=timing_pat)?2:-2;\r
+ }else if(l==timing_pat){\r
+ return (b!=timing_pat && r!=timing_pat)?3:-2;\r
+ }\r
+ }else if(b==timing_pat){\r
+ if(r==timing_pat){\r
+ return (t!=timing_pat && l!=timing_pat)?1:-2;\r
+ }else if(l==timing_pat){\r
+ return (t!=timing_pat && r!=timing_pat)?0:-2;\r
+ }\r
+ }\r
+ return -1;\r
+ }\r
+ /**\r
+ * 格納しているマーカパターンをエンコードして、マーカデータを返します。\r
+ * @param o_out\r
+ * @return\r
+ * 成功すればマーカの方位を返却します。失敗すると-1を返します。\r
+ */\r
+\r
+ public int encode(NyIdMarkerPattern o_out)\r
+ {\r
+ final int d=getDirection();\r
+ if(d<0){\r
+ return -1;\r
+ }\r
+ //回転ビットの取得\r
+ getRotatedBits(d,o_out.data);\r
+ final int model=this._model;\r
+ //周辺ビットの取得\r
+ o_out.model=model;\r
+ int control_bits=getControlValue(model,o_out.data);\r
+ o_out.check=getCheckValue(model,o_out.data);\r
+ o_out.ctrl_mask=control_bits%5;\r
+ o_out.ctrl_domain=control_bits/5;\r
+ if(o_out.ctrl_domain!=0 || o_out.ctrl_mask!=0){\r
+ return -1;\r
+ }\r
+ //マスク解除処理を実装すること\r
+ return d;\r
+ }\r
+ private void getRotatedBits(int i_direction,int[] o_out)\r
+ {\r
+ int sl=i_direction*2;\r
+ int sr=8-sl;\r
+\r
+ int w1;\r
+ o_out[0]=this._bits[0];\r
+ //RECT1\r
+ w1=this._bits[1];\r
+ o_out[1]=((w1<<sl)|(w1>>sr))& 0xff;\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
+ o_out[2]=w1 & 0xff;\r
+ o_out[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
+ o_out[4]=w1 & 0xff;\r
+ o_out[5]=(w1>>8) & 0xff;\r
+ o_out[6]=(w1>>16) & 0xff;\r
+ \r
+ if(this._model<3){\r
+ return;\r
+ }\r
+ //RECT4(Lv4以降はここの制限を変える)\r
+// shiftLeft(this._bits,7,3,i_direction*8);\r
+// if(this._model<4){\r
+// return;\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
+ * ラスタ画像の任意矩形から、NyARIdMarkerDataを抽出します。\r
+ *\r
+ */\r
+public class NyIdMarkerPickup\r
+{\r
+ private PerspectivePixelReader _perspective_reader;\r
+ private final PerspectivePixelReader.TThreshold __pickFromRaster_th=new PerspectivePixelReader.TThreshold();\r
+ private final MarkerPattEncoder __pickFromRaster_encoder=new MarkerPattEncoder();\r
+\r
+\r
+ public NyIdMarkerPickup()\r
+ {\r
+ this._perspective_reader=new PerspectivePixelReader();\r
+ return;\r
+ }\r
+ /**\r
+ * i_imageから、idマーカを読みだします。\r
+ * o_dataにはマーカデータ、o_paramにはまーかのパラメータを返却します。\r
+ * @param image\r
+ * @param i_square\r
+ * @param o_data\r
+ * @param o_param\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public boolean pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertex,NyIdMarkerPattern o_data,NyIdMarkerParam o_param)throws NyARException\r
+ {\r
+ \r
+ //遠近法のパラメータを計算\r
+ if(!this._perspective_reader.setSourceSquare(i_vertex)){\r
+ return false;\r
+ };\r
+ \r
+ INyARRgbPixelReader reader=image.getRgbPixelReader();\r
+ NyARIntSize raster_size=image.getSize();\r
+ \r
+\r
+\r
+ final PerspectivePixelReader.TThreshold th=this.__pickFromRaster_th;\r
+ final MarkerPattEncoder encoder=this.__pickFromRaster_encoder;\r
+ //マーカパラメータを取得\r
+ this._perspective_reader.detectThresholdValue(reader,raster_size,th);\r
+\r
+ if(!this._perspective_reader.readDataBits(reader,raster_size,th, encoder)){\r
+ return false;\r
+ }\r
+ final int d=encoder.encode(o_data);\r
+ if(d<0){\r
+ return false;\r
+ }\r
+ o_param.direction=d;\r
+ o_param.threshold=th.th;\r
+ \r
+ return true;\r
+ }\r
+}\r