ARToolKitのパターン抽出

NyARCodeとか、NyARColorPattのあたりの書き直しをしているんだけど、いまいちわからない部分がある。

NyARColorPattのパターンピックアップ処理なんだけど、8x8行列から逆行列を作って、そこに頂点座標を乗算して、パターンを抽出する時に使う３個の８個のパラメータを計算してる。

行列を計算しているのはget_cpara関数で、ここで行列AとBを掛け算してる。vx,vyは、それぞれ４頂点のxy座標。

行列A

100	100	1	0	0	0	-100*vx[0]	-100*vy[0]
0	0	0	100	100	1	-100*vx[0]	-100*vy[0]
110	100	1	0	0	0	-110*vx[1]	-100*vy[1]
0	0	0	110	100	1	-110*vx[1]	-100*vy[1]
110	110	1	0	0	0	-110*vx[2]	-110*vy[2]
0	0	0	110	110	1	-110*vx[2]	-110*vy[2]
100	110	1	0	0	0	-100*vx[3]	-110*vy[3]
0	0	0	100	110	1	-100*vx[3]	-110*vy[3]

行列B

vx[0] vy[0] vx[1] vy[1] vx[2] vy[2] vx[3] vy[3] vx[4] vy[4]

で、これをpickFromRasterのピクセル抽出の時に使ってる。

なんか名前付いてるアルゴリズムなのかな？
アフィン変換とかじゃなさそうだけど・・・。

public class NyARColorPatt_O1 implements INyARColorPatt
{
  private static final int AR_PATT_SAMPLE_NUM = 64;
  
  private int[] _patdata;
  private NyARBufferReader _buf_reader;

  private NyARIntSize _size;

  public NyARColorPatt_O1(int i_width, int i_height)
  {
    //入力制限
    assert i_width<=64;
    assert i_height<=64;
    
    this._size=new NyARIntSize(i_width,i_height);
    this._patdata = new int[i_height*i_width];
    this._buf_reader=new NyARBufferReader(this._patdata,NyARBufferReader.BUFFERFORMAT_INT1D_X8R8G8B8_32);
    return;
  }

  public int getWidth()
  {
    return this._size.w;
  }
  public int getHeight()
  {
    return this._size.h;
  }
  public NyARIntSize getSize()
  {
    return   this._size;
  }
  public INyARBufferReader getBufferReader()
  {
    return this._buf_reader;
  }

  private final NyARMat wk_get_cpara_a = new NyARMat(8, 8);

  private final NyARMat wk_get_cpara_b = new NyARMat(8, 1);

  private final NyARMat wk_get_cpara_c = new NyARMat(8, 1);

  /**
   * 
   * @param world
   * @param vertex
   * @param para
   *            [3x3]
   * @throws NyARException
   */
  private boolean get_cpara(double world[][], double vertex[][], double[] para)throws NyARException
  {
    NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
    double[][] a_array = a.getArray();
    NyARMat b = wk_get_cpara_b;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );
    double[][] b_array = b.getArray();
    double[] a_pt0, a_pt1, world_pti;

    for (int i = 0; i < 4; i++) {
      a_pt0 = a_array[i * 2];
      a_pt1 = a_array[i * 2 + 1];
      world_pti = world[i];

      a_pt0[0] = world_pti[0];// a->m[i*16+0] = world[i][0];
      a_pt0[1] = world_pti[1];// a->m[i*16+1] = world[i][1];
      a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;
      a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;
      a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;
      a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;
      a_pt0[6] = -world_pti[0] * vertex[i][0];// a->m[i*16+6] =-world[i][0] *vertex[i][0];
      a_pt0[7] = -world_pti[1] * vertex[i][0];// a->m[i*16+7] =-world[i][1] *vertex[i][0];
      a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;
      a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;
      a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;
      a_pt1[3] = world_pti[0];// a->m[i*16+11] = world[i][0];
      a_pt1[4] = world_pti[1];// a->m[i*16+12] = world[i][1];
      a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;
      a_pt1[6] = -world_pti[0] * vertex[i][1];// a->m[i*16+14] =-world[i][0] *vertex[i][1];
      a_pt1[7] = -world_pti[1] * vertex[i][1];// a->m[i*16+15] =-world[i][1] *vertex[i][1];
      b_array[i * 2 + 0][0] = vertex[i][0];// b->m[i*2+0] =vertex[i][0];
      b_array[i * 2 + 1][0] = vertex[i][1];// b->m[i*2+1] =vertex[i][1];
    }
    if (!a.matrixSelfInv()) {
      return false;// 逆行列を求められないので失敗
    }
    NyARMat c = wk_get_cpara_c;// 次処理で結果を受け取るので、初期化不要//new NyARMat( 8, 1 );
    double[][] c_array = c.getArray();

    c.matrixMul(a, b);
    for (int i = 0; i < 2; i++) {
      para[i * 3 + 0] = c_array[i * 3 + 0][0];// para[i][0] = c->m[i*3+0];
      para[i * 3 + 1] = c_array[i * 3 + 1][0];// para[i][1] = c->m[i*3+1];
      para[i * 3 + 2] = c_array[i * 3 + 2][0];// para[i][2] = c->m[i*3+2];
    }
    para[2 * 3 + 0] = c_array[2 * 3 + 0][0];// para[2][0] = c->m[2*3+0];
    para[2 * 3 + 1] = c_array[2 * 3 + 1][0];// para[2][1] = c->m[2*3+1];
    para[2 * 3 + 2] = 1.0;// para[2][2] = 1.0;
    return true;
  }

  private final double[][] wk_pickFromRaster_local = new double[4][2];

  private final double[] wk_pickFromRaster_para = new double[9];// [3][3];

  private final double[][] wk_pickFromRaster_world = {// double world[4][2];
  { 100.0, 100.0 }, { 100.0 + 10.0, 100.0 }, { 100.0 + 10.0, 100.0 + 10.0 },{ 100.0, 100.0 + 10.0 } };


  private final int[] wk_pickFromRaster_rgb_tmp = new int[3];

  /**
   * imageから、i_markerの位置にあるパターンを切り出して、保持します。 Optimize:STEP[769->]
   * 
   * @param image
   * @param i_marker
   * @return 切り出しに失敗した
   * @throws Exception
   */
  public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException
  {
    int lx1, lx2, ly1, ly2;

    int img_x = image.getWidth();
    int img_y = image.getHeight();

    double xdiv2_reciprocal; // [tp]
    double ydiv2_reciprocal; // [tp]

    // int[] x_coord=i_marker.x_coord;
    // int[] y_coord=i_marker.y_coord;
    // int[] vertex=i_marker.mkvertex;
    double[][] local = wk_pickFromRaster_local;// double local[4][2];
    //
    for (int i = 0; i < 4; i++) {
      local[i][0] = i_square.imvertex[i].x;
      local[i][1] = i_square.imvertex[i].y;
    }

    double[][] world = wk_pickFromRaster_world;
    /*
     * world[0][0] = 100.0; world[0][1] = 100.0; world[1][0] = 100.0 + 10.0;
     * world[1][1] = 100.0; world[2][0] = 100.0 + 10.0; world[2][1] = 100.0 +
     * 10.0; world[3][0] = 100.0; world[3][1] = 100.0 + 10.0;
     */
    double[] para = wk_pickFromRaster_para; // double para[3][3];
    // パターンの切り出しに失敗することもある。
    if (!get_cpara(world, local, para)) {
      return false;
    }
    lx1 = (int) ((local[0][0] - local[1][0]) * (local[0][0] - local[1][0]) + (local[0][1] - local[1][1])* (local[0][1] - local[1][1]));
    lx2 = (int) ((local[2][0] - local[3][0]) * (local[2][0] - local[3][0]) + (local[2][1] - local[3][1])* (local[2][1] - local[3][1]));
    ly1 = (int) ((local[1][0] - local[2][0]) * (local[1][0] - local[2][0]) + (local[1][1] - local[2][1])* (local[1][1] - local[2][1]));
    ly2 = (int) ((local[3][0] - local[0][0]) * (local[3][0] - local[0][0]) + (local[3][1] - local[0][1])* (local[3][1] - local[0][1]));
    if (lx2 > lx1) {
      lx1 = lx2;
    }
    if (ly2 > ly1) {
      ly1 = ly2;
    }
    int sample_pixel_x = this._size.w;
    int sample_pixel_y = this._size.h;

    while (sample_pixel_x * sample_pixel_x < lx1 / 4) {
      sample_pixel_x *= 2;
    }
    while (sample_pixel_y * sample_pixel_y < ly1 / 4) {
      sample_pixel_y *= 2;
    }

    if (sample_pixel_x > AR_PATT_SAMPLE_NUM) {
      sample_pixel_x = AR_PATT_SAMPLE_NUM;
    }
    if (sample_pixel_y > AR_PATT_SAMPLE_NUM) {
      sample_pixel_y = AR_PATT_SAMPLE_NUM;
    }

    final int xdiv = sample_pixel_x / this._size.w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;
    final int ydiv = sample_pixel_y / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;


    xdiv2_reciprocal = 1.0 / sample_pixel_x;
    ydiv2_reciprocal = 1.0 / sample_pixel_y;
    int r,g,b;
    int[] rgb_tmp = wk_pickFromRaster_rgb_tmp;

    //ピクセルリーダーを取得
    INyARRgbPixelReader reader=image.getRgbPixelReader();
    final int xdiv_x_ydiv = xdiv * ydiv;

    for(int iy=0;iy<this._size.h;iy++){
      for(int ix=0;ix<this._size.w;ix++){
        r=g=b=0;
        //1ピクセルを作成
        for(int j=0;j<ydiv;j++){
          final double yw = 102.5 + 5.0 * (iy*ydiv+j + 0.5) * ydiv2_reciprocal;            
          for(int i=0;i<xdiv;i++){
            final double xw = 102.5 + 5.0 * (ix*xdiv+i + 0.5) * xdiv2_reciprocal;
            final double d = para[2 * 3 + 0] * xw + para[2 * 3 + 1] * yw+ para[2 * 3 + 2];
            if (d == 0) {
              throw new NyARException();
            }
            final int xc = (int) ((para[0 * 3 + 0] * xw + para[0 * 3 + 1] * yw + para[0 * 3 + 2]) / d);
            final int yc = (int) ((para[1 * 3 + 0] * xw + para[1 * 3 + 1] * yw + para[1 * 3 + 2]) / d);
  
            if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y) {
              reader.getPixel(xc, yc, rgb_tmp);
              r += rgb_tmp[0];// R
              g += rgb_tmp[1];// G
              b += rgb_tmp[2];// B
              // System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
            }
          }
        }
        this._patdata[iy*this._size.w+ix]=(((r / xdiv_x_ydiv)&0xff)<<16)|(((g / xdiv_x_ydiv)&0xff)<<8)|(((b / xdiv_x_ydiv)&0xff));
      }
    }
    return true;
  }  
}