sample/toys/jp/nyatla/nyartoolkit/toys/qrcode/NyARQrCodeSymbolBinder.java

   1 package jp.nyatla.nyartoolkit.toys.qrcode;\r
   2 \r
   3 import java.awt.Color;\r
   4 import java.awt.Graphics;\r
   5 \r
   6 import jp.nyatla.nyartoolkit.core.*;\r
   7 import jp.nyatla.nyartoolkit.core.types.*;\r
   8 import jp.nyatla.utils.j2se.*;\r
   9 import jp.nyatla.nyartoolkit.core.param.*;\r
  10 /**\r
  11  * QRコードのシンボルを結びつける偉いクラス\r
  12  * アルゴリズムはこんな感じ。\r
  13  * 1.3シンボルの位置関係から中間のシンボルを探す。\r
  14  * 2.中間シンボルの内角点を探す\r
  15  * 3.残りの２シンボル間の最短距離の頂点セットを見つけて、それぞれの内角点を探す\r
  16  * 4.３個の内角点が決まったら、各シンボルごとに外角点（反対側の頂点）を特定する。\r
  17  * 5.対角のシンボルの外角頂点から伸びる線分を合成して、矩形を決める。\r
  18  * 6.矩形が決まったら、方程式を解いて交点を出して、頂点にする。\r
  19  * 7.交点と中央のシンボルの位置関係から、正しい計算が行われたかを判定（まだ実装してない）\r
  20  * \r
  21  * \r
  22  * この方法は浅い角度でシンボル集合を見たときに、1や3の手順が高い確率で失敗する。\r
  23  * その場合計算が途中で破綻するのでわかる（はず）\r
  24  * 他の方法もあるけど、それはまた今度。\r
  25  */\r
  26 public class NyARQrCodeSymbolBinder\r
  27 {\r
  28         private NyARCameraDistortionFactor _distfactor;\r
  29 \r
  30         public NyARQrCodeSymbolBinder(NyARCameraDistortionFactor i_ref_distortion)\r
  31         {\r
  32                 this._distfactor=i_ref_distortion;\r
  33                 return;\r
  34         }\r
  35         /**\r
  36          * 最小の三角形を構成する頂点セットを得る\r
  37          * @param i_s0\r
  38          * @param i_s1\r
  39          * @param i_s2\r
  40          * @param o_vertex\r
  41          */\r
  42         private static void getMinimumTriangleVertex(NyARSquare[] i_sqare,int[] o_vertex_id)\r
  43         {\r
  44                 //辺の長さが最小になる頂点の組合せを探す\r
  45                 int d;\r
  46                 int x,y;\r
  47                 int dmax=0x7fffffff;\r
  48                 final NyARIntPoint[] vertex0=i_sqare[0].imvertex;\r
  49                 final NyARIntPoint[] vertex1=i_sqare[1].imvertex;\r
  50                 final NyARIntPoint[] vertex2=i_sqare[2].imvertex;\r
  51                 for(int i=0;i<4;i++)\r
  52                 {\r
  53                         for(int i2=0;i2<4;i2++)\r
  54                         {\r
  55                                 for(int i3=0;i3<4;i3++){\r
  56                                         x=vertex0[i].x-vertex2[i3].x;\r
  57                                         y=vertex0[i].y-vertex2[i3].y;\r
  58                                         d=x*x+y*y;\r
  59                                         x=vertex1[i2].x-vertex2[i3].x;\r
  60                                         y=vertex1[i2].y-vertex2[i3].y;\r
  61                                         d+=x*x+y*y;\r
  62                                         x=vertex1[i2].x-vertex0[i].x;\r
  63                                         y=vertex1[i2].y-vertex0[i].y;\r
  64                                         d+=x*x+y*y;\r
  65                                         if(d<dmax){\r
  66                                                 dmax=d;\r
  67                                                 o_vertex_id[0]=i;                                       \r
  68                                                 o_vertex_id[1]=i2;\r
  69                                                 o_vertex_id[2]=i3;\r
  70                                         }\r
  71                                 }\r
  72                         }\r
  73                 }\r
  74                 return;\r
  75         }\r
  76         /**\r
  77          * 2矩形の頂点距離が最低の組合せを探す\r
  78          * @param i_sqare\r
  79          * @param o_vertex_id\r
  80          */\r
  81         private static void getMinimumLineVertex(NyARIntPoint[] i_sqare0,NyARIntPoint[] i_sqare1,int[] o_vertex_id)\r
  82         {\r
  83                 //辺の長さが最小になる頂点の組合せを探す\r
  84                 int d;\r
  85                 int x,y;\r
  86                 int dmax=0x7fffffff;\r
  87                 for(int i=0;i<4;i++)\r
  88                 {\r
  89                         for(int i2=0;i2<4;i2++)\r
  90                         {\r
  91                                 x=i_sqare1[i2].x-i_sqare0[i].x;\r
  92                                 y=i_sqare1[i2].y-i_sqare0[i].y;\r
  93                                 d=x*x+y*y;\r
  94                                 if(d<dmax){\r
  95                                         dmax=d;\r
  96                                         o_vertex_id[0]=i;                                       \r
  97                                         o_vertex_id[1]=i2;\r
  98                                 }\r
  99                         }\r
 100                 }\r
 101                 return;\r
 102         }\r
 103         /**\r
 104          * シンボルグループの重心を計算する\r
 105          * @param i_sqare\r
 106          * @param i_center\r
 107          */\r
 108         private void getSymbolGroupCenter(NyARSquare[] i_sqare,NyARIntPoint i_center)\r
 109         {\r
 110                 //シンボルグループの重心を計算\r
 111                 int cx,cy;\r
 112                 cx=cy=0;\r
 113                 for(int i=0;i<3;i++)\r
 114                 {\r
 115                         final NyARIntPoint[] sq_ptr=i_sqare[i].imvertex;\r
 116                         cx+=sq_ptr[0].x;                        \r
 117                         cx+=sq_ptr[1].x;                        \r
 118                         cx+=sq_ptr[2].x;                        \r
 119                         cx+=sq_ptr[3].x;                        \r
 120                         cy+=sq_ptr[0].y;                        \r
 121                         cy+=sq_ptr[1].y;                        \r
 122                         cy+=sq_ptr[2].y;                        \r
 123                         cy+=sq_ptr[3].y;                        \r
 124                 }\r
 125                 i_center.x=cx/12;\r
 126                 i_center.y=cy/12;       \r
 127                 return;\r
 128         }\r
 129         /**\r
 130          * キーシンボルのインデックスを得る\r
 131          * @param i_sqare\r
 132          * @param i_vertex_id\r
 133          * 最小三角形の頂点IDセット\r
 134          * @return\r
 135          */\r
 136         private static int getKeySymble(NyARSquare[] i_sqare,NyARIntPoint i_center,int[] i_vertex_id)\r
 137         {\r
 138                 //シンボルグループの重心を計算\r
 139                 final int cx=i_center.x;\r
 140                 final int cy=i_center.y;        \r
 141                 //前段で探した頂点候補のうち、最も重心に近いものが中心シンボルの内対角点\r
 142                 int key_symble_idx=0;\r
 143                 int x=i_sqare[0].imvertex[i_vertex_id[0]].x-cx;\r
 144                 int y=i_sqare[0].imvertex[i_vertex_id[0]].y-cy;\r
 145                 int dmax=x*x+y*y;\r
 146                 for(int i=1;i<3;i++){\r
 147                         x=i_sqare[i].imvertex[i_vertex_id[i]].x-cx;\r
 148                         y=i_sqare[i].imvertex[i_vertex_id[i]].y-cy;\r
 149                         final int d=x*x+y*y;\r
 150                         if(d<dmax){\r
 151                                 dmax=d;\r
 152                                 key_symble_idx=i;\r
 153                         }\r
 154                 }\r
 155                 return key_symble_idx;\r
 156         }\r
 157         private NyARDoublePoint2d __bindSquare_ideal_vertex=new NyARDoublePoint2d();\r
 158         /**\r
 159          * 2つの対角にある矩形から、それらを対角とする矩形を作る。\r
 160          * @param i_sq1\r
 161          * @param i_lv1\r
 162          * @param i_sq2\r
 163          * @param i_lv2\r
 164          */\r
 165         private void bindSquare(NyARSquare i_sq1,int i_lv1,NyARSquare i_sq2,int i_lv2,NyARSquare o_qr_square)\r
 166         {\r
 167                 //4辺の式を計算\r
 168                 o_qr_square.line[0].copyFrom(i_sq1.line[(i_lv1+3)%4]);\r
 169                 o_qr_square.line[1].copyFrom(i_sq1.line[(i_lv1+0)%4]);\r
 170                 o_qr_square.line[2].copyFrom(i_sq2.line[(i_lv2+3)%4]);\r
 171                 o_qr_square.line[3].copyFrom(i_sq2.line[(i_lv2+0)%4]);\r
 172                 //歪み無しの座標系を計算\r
 173                 final NyARDoublePoint2d[] l_sqvertex = o_qr_square.sqvertex;\r
 174                 final NyARIntPoint[] imvertex_ptr = o_qr_square.imvertex;\r
 175 \r
 176                 final NyARLinear[] l_line = o_qr_square.line;\r
 177                 final NyARDoublePoint2d ideal_vertex=this.__bindSquare_ideal_vertex;\r
 178                 for (int i = 0; i < 4; i++) {\r
 179                         final NyARLinear l_line_i = l_line[i];\r
 180                         final NyARLinear l_line_2 = l_line[(i + 3) % 4];\r
 181                         final double w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;\r
 182                         if (w1 == 0.0) {\r
 183                                 return;\r
 184                         }\r
 185                         l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;\r
 186                         l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;\r
 187                         _distfactor.ideal2Observ(l_sqvertex[i], ideal_vertex);\r
 188                         //Ideal→observに変換して、画面上の座標とする。\r
 189                         imvertex_ptr[i].x=(int)l_sqvertex[i].x;\r
 190                         imvertex_ptr[i].y=(int)l_sqvertex[i].y;\r
 191                 }       \r
 192 //              Graphics g=this.bimg.getGraphics();\r
 193 //              g.setColor(Color.red);\r
 194 //              int[] x=new int[4];\r
 195 //              int[] y=new int[4];\r
 196 //              for(int i=0;i<4;i++){\r
 197 //                      x[i]=(int)l_sqvertex[i].x;\r
 198 //                      y[i]=(int)l_sqvertex[i].y;\r
 199 //              }\r
 200 //              g.drawPolygon(x,y,4);\r
 201                 return;\r
 202                 //基準点はVertexをそのまま採用\r
 203                 //２個の想定点は座標を逆変換して設定\r
 204         }\r
 205         /**\r
 206          * directionはキーシンボルのインデックスでARToolKitの頂点座標じゃないので注意すること。\r
 207          * @param i_sq\r
 208          * @param o_sq\r
 209          * @return\r
 210          */\r
 211         public boolean composeSquare(NyARSquare[] i_sq,NyARSquare o_sq)\r
 212         {\r
 213                 int[] minimum_triangle_vertex=new int[3];\r
 214                 int[] minimum_line_vertex=new int[2];\r
 215                 \r
 216                 NyARIntPoint center=new NyARIntPoint();\r
 217 \r
 218                 //辺の長さが最小になる頂点の組合せを探す\r
 219                 getMinimumTriangleVertex(i_sq,minimum_triangle_vertex);\r
 220                 \r
 221                 //中心位置を計算する。\r
 222                 getSymbolGroupCenter(i_sq,center);\r
 223                 \r
 224                 //キーシンボルのインデクス番号を得る\r
 225                 int key_simble_idx=getKeySymble(i_sq,center,minimum_triangle_vertex);\r
 226                 \r
 227                 //対角シンボルのインデックス番号を決める\r
 228                 int symbol_e1_idx=(key_simble_idx+1)%3;\r
 229                 int symbol_e2_idx=(key_simble_idx+2)%3;\r
 230                 \r
 231                 //対角シンボル間で最短距離を取る頂点ペアを取る\r
 232                 //(角度を低くするとエラーが出やすい。対角線との類似性を確認する方法のほうがいい。多分)\r
 233                 getMinimumLineVertex(i_sq[symbol_e1_idx].imvertex,i_sq[symbol_e2_idx].imvertex,minimum_line_vertex);\r
 234                 \r
 235                 //内対角を外対角に変換\r
 236                 int lv1=(minimum_line_vertex[0]+2)%4;\r
 237                 int lv2=(minimum_line_vertex[1]+2)%4;\r
 238                 int kv =(minimum_triangle_vertex[key_simble_idx]+2)%4;\r
 239                 //矩形の合成\r
 240                 bindSquare(i_sq[symbol_e1_idx],lv1,i_sq[symbol_e2_idx],lv2,o_sq);\r
 241                 \r
 242                 //方位判定\r
 243                 //基点(中央シンボルを0として時計回りにインクリメント.基点の座標1か3\r
 244                 \r
 245                 int direction=getDirection(o_sq,i_sq[key_simble_idx].imvertex[kv],center);\r
 246                 if(direction==-1){\r
 247                         return false;\r
 248                 }\r
 249                 o_sq.direction=direction;\r
 250                 System.out.println(o_sq.direction);\r
 251                 \r
 252                 return true;\r
 253         }       \r
 254         /**\r
 255          * この関数はあんまり頂点ズレがひどいと失敗する\r
 256          * @param i_square\r
 257          * @param i_vertex\r
 258          * @param i_center\r
 259          * @return\r
 260          */\r
 261         private int getDirection(NyARSquare i_square,NyARIntPoint i_vertex,NyARIntPoint i_center)\r
 262         {\r
 263                 //開始点(中央シンボル)までの頂点のシフト数を決める\r
 264                 int x,y;\r
 265                 x=i_square.imvertex[0].x-i_vertex.x;\r
 266                 y=i_square.imvertex[0].y-i_vertex.y;\r
 267                 int v1=x*x+y*y;\r
 268                 x=i_square.imvertex[2].x-i_vertex.x;\r
 269                 y=i_square.imvertex[2].y-i_vertex.y;\r
 270                 int v2=x*x+y*y;\r
 271                 int shift;\r
 272                 int v;\r
 273                 if(v1<v2){\r
 274                         shift=0;\r
 275                         v=v1;\r
 276                 }else{\r
 277                         shift=2;\r
 278                         v=v2;\r
 279                 }\r
 280                 //小さい方の対角線が64(8x8)より大きくずれてたら認識ミスとみなす\r
 281                 if(v>64){\r
 282                         return -1;\r
 283                 }\r
 284                 //シンボルがどの象限にあるか確認する\r
 285                 x=i_vertex.x=i_center.x;\r
 286                 y=i_vertex.y=i_center.y;\r
 287                 int dir;\r
 288                 if(x<0){\r
 289                         dir=2;//dir=y<0?1:2;\r
 290                 }else{\r
 291                         dir=4;//dir=y<0?3:4;\r
 292                 }\r
 293                 return (dir+shift)%4;\r
 294         }\r
 295         \r
 296         \r
 297         \r
 298 }