src/jp/nyatla/nyartoolkit/core/NyARParam.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;\r
  33 \r
  34 \r
  35 import java.io.*;\r
  36 import java.nio.*;\r
  37 \r
  38 import jp.nyatla.nyartoolkit.NyARException;\r
  39 import jp.nyatla.util.DoubleValue;\r
  40 \r
  41 /*typedef struct {\r
  42     int      xsize, ysize;\r
  43     double   mat[3][4];\r
  44     double   dist_factor[4];\r
  45 } ARParam;*/\r
  46 public class NyARParam{\r
  47     private static final int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8);\r
  48     private static final int PD_LOOP = 3;\r
  49     protected int               xsize, ysize;\r
  50     private double[][]          mat=new double[3][4];//Double2dArray    mat=new Double2dArray(3,4);\r
  51     private double[]            dist_factor=new double[4];\r
  52     public int getX()\r
  53     {\r
  54         return xsize;\r
  55     }\r
  56     public int getY()\r
  57     {\r
  58         return ysize;\r
  59     }\r
  60     public double[] getDistFactor()\r
  61     {\r
  62         return dist_factor;\r
  63     }\r
  64     public double[][] getMat()\r
  65     {\r
  66         return mat;\r
  67     }\r
  68     /**\r
  69      * ARToolKit標準ファイルから1個目の設定をロードする。\r
  70      * @param i_filename\r
  71      * @throws NyARException\r
  72      */\r
  73     public void loadFromARFile(String i_filename) throws NyARException\r
  74     {\r
  75         try {\r
  76             loadFromARFile(new FileInputStream(i_filename));\r
  77         } catch (Exception e) {\r
  78             throw new NyARException(e);\r
  79         }\r
  80     }\r
  81     public void loadFromARFile(InputStream i_stream) throws NyARException\r
  82     {\r
  83         try {\r
  84             NyARParam new_inst[] = arParamLoad(i_stream, 1);\r
  85             i_stream.close();\r
  86             xsize = new_inst[0].xsize;\r
  87             ysize = new_inst[0].ysize;\r
  88             mat = new_inst[0].mat;\r
  89             dist_factor = new_inst[0].dist_factor;\r
  90         } catch (Exception e) {\r
  91             throw new NyARException(e);\r
  92         }\r
  93     }\r
  94     /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/\r
  95     private static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )\r
  96     {\r
  97         return( a1 * b1 + a2 * b2 + a3 * b3 );\r
  98     }\r
  99     /*  static double norm( double a, double b, double c )*/\r
 100     private static double norm( double a, double b, double c )\r
 101     {\r
 102         return Math.sqrt( a*a + b*b + c*c );\r
 103     }\r
 104     /**\r
 105      * int  arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] );\r
 106      * 関数の置き換え\r
 107      * @param o_cpara\r
 108      * 戻り引数。3x4のマトリクスを指定すること。\r
 109      * @param o_trans\r
 110      * 戻り引数。3x4のマトリクスを指定すること。\r
 111      * @return\r
 112      */\r
 113     public void decompMat(NyARMat o_cpara, NyARMat o_trans)\r
 114     {\r
 115         double[][] source=mat;\r
 116         double[][] Cpara=new double[3][4];//double    Cpara[3][4];\r
 117         double    rem1, rem2, rem3;\r
 118     \r
 119         if(source[2][3]>= 0 ){//if( source[2][3] >= 0 ) {\r
 120             for(int r = 0; r < 3; r++ ){\r
 121                 for(int c = 0; c < 4; c++ ){\r
 122                     Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c];\r
 123                 }\r
 124             }\r
 125         }else {\r
 126             for(int r = 0; r < 3; r++ ){\r
 127                 for(int c = 0; c < 4; c++ ){\r
 128                     Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);\r
 129                 }\r
 130             }\r
 131         }\r
 132     \r
 133         double[][] cpara=o_cpara.getArray();\r
 134         double[][] trans=o_trans.getArray();\r
 135         for(int r = 0; r < 3; r++ ){\r
 136             for(int c = 0; c < 4; c++ ){\r
 137                 cpara[r][c]=0.0;//cpara[r][c] = 0.0;\r
 138             }\r
 139         }\r
 140         cpara[2][2]=norm(Cpara[2][0],Cpara[2][1], Cpara[2][2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );\r
 141         trans[2][0]=Cpara[2][0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2];\r
 142         trans[2][1]=Cpara[2][1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2];\r
 143         trans[2][2]=Cpara[2][2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2];\r
 144         trans[2][3]=Cpara[2][3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2];\r
 145         \r
 146         cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] );\r
 147         rem1=Cpara[1][0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];\r
 148         rem2=Cpara[1][1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];\r
 149         rem3=Cpara[1][2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];\r
 150         cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 );\r
 151         trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1];\r
 152         trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1];\r
 153         trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1];\r
 154     \r
 155         cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );\r
 156         cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );\r
 157         rem1=Cpara[0][0]- cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];//rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];\r
 158         rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];//rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];\r
 159         rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];//rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];\r
 160         cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 );\r
 161         trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0];\r
 162         trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0];\r
 163         trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0];\r
 164     \r
 165         trans[1][3]=(Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];//trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];\r
 166         trans[0][3]=(Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];//trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]- cpara[0][2]*trans[2][3]) / cpara[0][0];\r
 167     \r
 168         for(int  r = 0; r < 3; r++ ){\r
 169             for(int c = 0; c < 3; c++ ){\r
 170                 cpara[r][c]/=cpara[2][2];//cpara[r][c] /= cpara[2][2];\r
 171             }\r
 172         }\r
 173     }\r
 174 \r
 175 \r
 176     /*int    arParamDisp( ARParam *param );*/\r
 177     public int paramDisp()\r
 178     {\r
 179         System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
 180         System.out.print("SIZE = "+xsize+", "+ysize);//printf("SIZE = %d, %d\n", param->xsize, param->ysize);\r
 181         System.out.println("Distortion factor = "+dist_factor[0]+" "+dist_factor[1]+" "+dist_factor[2]+" "+dist_factor[3]);//printf("Distortion factor = %f %f %f %f\n", param->dist_factor[0],param->dist_factor[1], param->dist_factor[2], param->dist_factor[3] );\r
 182         for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) {\r
 183             for(int i = 0; i < 4; i++ ){\r
 184                 System.out.print(mat[j][i]+" ");//printf("%7.5f ", param->mat[j][i]);\r
 185             }\r
 186             System.out.println();//    printf("\n");\r
 187         }//}\r
 188         System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
 189         return 0;\r
 190     }\r
 191 //    /*int  arParamDecomp( ARParam *source, ARParam *icpara, double trans[3][4] );*/\r
 192 //    private static int arParamDecomp( NyARParam source, NyARParam icpara, double[][] trans)\r
 193 //    {\r
 194 //        icpara.xsize          = source.xsize;//icpara->xsize          = source->xsize;\r
 195 //        icpara.ysize          = source.ysize;//icpara->ysize          = source->ysize;\r
 196 //        icpara.dist_factor[0] = source.dist_factor[0];//icpara->dist_factor[0] = source->dist_factor[0];\r
 197 //        icpara.dist_factor[1] = source.dist_factor[1];// icpara->dist_factor[1] = source->dist_factor[1];\r
 198 //        icpara.dist_factor[2] = source.dist_factor[2];//icpara->dist_factor[2] = source->dist_factor[2];\r
 199 //        icpara.dist_factor[3] = source.dist_factor[3];//icpara->dist_factor[3] = source->dist_factor[3];\r
 200 //        return arParamDecompMat(source.mat, icpara.mat, trans );\r
 201 //    }\r
 202     /**\r
 203      * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam );\r
 204      * 関数の代替関数\r
 205      * サイズプロパティをi_xsize,i_ysizeに変更します。\r
 206      * @param xsize\r
 207      * @param ysize\r
 208      * @param newparam\r
 209      * @return\r
 210      * \r
 211      */\r
 212     public void changeSize(int i_xsize, int i_ysize)\r
 213     {\r
 214         double  scale;    \r
 215         scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize);\r
 216     \r
 217         for(int i = 0; i < 4; i++ ) {\r
 218             mat[0][i]=mat[0][i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale;\r
 219             mat[1][i]=mat[1][i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale;\r
 220             mat[2][i]=mat[2][i];//newparam->mat[2][i] = source->mat[2][i];\r
 221         }\r
 222     \r
 223         dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale;\r
 224         dist_factor[1] = dist_factor[1] * scale;//newparam->dist_factor[1] = source->dist_factor[1] * scale;\r
 225         dist_factor[2] = dist_factor[2] / (scale*scale);//newparam->dist_factor[2] = source->dist_factor[2] / (scale*scale);\r
 226         dist_factor[3] = dist_factor[3];//newparam->dist_factor[3] = source->dist_factor[3];\r
 227     \r
 228         xsize = i_xsize;//newparam->xsize = xsize;\r
 229         ysize = i_ysize;//newparam->ysize = ysize;\r
 230     }\r
 231     /**\r
 232      * int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy,double *ox, double *oy )\r
 233      * 関数の代替関数\r
 234      * @param ix\r
 235      * @param iy\r
 236      * @param ox\r
 237      * @param oy\r
 238      */\r
 239     public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy)\r
 240     {\r
 241         double    x, y, d;\r
 242         \r
 243         x = (ix - dist_factor[0]) * dist_factor[3];\r
 244         y = (iy - dist_factor[1]) * dist_factor[3];\r
 245         if( x == 0.0 && y == 0.0 ) {\r
 246             ox.value=dist_factor[0];\r
 247             oy.value=dist_factor[1];\r
 248         }else{\r
 249             d = 1.0 - dist_factor[2]/100000000.0 * (x*x+y*y);\r
 250             ox.value=x * d + dist_factor[0];\r
 251             oy.value=y * d + dist_factor[1];\r
 252         }\r
 253     }\r
 254     /*int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy );*/\r
 255     public int observ2Ideal(double ox,double oy,DoubleValue ix,DoubleValue iy)\r
 256     {\r
 257         double  z02, z0, p, q, z, px, py,opttmp_1;\r
 258         \r
 259         px = ox - dist_factor[0];\r
 260         py = oy - dist_factor[1];\r
 261         p = dist_factor[2]/100000000.0;\r
 262         z02 = px*px+py*py;\r
 263         q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
 264         \r
 265         for(int i = 1; ; i++ ) {\r
 266             if( z0 != 0.0 ) {\r
 267                 //Optimize opttmp_1\r
 268                 opttmp_1=p*z02;\r
 269                 z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1);\r
 270                 //Optimize opttmp_1       \r
 271                 opttmp_1=z / z0;\r
 272                 px = px * opttmp_1;\r
 273                 py = py * opttmp_1;\r
 274             }else {\r
 275                 px = 0.0;\r
 276                 py = 0.0;\r
 277                 break;\r
 278             }\r
 279             if( i == PD_LOOP ){\r
 280                 break;\r
 281             }\r
 282             z02 = px*px+ py*py;\r
 283             z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py);\r
 284         }\r
 285         \r
 286         ix.value=px / dist_factor[3] + dist_factor[0];\r
 287         iy.value=py / dist_factor[3] + dist_factor[1];\r
 288         return 0;\r
 289     }\r
 290     /**\r
 291      * int    arParamLoad( const char *filename, int num, ARParam *param, ...);\r
 292      * i_streamの入力ストリームからi_num個の設定を読み込み、パラメタを配列にして返します。\r
 293      * @param filename\r
 294      * @param num\r
 295      * @param param\r
 296      * @return\r
 297      *  設定を格納した配列を返します。\r
 298      * @throws Exception\r
 299      * i_num個の設定が読み出せない場合、JartkExceptionを発生します。\r
 300      */\r
 301     private static NyARParam[] arParamLoad(InputStream i_stream, int i_num) throws NyARException\r
 302     {\r
 303         try{\r
 304             int read_size=SIZE_OF_PARAM_SET*i_num;\r
 305             byte[] buf=new byte[read_size];\r
 306             i_stream.read(buf);\r
 307             //返却配列を確保\r
 308             NyARParam[] result=new NyARParam[i_num];\r
 309             \r
 310             //バッファを加工\r
 311             ByteBuffer bb = ByteBuffer.wrap(buf);\r
 312             bb.order(ByteOrder.BIG_ENDIAN);\r
 313     \r
 314             //固定回数パースして配列に格納\r
 315             for(int i=0;i<i_num;i++){\r
 316                 NyARParam new_param=new NyARParam();;\r
 317                 new_param.xsize=bb.getInt();\r
 318                 new_param.ysize=bb.getInt();\r
 319                 for(int i2=0;i2<3;i2++){\r
 320                     for(int i3=0;i3<4;i3++){\r
 321                         new_param.mat[i2][i3]=bb.getDouble();\r
 322                     }\r
 323                 }\r
 324                 for(int i2=0;i2<4;i2++){\r
 325                     new_param.dist_factor[i2]=bb.getDouble();\r
 326                 }\r
 327                 result[i]=new_param;\r
 328             }\r
 329             return result;\r
 330         }catch(Exception e){\r
 331             throw new NyARException(e);\r
 332         }\r
 333     }\r
 334     public static int arParamSave(String filename,int num, NyARParam param[]) throws Exception\r
 335     {\r
 336         NyARException.trap("未チェックの関数");\r
 337         byte buf[]=new byte[SIZE_OF_PARAM_SET*param.length];\r
 338         //バッファをラップ\r
 339         ByteBuffer bb = ByteBuffer.wrap(buf);\r
 340         bb.order(ByteOrder.BIG_ENDIAN);\r
 341 \r
 342         //書き込み\r
 343         for(int i=0;i<param.length;i++){\r
 344             bb.putInt(param[i].xsize);\r
 345             bb.putInt(param[i].ysize);\r
 346             for(int i2=0;i2<3;i2++){\r
 347                 for(int i3=0;i3<4;i3++){\r
 348                     bb.putDouble(param[i].mat[i2][i3]);\r
 349                 }\r
 350             }\r
 351                 for(int i2=0;i2<4;i2++){\r
 352                     bb.putDouble(param[i].dist_factor[i2]);\r
 353                 }\r
 354             }\r
 355         //ファイルに保存\r
 356         FileOutputStream fs=new FileOutputStream(filename);\r
 357         fs.write(buf);\r
 358         fs.close();\r
 359 \r
 360         return 0;\r
 361     }\r
 362 }\r