tags/2.2.0/src/jp/nyatla/nyartoolkit/core/transmat/optimize/NyARRotTransOptimize_Base.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.optimize;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.param.*;\r
  36 import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.NyARFitVecCalculator;\r
  37 import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix;\r
  38 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  39 import jp.nyatla.nyartoolkit.core.types.*;\r
  40 \r
  41 /**\r
  42  * 処理構造がわかる程度に展開したNyARRotTransOptimize\r
  43  * \r
  44  */\r
  45 public class NyARRotTransOptimize_Base implements INyARRotTransOptimize\r
  46 {\r
  47         private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
  48 \r
  49         private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
  50 \r
  51         private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
  52 \r
  53         public NyARRotTransOptimize_Base(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
  54         {\r
  55                 this._projection_mat_ref = i_projection_mat_ref;\r
  56                 return;\r
  57         }\r
  58 \r
  59         final public double optimize(NyARRotMatrix io_rotmat, NyARDoublePoint3d io_transvec, NyARFitVecCalculator i_calculator) throws NyARException\r
  60         {\r
  61                 final NyARDoublePoint2d[] fit_vertex = i_calculator.getFitSquare();\r
  62                 final NyARDoublePoint3d[] offset_square = i_calculator.getOffsetVertex().vertex;\r
  63 \r
  64                 double err = -1;\r
  65                 /* ループを抜けるタイミングをARToolKitと合わせるために変なことしてます。 */\r
  66                 for (int i = 0;; i++) {\r
  67                         // <arGetTransMat3>\r
  68                         err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
  69                         i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
  70                         err = modifyMatrix(io_rotmat, io_transvec, offset_square, fit_vertex);\r
  71                         // //</arGetTransMat3>\r
  72                         if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR || i == AR_GET_TRANS_MAT_MAX_LOOP_COUNT - 1) {\r
  73                                 break;\r
  74                         }\r
  75                         i_calculator.calculateTransfer(io_rotmat, io_transvec);\r
  76                 }\r
  77                 return err;\r
  78         }\r
  79         private double[] __createRotationMap_b_map=new double[6];\r
  80         private double[] __createRotationMap_c_map=new double[6];\r
  81         private double[] __createRotationMap_f=new double[3];\r
  82         private void createRotationMap(NyARDoublePoint3d i_angle,double i_factor,NyARDoubleMatrix33[] i_rot_matrix)\r
  83         {\r
  84                 double sina,cosa,sinb,cosb,sinc,cosc;\r
  85                 double CACA,SASA,SACA,SASB,CASB,SACACB,CACACB,SASACB;\r
  86 \r
  87                 \r
  88                 final double[] f=this.__createRotationMap_f;\r
  89                 final double[] b_map=this.__createRotationMap_b_map;\r
  90                 final double[] c_map=this.__createRotationMap_c_map;\r
  91                 f[0]=-i_factor;\r
  92                 f[1]=0;\r
  93                 f[2]=i_factor;\r
  94                 double ang1,ang2;\r
  95                 //BとCのsinマップを先に作成\r
  96                 for(int i=0;i<3;i++)\r
  97                 {\r
  98                         ang1=i_angle.y + f[i];\r
  99                         b_map[i]  =Math.sin(ang1);\r
 100                         b_map[i+3]=Math.cos(ang1);\r
 101                         ang2=i_angle.z + f[i];\r
 102                         c_map[i]  =Math.sin(ang2);\r
 103                         c_map[i+3]=Math.cos(ang2);\r
 104                 }\r
 105                 int idx=0;\r
 106                 int t1,t2,t3;\r
 107                 for (t1 = 0; t1 < 3; t1++){\r
 108                         ang1=i_angle.x + f[t1];\r
 109                         sina = Math.sin(ang1);\r
 110                         cosa = Math.cos(ang1);                  \r
 111                         CACA = cosa * cosa;\r
 112                         SASA = sina * sina;\r
 113                         SACA = sina * cosa;\r
 114 \r
 115                         for (t2=0;t2<3;t2++){\r
 116                                 sinb = b_map[t2];\r
 117                                 cosb = b_map[t2+3];\r
 118                                 SASB = sina * sinb;\r
 119                                 CASB = cosa * sinb;\r
 120                                 SACACB = SACA * cosb;\r
 121                 CACACB = CACA * cosb;\r
 122                 SASACB = SASA * cosb;           \r
 123                                 for (t3=0;t3<3;t3++) {\r
 124                                         sinc = c_map[t3];\r
 125                                         cosc = c_map[t3+3];\r
 126                                         final NyARDoubleMatrix33 mat_ptr=i_rot_matrix[idx];\r
 127                                         mat_ptr.m00 = CACACB * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
 128                                         mat_ptr.m01 = -CACACB * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
 129                                         mat_ptr.m02 = CASB;\r
 130                                         mat_ptr.m10 = SACACB * cosc - SACA * cosc + SASACB * sinc + CACA * sinc;\r
 131                                         mat_ptr.m11 = -SACACB * sinc + SACA * sinc + SASACB * cosc + CACA * cosc;\r
 132                                         mat_ptr.m12 = SASB;\r
 133                                         mat_ptr.m20 = -CASB * cosc - SASB * sinc;\r
 134                                         mat_ptr.m21 = CASB * sinc - SASB * cosc;\r
 135                                         mat_ptr.m22 = cosb;\r
 136                                         idx++;\r
 137                                 }\r
 138                         }\r
 139                 }\r
 140                 return;\r
 141         }\r
 142         private final void getNewMatrix(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoubleMatrix34 o_combo)\r
 143         {\r
 144                 double cp0,cp1,cp2,cp3;\r
 145                 NyARPerspectiveProjectionMatrix cp=this._projection_mat_ref;\r
 146 \r
 147                 cp3=cp.m03;\r
 148                 cp0=cp.m00;cp1=cp.m01;cp2=cp.m02;\r
 149                 o_combo.m00=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
 150                 o_combo.m01=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
 151                 o_combo.m02=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
 152                 o_combo.m03=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
 153 \r
 154                 cp0=cp.m10;cp1=cp.m11;cp2=cp.m12;\r
 155                 o_combo.m10=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
 156                 o_combo.m11=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
 157                 o_combo.m12=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
 158                 o_combo.m13=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
 159 \r
 160                 cp0=cp.m20;cp1=cp.m21;cp2=cp.m22;\r
 161                 o_combo.m20=cp0 * i_rot.m00 + cp1 * i_rot.m10 + cp2 * i_rot.m20;\r
 162                 o_combo.m21=cp0 * i_rot.m01 + cp1 * i_rot.m11 + cp2 * i_rot.m21;\r
 163                 o_combo.m22=cp0 * i_rot.m02 + cp1 * i_rot.m12 + cp2 * i_rot.m22;\r
 164                 o_combo.m23=cp0 * i_trans.x + cp1 * i_trans.y + cp2 * i_trans.z +cp3;\r
 165                 return;\r
 166         }\r
 167         private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();\r
 168         private final NyARDoubleMatrix34 __modifyMatrix_combo=new NyARDoubleMatrix34();\r
 169         private final NyARDoubleMatrix33[] __modifyMatrix_next_rot_matrix=NyARDoubleMatrix33.createArray(27); \r
 170         public final double modifyMatrix(NyARRotMatrix io_rot, NyARDoublePoint3d trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException\r
 171         {\r
 172                 final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
 173                 final NyARDoubleMatrix34 combo=this.__modifyMatrix_combo;\r
 174                 final NyARDoubleMatrix33[] next_rot_matrix=this.__modifyMatrix_next_rot_matrix;\r
 175                 double factor;\r
 176                 double hx, hy, h, x, y;\r
 177                 double err, minerr = 0;\r
 178                 int i,i2;\r
 179                 int best_idx=0;\r
 180                 angle.copyFrom(io_rot.refAngle());// arGetAngle( rot, &a, &b, &c );\r
 181                 factor = 10.0 * Math.PI / 180.0;\r
 182                 for (int j = 0; j < 10; j++){\r
 183                         minerr = 1000000000.0;\r
 184                         //評価用の角度マップ作成\r
 185                         createRotationMap(angle,factor,next_rot_matrix);\r
 186                         //評価して一番宜しいIDを保存\r
 187                         best_idx=(1+1*3+1*9);\r
 188                         for(i2=0;i2<27;i2++){\r
 189                                 this.getNewMatrix(next_rot_matrix[i2],trans,combo);\r
 190                                 err = 0.0;\r
 191                                 for (i = 0; i < 4; i++) {\r
 192                                         hx =  combo.m00 * i_vertex3d[i].x + combo.m01 * i_vertex3d[i].y + combo.m02 * i_vertex3d[i].z + combo.m03;\r
 193                                         hy = combo.m10 * i_vertex3d[i].x + combo.m11 *i_vertex3d[i].y + combo.m12 * i_vertex3d[i].z + combo.m13;\r
 194                                         h = combo.m20 * i_vertex3d[i].x + combo.m21 * i_vertex3d[i].y + combo.m22 * i_vertex3d[i].z + combo.m23;\r
 195                                         x = i_vertex2d[i].x-(hx / h);\r
 196                                         y = i_vertex2d[i].y-(hy / h);\r
 197                                         err += x*x+y*y;\r
 198                                 }\r
 199                                 if (err < minerr){\r
 200                                         minerr = err;\r
 201                                         best_idx=i2;\r
 202                                 }\r
 203                                 \r
 204                         }\r
 205                         if (best_idx==(1+1*3+1*9)){\r
 206                                 factor *= 0.5;                  \r
 207                         }else{\r
 208                                 angle.z+=factor*(best_idx%3-1);\r
 209                                 angle.y+=factor*((best_idx/3)%3-1);\r
 210                                 angle.x+=factor*((best_idx/9)%3-1);                             \r
 211                         }\r
 212                 }\r
 213                 io_rot.setAngle(angle.x,angle.y,angle.z);\r
 214                 /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
 215                 return minerr / 4;\r
 216         }\r
 217 \r
 218 \r
 219 \r
 220 \r
 221 }\r