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