sample/toys/jp/nyatla/nyartoolkit/toys/x2/NyARRotTransOptimize_X2.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.toys.x2;\r
  33 \r
  34 \r
  35 import jp.nyatla.nyartoolkit.NyARException;\r
  36 import jp.nyatla.nyartoolkit.core.param.*;\r
  37 import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.*;\r
  38 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*;\r
  39 import jp.nyatla.nyartoolkit.core.types.*;\r
  40 import jp.nyatla.nyartoolkit.core.transmat.optimize.*;\r
  41 import jp.nyatla.nyartoolkit.toys.x2.NyARSinTable;\r
  42 \r
  43 /**\r
  44  * 基本姿勢と実画像を一致するように、角度を微調整→平行移動量を再計算\r
  45  * を繰り返して、変換行列を最適化する。\r
  46  *\r
  47  */\r
  48 public class NyARRotTransOptimize_X2 implements INyARRotTransOptimize\r
  49 {\r
  50         private NyARSinTable _sin_table_ref;    \r
  51         private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
  52         private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
  53         private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
  54         public NyARRotTransOptimize_X2(NyARPerspectiveProjectionMatrix i_projection_mat_ref,NyARSinTable _sin_table_ref)\r
  55         {\r
  56                 this._sin_table_ref=_sin_table_ref;\r
  57                 this._projection_mat_ref=i_projection_mat_ref;\r
  58                 return;\r
  59         }\r
  60         \r
  61         final public double optimize(NyARRotMatrix io_rotmat,NyARDoublePoint3d io_transvec,NyARFitVecCalculator i_calculator) throws NyARException\r
  62         {\r
  63                 final NyARDoublePoint2d[] fit_vertex=i_calculator.getFitSquare();\r
  64                 final NyARDoublePoint3d[] offset_square=i_calculator.getOffsetVertex().vertex;\r
  65 \r
  66                 \r
  67                 double err = -1;\r
  68                 /*ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。*/\r
  69                 for (int i = 0;; i++) {\r
  70                         // <arGetTransMat3>\r
  71                         err = modifyMatrix(io_rotmat,io_transvec,offset_square,fit_vertex);\r
  72                         i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
  73                         err = modifyMatrix(io_rotmat,io_transvec,offset_square,fit_vertex);                     \r
  74                         // //</arGetTransMat3>\r
  75                         if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT-1) {\r
  76                                 break;\r
  77                         }\r
  78                         i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
  79                 }               \r
  80                 return err;\r
  81         }\r
  82         \r
  83         private final double[][] __modifyMatrix_double1D = new double[8][3];\r
  84         private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d(); \r
  85         /**\r
  86          * arGetRot計算を階層化したModifyMatrix 896\r
  87          * \r
  88          * @param nyrot\r
  89          * @param trans\r
  90          * @param i_vertex3d\r
  91          * [m][3]\r
  92          * @param i_vertex2d\r
  93          * [n][2]\r
  94          * @return\r
  95          * @throws NyARException\r
  96          */\r
  97         private double modifyMatrix(NyARRotMatrix io_rot,NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException\r
  98         {\r
  99                 double factor;\r
 100                 double a2, b2, c2;\r
 101                 double ma = 0.0, mb = 0.0, mc = 0.0;\r
 102                 double h, x, y;\r
 103                 double err, minerr = 0;\r
 104                 int t1, t2, t3;\r
 105                 int s1 = 0, s2 = 0, s3 = 0;\r
 106 \r
 107                 factor = 10.0 * Math.PI / 180.0;\r
 108                 double rot0, rot1, rot3, rot4, rot6, rot7;\r
 109                 double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23;\r
 110                 double combo02_2, combo02_5, combo02_8, combo02_11;\r
 111                 double combo22_2, combo22_5, combo22_8, combo22_11;\r
 112                 double combo12_2, combo12_5, combo12_8, combo12_11;\r
 113                 // vertex展開\r
 114                 final double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;\r
 115                 NyARDoublePoint3d d_pt;\r
 116                 d_pt = i_vertex3d[0];\r
 117                 VX00 = d_pt.x;\r
 118                 VX01 = d_pt.y;\r
 119                 VX02 = d_pt.z;\r
 120                 d_pt = i_vertex3d[1];\r
 121                 VX10 = d_pt.x;\r
 122                 VX11 = d_pt.y;\r
 123                 VX12 = d_pt.z;\r
 124                 d_pt = i_vertex3d[2];\r
 125                 VX20 = d_pt.x;\r
 126                 VX21 = d_pt.y;\r
 127                 VX22 = d_pt.z;\r
 128                 d_pt = i_vertex3d[3];\r
 129                 VX30 = d_pt.x;\r
 130                 VX31 = d_pt.y;\r
 131                 VX32 = d_pt.z;\r
 132                 final double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;\r
 133                 NyARDoublePoint2d d_pt2;\r
 134                 d_pt2 = i_vertex2d[0];\r
 135                 P2D00 = d_pt2.x;\r
 136                 P2D01 = d_pt2.y;\r
 137                 d_pt2 = i_vertex2d[1];\r
 138                 P2D10 = d_pt2.x;\r
 139                 P2D11 = d_pt2.y;\r
 140                 d_pt2 = i_vertex2d[2];\r
 141                 P2D20 = d_pt2.x;\r
 142                 P2D21 = d_pt2.y;\r
 143                 d_pt2 = i_vertex2d[3];\r
 144                 P2D30 = d_pt2.x;\r
 145                 P2D31 = d_pt2.y;\r
 146                 final NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref;\r
 147                 final double CP0, CP1, CP2, CP4, CP5, CP6, CP8, CP9, CP10;\r
 148                 CP0 = prjmat.m00;\r
 149                 CP1 = prjmat.m01;\r
 150                 CP2 = prjmat.m02;\r
 151                 CP4 = prjmat.m10;\r
 152                 CP5 = prjmat.m11;\r
 153                 CP6 = prjmat.m12;\r
 154                 CP8 = prjmat.m20;\r
 155                 CP9 = prjmat.m21;\r
 156                 CP10 = prjmat.m22;\r
 157                 combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03;\r
 158                 combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13;\r
 159                 combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23;\r
 160                 double CACA, SASA, SACA, CA, SA;\r
 161                 double CACACB, SACACB, SASACB, CASB, SASB;\r
 162                 double SACASC, SACACBSC, SACACBCC, SACACC;\r
 163                 final double[][] double1D = this.__modifyMatrix_double1D;\r
 164 \r
 165                 final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
 166 \r
 167                 final double[] a_factor = double1D[1];\r
 168                 final double[] sinb = double1D[2];\r
 169                 final double[] cosb = double1D[3];\r
 170                 final double[] b_factor = double1D[4];\r
 171                 final double[] sinc = double1D[5];\r
 172                 final double[] cosc = double1D[6];\r
 173                 final double[] c_factor = double1D[7];\r
 174                 double w, w2;\r
 175                 double wsin, wcos;\r
 176                 \r
 177                 io_rot.getAngle(angle);// arGetAngle( rot, &a, &b, &c );\r
 178                 a2 = angle.x;\r
 179                 b2 = angle.y;\r
 180                 c2 = angle.z;\r
 181 \r
 182                 // comboの3行目を先に計算\r
 183                 for (int i = 0; i < 10; i++) {\r
 184                         minerr = 1000000000.0;\r
 185                         // sin-cosテーブルを計算(これが外に出せるとは…。)\r
 186                         for (int j = 0; j < 3; j++) {\r
 187                                 w2 = factor * (j - 1);\r
 188                                 w = a2 + w2;\r
 189                                 a_factor[j] = w;\r
 190                                 w = b2 + w2;\r
 191                                 b_factor[j] = w;\r
 192                                 sinb[j] = this._sin_table_ref.sin(w);\r
 193                                 cosb[j] = this._sin_table_ref.cos(w);\r
 194                                 w = c2 + w2;\r
 195                                 c_factor[j] = w;\r
 196                                 sinc[j] = this._sin_table_ref.sin(w);\r
 197                                 cosc[j] = this._sin_table_ref.cos(w);\r
 198                         }\r
 199                         //\r
 200                         for (t1 = 0; t1 < 3; t1++) {\r
 201                                 SA = this._sin_table_ref.sin(a_factor[t1]);\r
 202                                 CA = this._sin_table_ref.cos(a_factor[t1]);\r
 203                                 // Optimize\r
 204                                 CACA = CA * CA;\r
 205                                 SASA = SA * SA;\r
 206                                 SACA = SA * CA;\r
 207                                 for (t2 = 0; t2 < 3; t2++) {\r
 208                                         wsin = sinb[t2];\r
 209                                         wcos = cosb[t2];\r
 210                                         CACACB = CACA * wcos;\r
 211                                         SACACB = SACA * wcos;\r
 212                                         SASACB = SASA * wcos;\r
 213                                         CASB = CA * wsin;\r
 214                                         SASB = SA * wsin;\r
 215                                         // comboの計算1\r
 216                                         combo02 = CP0 * CASB + CP1 * SASB + CP2 * wcos;\r
 217                                         combo12 = CP4 * CASB + CP5 * SASB + CP6 * wcos;\r
 218                                         combo22 = CP8 * CASB + CP9 * SASB + CP10 * wcos;\r
 219 \r
 220                                         combo02_2 = combo02 * VX02 + combo03;\r
 221                                         combo02_5 = combo02 * VX12 + combo03;\r
 222                                         combo02_8 = combo02 * VX22 + combo03;\r
 223                                         combo02_11 = combo02 * VX32 + combo03;\r
 224                                         combo12_2 = combo12 * VX02 + combo13;\r
 225                                         combo12_5 = combo12 * VX12 + combo13;\r
 226                                         combo12_8 = combo12 * VX22 + combo13;\r
 227                                         combo12_11 = combo12 * VX32 + combo13;\r
 228                                         combo22_2 = combo22 * VX02 + combo23;\r
 229                                         combo22_5 = combo22 * VX12 + combo23;\r
 230                                         combo22_8 = combo22 * VX22 + combo23;\r
 231                                         combo22_11 = combo22 * VX32 + combo23;\r
 232                                         for (t3 = 0; t3 < 3; t3++) {\r
 233                                                 wsin = sinc[t3];\r
 234                                                 wcos = cosc[t3];\r
 235                                                 SACASC = SACA * wsin;\r
 236                                                 SACACC = SACA * wcos;\r
 237                                                 SACACBSC = SACACB * wsin;\r
 238                                                 SACACBCC = SACACB * wcos;\r
 239 \r
 240                                                 rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC;\r
 241                                                 rot3 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;\r
 242                                                 rot6 = -CASB * wcos - SASB * wsin;\r
 243 \r
 244                                                 combo00 = CP0 * rot0 + CP1 * rot3 + CP2 * rot6;\r
 245                                                 combo10 = CP4 * rot0 + CP5 * rot3 + CP6 * rot6;\r
 246                                                 combo20 = CP8 * rot0 + CP9 * rot3 + CP10 * rot6;\r
 247 \r
 248                                                 rot1 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;\r
 249                                                 rot4 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;\r
 250                                                 rot7 = CASB * wsin - SASB * wcos;\r
 251                                                 combo01 = CP0 * rot1 + CP1 * rot4 + CP2 * rot7;\r
 252                                                 combo11 = CP4 * rot1 + CP5 * rot4 + CP6 * rot7;\r
 253                                                 combo21 = CP8 * rot1 + CP9 * rot4 + CP10 * rot7;\r
 254                                                 //\r
 255                                                 err = 0.0;\r
 256                                                 h = combo20 * VX00 + combo21 * VX01 + combo22_2;\r
 257                                                 x = P2D00 - (combo00 * VX00 + combo01 * VX01 + combo02_2) / h;\r
 258                                                 y = P2D01 - (combo10 * VX00 + combo11 * VX01 + combo12_2) / h;\r
 259                                                 err += x * x + y * y;\r
 260                                                 h = combo20 * VX10 + combo21 * VX11 + combo22_5;\r
 261                                                 x = P2D10 - (combo00 * VX10 + combo01 * VX11 + combo02_5) / h;\r
 262                                                 y = P2D11 - (combo10 * VX10 + combo11 * VX11 + combo12_5) / h;\r
 263                                                 err += x * x + y * y;\r
 264                                                 h = combo20 * VX20 + combo21 * VX21 + combo22_8;\r
 265                                                 x = P2D20 - (combo00 * VX20 + combo01 * VX21 + combo02_8) / h;\r
 266                                                 y = P2D21 - (combo10 * VX20 + combo11 * VX21 + combo12_8) / h;\r
 267                                                 err += x * x + y * y;\r
 268                                                 h = combo20 * VX30 + combo21 * VX31 + combo22_11;\r
 269                                                 x = P2D30 - (combo00 * VX30 + combo01 * VX31 + combo02_11) / h;\r
 270                                                 y = P2D31 - (combo10 * VX30 + combo11 * VX31 + combo12_11) / h;\r
 271                                                 err += x * x + y * y;\r
 272                                                 if (err < minerr) {\r
 273                                                         minerr = err;\r
 274                                                         ma = a_factor[t1];\r
 275                                                         mb = b_factor[t2];\r
 276                                                         mc = c_factor[t3];\r
 277                                                         s1 = t1 - 1;\r
 278                                                         s2 = t2 - 1;\r
 279                                                         s3 = t3 - 1;\r
 280                                                 }\r
 281                                         }\r
 282                                 }\r
 283                         }\r
 284                         if (s1 == 0 && s2 == 0 && s3 == 0) {\r
 285                                 factor *= 0.5;\r
 286                         }\r
 287                         a2 = ma;\r
 288                         b2 = mb;\r
 289                         c2 = mc;\r
 290                 }\r
 291                 io_rot.setAngle(ma, mb, mc);\r
 292                 /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
 293                 return minerr / 4;\r
 294         }\r
 295 }\r