tags/2.5.0/test/jp/nyatla/nyartoolkit/dev/NyARSquareDetector_Vector.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.dev;\r
  32 \r
  33 import jp.nyatla.nyartoolkit.NyARException;\r
  34 import jp.nyatla.nyartoolkit.core.labeling.NyARLabelOverlapChecker;\r
  35 import jp.nyatla.nyartoolkit.core.labeling.rlelabeling.*;\r
  36 import jp.nyatla.nyartoolkit.core.param.NyARCameraDistortionFactor;\r
  37 import jp.nyatla.nyartoolkit.core.squaredetect.NyARCoord2SquareVertexIndexes;\r
  38 import jp.nyatla.nyartoolkit.core.squaredetect.NyARSquare;\r
  39 import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
  40 import jp.nyatla.nyartoolkit.core.types.NyARIntPoint2d;\r
  41 import jp.nyatla.nyartoolkit.core.types.NyARIntSize;\r
  42 import jp.nyatla.nyartoolkit.core.types.NyARLinear;\r
  43 import jp.nyatla.nyartoolkit.core.raster.*;\r
  44 import jp.nyatla.nyartoolkit.core.squaredetect.*;\r
  45 import jp.nyatla.nyartoolkit.core.param.*;\r
  46 \r
  47 \r
  48 \r
  49 /**\r
  50  * \r
  51  *\r
  52  */\r
  53 public class NyARSquareDetector_Vector\r
  54 {\r
  55         private static final int AR_AREA_MAX = 100000;// #define AR_AREA_MAX 100000\r
  56         private static final int AR_AREA_MIN = 70;// #define AR_AREA_MIN 70\r
  57         private final int _width;\r
  58         private final int _height;\r
  59         private final int[] _xcoord;\r
  60         private final int[] _ycoord;\r
  61 \r
  62         private final NyARLabeling_Rle _labeling;\r
  63 \r
  64         private final NyARLabelOverlapChecker<NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo> _overlap_checker = new NyARLabelOverlapChecker<NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo>(32,NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo.class);\r
  65         private final SquareContourDetector_Vector _sqconvertor;\r
  66         private final NyARContourPickup _cpickup=new NyARContourPickup();\r
  67         private final NyARRleLabelFragmentInfoStack _stack;\r
  68         \r
  69         private final int _max_coord;\r
  70         /**\r
  71          * 最大i_squre_max個のマーカーを検出するクラスを作成する。\r
  72          * \r
  73          * @param i_param\r
  74          */\r
  75         public NyARSquareDetector_Vector(NyARCameraDistortionFactor i_dist_factor_ref,NyARIntSize i_size) throws NyARException\r
  76         {\r
  77                 this._width = i_size.w;\r
  78                 this._height = i_size.h;\r
  79                 //ラベリングのサイズを指定したいときはsetAreaRangeを使ってね。\r
  80                 this._labeling = new NyARLabeling_Rle(this._width,this._height);\r
  81                 this._labeling.setAreaRange(AR_AREA_MAX, AR_AREA_MIN);\r
  82                 this._sqconvertor=new SquareContourDetector_Vector(i_size,i_dist_factor_ref);\r
  83                 this._stack=new NyARRleLabelFragmentInfoStack(i_size.w*i_size.h*2048/(320*240)+32);//検出可能な最大ラベル数\r
  84                 \r
  85 \r
  86                 // 輪郭の最大長は画面に映りうる最大の長方形サイズ。\r
  87                 int number_of_coord = (this._width + this._height) * 2;\r
  88 \r
  89                 // 輪郭バッファ\r
  90                 this._max_coord = number_of_coord;\r
  91                 this._xcoord = new int[number_of_coord];\r
  92                 this._ycoord = new int[number_of_coord];\r
  93                 return;\r
  94         }\r
  95 \r
  96         /**\r
  97          * arDetectMarker2を基にした関数\r
  98          * この関数はNyARSquare要素のうち、directionを除くパラメータを取得して返します。\r
  99          * directionの確定は行いません。\r
 100          * @param i_raster\r
 101          * 解析する２値ラスタイメージを指定します。\r
 102          * @param o_square_stack\r
 103          * 抽出した正方形候補を格納するリスト\r
 104          * @throws NyARException\r
 105          */\r
 106         public final void detectMarker(NyARGrayscaleRaster i_gs,int i_th,NyARSquareStack o_square_stack) throws NyARException\r
 107         {\r
 108                 final NyARRleLabelFragmentInfoStack flagment=this._stack;\r
 109                 final NyARLabelOverlapChecker<NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo> overlap = this._overlap_checker;\r
 110 \r
 111                 // マーカーホルダをリセット\r
 112                 o_square_stack.clear();\r
 113 \r
 114                 // ラベル数が0ならここまで\r
 115                 final int label_num=this._labeling.labeling(i_gs,i_th, 0, i_gs.getHeight(), flagment);\r
 116                 if (label_num < 1) {\r
 117                         return;\r
 118                 }\r
 119                 //ラベルをソートしておく\r
 120                 flagment.sortByArea();\r
 121                 //ラベルリストを取得\r
 122                 NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo[] labels=flagment.getArray();\r
 123 \r
 124                 final int xsize = this._width;\r
 125                 final int ysize = this._height;\r
 126                 final int coord_max = this._max_coord;\r
 127                 int[] xcoord = this._xcoord;\r
 128                 int[] ycoord = this._ycoord;\r
 129 \r
 130 \r
 131                 //重なりチェッカの最大数を設定\r
 132                 overlap.setMaxLabels(label_num);\r
 133 \r
 134                 for (int i=0; i < label_num; i++) {\r
 135                         final NyARRleLabelFragmentInfoStack.RleLabelFragmentInfo label_pt=labels[i];\r
 136                         final int label_area = label_pt.area;\r
 137                 \r
 138                         // クリップ領域が画面の枠に接していれば除外\r
 139                         if (label_pt.clip_l == 0 || label_pt.clip_r == xsize-1){\r
 140                                 continue;\r
 141                         }\r
 142                         if (label_pt.clip_t == 0 || label_pt.clip_b == ysize-1){\r
 143                                 continue;\r
 144                         }\r
 145                         // 既に検出された矩形との重なりを確認\r
 146                         if (!overlap.check(label_pt)) {\r
 147                                 // 重なっているようだ。\r
 148                                 continue;\r
 149                         }\r
 150                         \r
 151                         // 輪郭を取得\r
 152                         final int coord_num = _cpickup.getContour(i_gs,i_th,label_pt.entry_x,label_pt.clip_t, coord_max, xcoord, ycoord);\r
 153                         if (coord_num == coord_max) {\r
 154                                 // 輪郭が大きすぎる。\r
 155                                 continue;\r
 156                         }\r
 157 \r
 158                         //ここから先が輪郭分析\r
 159                         NyARSquare square_ptr = o_square_stack.prePush();\r
 160                         if(!this._sqconvertor.coordToSquare(i_gs,xcoord,ycoord,coord_num,label_area,square_ptr)){\r
 161                                 o_square_stack.pop();// 頂点の取得が出来なかったので破棄\r
 162                                 continue;                               \r
 163                         }\r
 164                         // 検出済の矩形の属したラベルを重なりチェックに追加する。\r
 165                         overlap.push(label_pt);\r
 166                 }\r
 167                 return;\r
 168         }\r
 169         /**\r
 170          * デバック用API\r
 171          * @return\r
 172          */\r
 173         public NyARRleLabelFragmentInfoStack _getFragmentStack()\r
 174         {\r
 175                 return this._stack;\r
 176         }\r
 177         /********************************************************************************\r
 178          * \r
 179          * 追加クラス \r
 180          * \r
 181          ********************************************************************************/\r
 182         private class SquareContourDetector_Vector\r
 183         {\r
 184                 private final NyARObserv2IdealMap2 _distmap;\r
 185                 private final int[] __detectMarker_mkvertex = new int[4];\r
 186                 private final NyARCoord2SquareVertexIndexes _coord2vertex=new NyARCoord2SquareVertexIndexes();\r
 187                 public SquareContourDetector_Vector(NyARIntSize i_size,NyARCameraDistortionFactor i_distfactor_ref)\r
 188                 {\r
 189                         this._distmap=new NyARObserv2IdealMap2(i_distfactor_ref,i_size);\r
 190                         return;\r
 191                 }\r
 192 \r
 193                 public boolean coordToSquare(NyARGrayscaleRaster i_raster,int[] i_xcoord,int[] i_ycoord,int i_coord_num,int i_label_area,NyARSquare o_square) throws NyARException\r
 194                 {\r
 195 \r
 196                         final int[] mkvertex = this.__detectMarker_mkvertex;\r
 197 \r
 198                         // 頂点情報を取得\r
 199                         if (!this._coord2vertex.getVertexIndexes(i_xcoord, i_ycoord,i_coord_num, i_label_area, mkvertex)) {\r
 200                                 // 頂点の取得が出来なかったので破棄\r
 201                                 return false;\r
 202                         }               \r
 203                         // マーカーを検出\r
 204                         if (!getSquareLine(i_raster,mkvertex, i_xcoord, i_ycoord,i_coord_num, o_square)){\r
 205                                 // 矩形が成立しなかった。\r
 206                                 return false;\r
 207                         }\r
 208                         return true;\r
 209                 }\r
 210                 /**\r
 211                  * 指定した範囲の輪郭点ベクトル・座標を、加算する。\r
 212                  * @param i_raster\r
 213                  * @param i_xcoord\r
 214                  * @param i_ycoord\r
 215                  * @param i_st\r
 216                  * @param i_ed\r
 217                  * @param o_vecsum\r
 218                  * @param o_possum\r
 219                  * @return\r
 220                  */\r
 221                 private boolean addCoordVecPos(NyARGrayscaleRaster i_raster,int[] i_xcoord, int[] i_ycoord,int i_st,int i_ed,NyARIntPoint2d io_vecsum)\r
 222                 {\r
 223                         int dxi,dyi;\r
 224                         //ベクトル分析\r
 225                         dxi=io_vecsum.x;\r
 226                         dyi=io_vecsum.y;\r
 227                         for(int i=i_st;i<i_ed;i++){\r
 228                                 //境界に設置していたら失敗する。\r
 229                                 if(i_xcoord[i]<1 || i_ycoord[i]<1 || i_xcoord[i]>=319 || i_ycoord[i]>=239){\r
 230                                         return false;\r
 231                                 }\r
 232                                 //o_vecsumをワークに流用\r
 233                                 i_raster.getPixelVector8(i_xcoord[i],i_ycoord[i],io_vecsum);\r
 234                                 dxi+=io_vecsum.x;\r
 235                                 dyi-=io_vecsum.y;\r
 236                         }\r
 237                         io_vecsum.x=dxi;\r
 238                         io_vecsum.y=dyi;\r
 239                         return true;            \r
 240                 }\r
 241                 \r
 242                 \r
 243                 private NyARDoublePoint2d __work_pos=new NyARDoublePoint2d();\r
 244                 \r
 245                 private boolean getSquareLine(NyARGrayscaleRaster i_raster,int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord,int i_cood_num, NyARSquare o_square) throws NyARException\r
 246                 {\r
 247                         final NyARLinear[] l_line = o_square.line;\r
 248                         final NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;\r
 249                         final NyARIntPoint2d[] l_imvertex = o_square.imvertex;\r
 250                         final NyARDoublePoint2d idealcenter=this.__work_pos;\r
 251                 \r
 252                         NyARIntPoint2d vecsum=new NyARIntPoint2d();\r
 253                         for (int i = 0; i < 4; i++){\r
 254                                 //頂点を取得\r
 255                                 int ver1=i_mkvertex[i];\r
 256                                 int ver2=i_mkvertex[(i+1)%4];\r
 257 \r
 258                                 int n,st,ed;\r
 259                                 double w1;\r
 260                                 //探索区間の決定\r
 261                                 if(ver2>=i_mkvertex[i]){\r
 262                                         //頂点[i]から頂点[i+1]までの輪郭が、1区間にあるとき\r
 263                                         w1 = (double) (ver2 - ver1 + 1) * 0.05 + 0.5;\r
 264                                         //探索区間の決定\r
 265                                         st = (int) (ver1+w1);\r
 266                                         ed = (int) (ver2 - w1);\r
 267                                 }else{\r
 268                                         //頂点[i]から頂点[i+1]までの輪郭が、2区間に分かれているとき\r
 269                                         w1 = (double) (ver2+i_cood_num-ver1+1)%i_cood_num * 0.05 + 0.5;\r
 270                                         //探索区間の決定\r
 271                                         st = (int) (ver1+w1)%i_cood_num;\r
 272                                         ed = (int) (ver2+i_cood_num-w1)%i_cood_num;\r
 273                                 }\r
 274                                 vecsum.x=vecsum.y=0;\r
 275                                 //ベクトル分析\r
 276                                 if(st<=ed){\r
 277                                         //1区間\r
 278                                         n = ed - st+1;\r
 279                                         addCoordVecPos(i_raster,i_xcoord,i_ycoord,st,ed+1,vecsum);\r
 280                                         this._distmap.getIdealCoodCenter(i_xcoord, i_ycoord,st,n,idealcenter);\r
 281                                 }else{\r
 282                                         //探索区間は2区間\r
 283                                         double cx,cy;\r
 284                                         n=ed+i_cood_num-st+1;\r
 285                                         addCoordVecPos(i_raster,i_xcoord,i_ycoord,st,i_cood_num,vecsum);\r
 286                                         addCoordVecPos(i_raster,i_xcoord,i_ycoord,0,ed,vecsum);\r
 287                                         //輪郭の中心位置を計算\r
 288                                         this._distmap.getIdealCoodCenter(i_xcoord, i_ycoord,st,i_cood_num-st,idealcenter);\r
 289                                         cx=idealcenter.x;\r
 290                                         cy=idealcenter.y;\r
 291                                         this._distmap.getIdealCoodCenter(i_xcoord, i_ycoord,0,ed+1,idealcenter);\r
 292                                         idealcenter.x=(idealcenter.x+cx)/2;\r
 293                                         idealcenter.y=(idealcenter.y+cy)/2;\r
 294                                 }\r
 295                                 //中央値を歪修正（ほんとはピクセル単位にゆがみ矯正するべきだと思う）\r
 296                                 \r
 297                                 double l=Math.sqrt((double)(vecsum.x*vecsum.x+vecsum.y*vecsum.y));\r
 298                                 final NyARLinear l_line_i = l_line[i];\r
 299                                 //直交するベクトルを計算\r
 300                                 l_line_i.dy =  vecsum.x/l;\r
 301                                 l_line_i.dx =  -vecsum.y/l;\r
 302                                 //cを計算\r
 303                                 l_line_i.c  = -(l_line_i.dy * (idealcenter.x) + l_line_i.dx * (idealcenter.y));\r
 304                                 \r
 305                                 // 頂点インデクスから頂点座標を得て保存\r
 306                                 l_imvertex[i].x = i_xcoord[ver1];\r
 307                                 l_imvertex[i].y = i_ycoord[ver1];\r
 308                         }\r
 309                         //線分式から頂点を計算\r
 310                         for(int i=0;i<4;i++)\r
 311                         {\r
 312                                 if(!NyARLinear.crossPos(l_line[i],l_line[(i + 3) % 4],l_sqvertex[i])){\r
 313                                         return false;\r
 314                                 }\r
 315                         }               \r
 316                         return true;\r
 317                 }\r
 318         }\r
 319 \r
 320         /**\r
 321          * 輪郭線の中心位置を計算する関数を追加したマップクラス\r
 322          */\r
 323         private class NyARObserv2IdealMap2 extends NyARObserv2IdealMap\r
 324         {\r
 325                 public NyARObserv2IdealMap2(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size)\r
 326                 {\r
 327                         super(i_distfactor,i_screen_size);\r
 328                 }\r
 329                 /**\r
 330                  * 歪み矯正した座標における、各座標の合計値を\r
 331                  * @param i_x_coord\r
 332                  * @param i_y_coord\r
 333                  * @param i_start\r
 334                  * @param i_num\r
 335                  * @param o_center\r
 336                  */\r
 337                 public void getIdealCoodCenter(int[] i_x_coord, int[] i_y_coord,int i_start, int i_num,NyARDoublePoint2d o_center)\r
 338                 {\r
 339                         int idx;\r
 340                         double x,y;\r
 341                         x=y=0;\r
 342                         final double[] mapx=this._mapx;\r
 343                         final double[] mapy=this._mapy;\r
 344                         final int stride=this._stride;\r
 345                         for (int j = 0; j < i_num; j++){\r
 346                                 idx=i_x_coord[i_start + j]+i_y_coord[i_start + j]*stride;\r
 347                                 x+=mapx[idx];\r
 348                                 y+=mapy[idx];\r
 349                         }\r
 350                         o_center.x=x/(double)i_num;\r
 351                         o_center.y=y/(double)i_num;\r
 352                         return;\r
 353                 }\r
 354         }       \r
 355 }\r
 356 \r
 357 \r
 358 \r