tags/2.3.1/src/jp/nyatla/nyartoolkit/core/transmat/fitveccalc/NyARFitVecCalculator.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.transmat.fitveccalc;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.core.*;\r
  35 import jp.nyatla.nyartoolkit.core.transmat.NyARTransOffset;\r
  36 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
  37 import jp.nyatla.nyartoolkit.core.types.*;\r
  38 import jp.nyatla.nyartoolkit.*;\r
  39 import jp.nyatla.nyartoolkit.core.param.*;\r
  40 /**\r
  41  * 平行移動量を計算するクラス\r
  42  * \r
  43  * NyARPerspectiveProjectionMatrixに直接アクセスしてる場所があるけど、\r
  44  * この辺の計算はNyARPerspectiveProjectionMatrixクラスの関数にして押し込む予定。\r
  45  *\r
  46  */\r
  47 public class NyARFitVecCalculator\r
  48 {\r
  49         private final NyARMat _mat_b = new NyARMat(3,8);//3,NUMBER_OF_VERTEX*2\r
  50         private final NyARMat _mat_a = new NyARMat(8,3);/*NUMBER_OF_VERTEX,3*/\r
  51         private final NyARMat _mat_d = new NyARMat(3,3);\r
  52         private final NyARPerspectiveProjectionMatrix _projection_mat;\r
  53         private final NyARCameraDistortionFactor _distortionfactor;\r
  54 \r
  55 \r
  56 //      private NyARDoublePoint2d[] _vertex_2d_ref;\r
  57         public NyARFitVecCalculator(final NyARPerspectiveProjectionMatrix i_projection_mat_ref,final NyARCameraDistortionFactor i_distortion_ref)\r
  58         {\r
  59                 // 変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
  60                 final double[][] a_array = this._mat_a.getArray();\r
  61                 final double[][] b_array = this._mat_b.getArray();\r
  62 \r
  63                 //変換用行列のcpara固定値の部分を先に初期化してしまう。\r
  64                 for (int i = 0; i < 4; i++) {\r
  65                         final int x2 = i * 2;\r
  66                         a_array[x2][0] = b_array[0][x2] = i_projection_mat_ref.m00;// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0];\r
  67                         a_array[x2][1] = b_array[1][x2] = i_projection_mat_ref.m01;// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1];\r
  68                         //a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_marker_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];\r
  69                         a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0;\r
  70                         a_array[x2 + 1][1] = b_array[1][x2 + 1] = i_projection_mat_ref.m11;// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1];\r
  71                         //a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - o_marker_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];\r
  72                 }\r
  73                 this._projection_mat=i_projection_mat_ref;\r
  74                 this._distortionfactor=i_distortion_ref;\r
  75                 return;\r
  76         }\r
  77         private final NyARDoublePoint2d[] _fitsquare_vertex=NyARDoublePoint2d.createArray(4);;\r
  78         private NyARTransOffset _offset_square;\r
  79         public void setOffsetSquare(NyARTransOffset i_offset)\r
  80         {\r
  81                 this._offset_square=i_offset;\r
  82                 return;\r
  83         }\r
  84         public NyARDoublePoint2d[] getFitSquare()\r
  85         {\r
  86                 return this._fitsquare_vertex;\r
  87         }\r
  88         public NyARTransOffset getOffsetVertex()\r
  89         {\r
  90                 return this._offset_square;\r
  91         }\r
  92 \r
  93         /**\r
  94          * 適合させる矩形座標を指定します。\r
  95          * @param i_square_vertex\r
  96          * @throws NyARException\r
  97          */\r
  98         public void setFittedSquare(NyARDoublePoint2d[] i_square_vertex) throws NyARException\r
  99         {\r
 100                 final NyARDoublePoint2d[] vertex=_fitsquare_vertex;\r
 101 //              int i;\r
 102 //              if (arFittingMode == AR_FITTING_TO_INPUT) {\r
 103 //                      // arParamIdeal2Observをバッチ処理\r
 104                 this._distortionfactor.ideal2ObservBatch(i_square_vertex, vertex,4);\r
 105 //              } else {\r
 106 //                      for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
 107 //                              o_marker_vertex_2d[i].x = i_square_vertex[i].x;\r
 108 //                              o_marker_vertex_2d[i].y = i_square_vertex[i].y;\r
 109 //                      }\r
 110 //              }               \r
 111                 \r
 112                 \r
 113                 final double cpara02=this._projection_mat.m02;\r
 114                 final double cpara12=this._projection_mat.m12;          \r
 115                 final NyARMat mat_d=_mat_d;\r
 116                 final NyARMat mat_a=this._mat_a;\r
 117                 final NyARMat mat_b=this._mat_b;\r
 118                 final double[][] a_array = mat_a.getArray();\r
 119                 final double[][] b_array = mat_b.getArray();\r
 120                 for (int i = 0; i < 4; i++) {\r
 121                         final int x2 = i * 2;   \r
 122                         a_array[x2][2] = b_array[2][x2] = cpara02 - vertex[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];\r
 123                         a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara12 - vertex[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];\r
 124                 }\r
 125                 // mat_d\r
 126                 mat_d.matrixMul(mat_b, mat_a);\r
 127                 mat_d.matrixSelfInv();          \r
 128                 return;\r
 129         }\r
 130         private final NyARMat _mat_e = new NyARMat(3, 1);\r
 131         private final NyARMat _mat_f = new NyARMat(3, 1);\r
 132         private final NyARMat __calculateTransferVec_mat_c = new NyARMat(8, 1);//NUMBER_OF_VERTEX * 2, 1\r
 133         private final NyARDoublePoint3d[] __calculateTransfer_point3d=NyARDoublePoint3d.createArray(4); \r
 134         \r
 135         /**\r
 136          * 現在のオフセット矩形、適合先矩形と、回転行列から、平行移動量を計算します。\r
 137          * @param i_rotation\r
 138          * @param o_transfer\r
 139          * @throws NyARException\r
 140          */\r
 141         final public void calculateTransfer(NyARRotMatrix i_rotation,NyARDoublePoint3d o_transfer) throws NyARException\r
 142         {\r
 143                 assert(this._offset_square!=null);\r
 144                 final double cpara00=this._projection_mat.m00;\r
 145                 final double cpara01=this._projection_mat.m01;\r
 146                 final double cpara02=this._projection_mat.m02;\r
 147                 final double cpara11=this._projection_mat.m11;\r
 148                 final double cpara12=this._projection_mat.m12;\r
 149                 \r
 150                 final NyARDoublePoint3d[] point3d=this.__calculateTransfer_point3d;\r
 151                 final NyARDoublePoint3d[] vertex3d=this._offset_square.vertex;          \r
 152                 final NyARDoublePoint2d[] vertex2d=this._fitsquare_vertex;\r
 153                 final NyARMat mat_c = this.__calculateTransferVec_mat_c;// 次処理で値をもらうので、初期化の必要は無い。\r
 154         \r
 155                 final double[][] f_array = this._mat_f.getArray();\r
 156                 final double[][] c_array = mat_c.getArray();\r
 157                 \r
 158                 \r
 159                 //（3D座標？）を一括請求\r
 160                 i_rotation.getPoint3dBatch(vertex3d,point3d,4);\r
 161                 for (int i = 0; i < 4; i++) {\r
 162                         final int x2 = i+i;\r
 163                         final NyARDoublePoint3d point3d_ptr=point3d[i];\r
 164 //                      i_rotation.getPoint3d(vertex3d[i],point3d);\r
 165                         //透視変換？\r
 166                         c_array[x2][0] = point3d_ptr.z * vertex2d[i].x - cpara00 * point3d_ptr.x - cpara01 * point3d_ptr.y - cpara02 * point3d_ptr.z;// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz;\r
 167                         c_array[x2 + 1][0] = point3d_ptr.z * vertex2d[i].y - cpara11 * point3d_ptr.y - cpara12 * point3d_ptr.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz;\r
 168                 }\r
 169                 this._mat_e.matrixMul(this._mat_b, mat_c);\r
 170                 this._mat_f.matrixMul(this._mat_d, this._mat_e);\r
 171 \r
 172                 // double[] trans=wk_arGetTransMatSub_trans;//double trans[3];\r
 173                 o_transfer.x= f_array[0][0];// trans[0] = mat_f->m[0];\r
 174                 o_transfer.y= f_array[1][0];\r
 175                 o_transfer.z= f_array[2][0];// trans[2] = mat_f->m[2];\r
 176                 return;\r
 177         }\r
 178         \r
 179         \r
 180         \r
 181 }\r