src/jp/nyatla/nyartoolkit/base/Param.java

   1 package jp.nyatla.nyartoolkit.base;\r
   2 \r
   3 import jp.nyatla.nyartoolkit.NyARException;\r
   4 import jp.nyatla.nyartoolkit.core.NyARMat;\r
   5 \r
   6 \r
   7 \r
   8 \r
   9 public class Param {\r
  10     private static final int AR_PARAM_NMIN=6;//#define   AR_PARAM_NMIN         6\r
  11     private static final int AR_PARAM_NMAX=1000;//#define   AR_PARAM_NMAX      1000\r
  12     private static final double AR_PARAM_C34=100.0;//#define   AR_PARAM_C34        100.0\r
  13 \r
  14     /*\r
  15         typedef struct {\r
  16             int      xsize, ysize;\r
  17             double   matL[3][4];\r
  18             double   matR[3][4];\r
  19             double   matL2R[3][4];\r
  20             double   dist_factorL[4];\r
  21             double   dist_factorR[4];\r
  22         } ARSParam;*/\r
  23 /*      static class ARSParam{\r
  24             int      xsize, ysize;\r
  25             Double2dArray   matL=new Double2dArray(3,4);\r
  26             Double2dArray   matR=new Double2dArray(3,4);\r
  27             Double2dArray   matL2R=new Double2dArray(3,4);\r
  28             double[]   dist_factorL=new double[4];\r
  29             double[]   dist_factorR=new double[4];\r
  30         }*/\r
  31         private static final int arParamGet_AR_PARAM_CDMIN = 12;\r
  32         /*int  arParamGet( double global[][3], double screen[][2], int data_num,double mat[3][4] );*/\r
  33         public static int arParamGet( double global[][], double[][] screen, int num,double[][] mat) throws NyARException\r
  34         {\r
  35                 NyARMat     mat_a, mat_at, mat_r;//ARMat     *mat_a, *mat_at, *mat_r, mat_cpara;\r
  36                 NyARMat     mat_wm1,mat_wm2;//ARMat     *mat_wm1, *mat_wm2;\r
  37                 int i;\r
  38                 \r
  39                 if(num < AR_PARAM_NMIN){\r
  40                         return( -1 );\r
  41                 }\r
  42                 if(num > AR_PARAM_NMAX){\r
  43                         return( -1 );\r
  44                 }\r
  45                 NyARException.trap("未チェックのパス");{\r
  46                 mat_a = new NyARMat(2*num,arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );\r
  47                 mat_at =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );\r
  48                 mat_r = new NyARMat(2*num,1);//mat_r = arMatrixAlloc( 2*num, 1 );\r
  49                 mat_wm1 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );\r
  50                 mat_wm2 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );\r
  51                 }\r
  52 /*\r
  53                 mat_a = Matrix.arMatrixAlloc( 2*num, arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );\r
  54                 if( mat_a == null){\r
  55                    return -1;\r
  56                 }\r
  57                 mat_at =Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );\r
  58                 if(mat_at == null){\r
  59                    return -1;\r
  60                 }\r
  61                 mat_r = Matrix.arMatrixAlloc( 2*num, 1 );//mat_r = arMatrixAlloc( 2*num, 1 );\r
  62                 if(mat_r ==null){\r
  63                    return -1;\r
  64                 }\r
  65                 mat_wm1 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );\r
  66                 if( mat_wm1 == null) {\r
  67                    return -1;\r
  68                 }\r
  69                 mat_wm2 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );\r
  70                 if( mat_wm2 == null ) {\r
  71                         return -1;\r
  72                 }\r
  73 */\r
  74                 /* Initializing array */\r
  75                 mat_a.zeroClear();//Javaではゼロクリアされるので不要\r
  76 //              pa1=mat_a.getPointer();//pa1 = mat_a->m;\r
  77 //              for(i = 0; i < 2 * num * (arParamGet_AR_PARAM_CDMIN-1); i++){\r
  78 //                      {//*pa1++ = 0.0;\r
  79 //                      pa1.set(0.0);\r
  80 //                      pa1.incPtr();}\r
  81 //              }\r
  82                 double[][] pa1  =mat_a.getArray();\r
  83                 double[][] pa2 =mat_a.getArray();\r
  84                 /* Calculate A,R matrix */\r
  85                 double[][] pr=mat_r.getArray();\r
  86                 int pr_ptr_col=0;\r
  87                 for(i = 0; i < num; i++) {//for(i = 0, pr = mat_r->m; i < num; i++) {\r
  88                     int pa1_ptr_row     =2*i;  //pa1 = &(mat_a->m[ (2*i)   * (AR_PARAM_CDMIN-1)\r
  89                     int pa2_ptr_row     =2*i+1;//pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]);\r
  90 \r
  91 \r
  92 //                  pa1=mat_a.getPointer((2*i)*(arParamGet_AR_PARAM_CDMIN-1));    ]);\r
  93 //                  pa2=mat_a.getPointer((2*i+1)*(arParamGet_AR_PARAM_CDMIN-1) + 4);\r
  94                     //*pa1++ = global[i][0]; *pa1++ = global[i][1];\r
  95                     pa1[pa1_ptr_row][ 0]=global[i][0];\r
  96                     pa1[pa1_ptr_row][ 1]=global[i][1];\r
  97                     //*pa1++ = global[i][2]; *pa1++  = 1.0;\r
  98                     pa1[pa1_ptr_row][ 2]=global[i][2];\r
  99                     pa1[pa1_ptr_row][ 3]=1.0;\r
 100                     //*pa2++ = global[i][0]; *pa2++ = global[i][1];\r
 101                     pa2[pa2_ptr_row][ 4]=global[i][0];\r
 102                     pa2[pa2_ptr_row][ 5]=global[i][1];\r
 103                     //*pa2++ = global[i][2]; *pa2++ = 1.0;\r
 104                     pa2[pa2_ptr_row][ 6]=global[i][2];\r
 105                     pa2[pa2_ptr_row][ 7]=1.0;\r
 106                     //pa1 += 4;\r
 107                     //*pa1++ = -global[i][0] * screen[i][0];\r
 108                     pa1[pa1_ptr_row][ 8]=-global[i][0] * screen[i][0];\r
 109                     //*pa1++ = -global[i][1] * screen[i][0];\r
 110                     pa1[pa1_ptr_row][ 9]=-global[i][1] * screen[i][0];\r
 111                     //*pa1   = -global[i][2] * screen[i][0];\r
 112                     pa1[pa1_ptr_row][10]=-global[i][2]* screen[i][0];\r
 113                     //*pa2++ = -global[i][0] * screen[i][1];\r
 114                     pa2[pa2_ptr_row][ 8]=-global[i][0] * screen[i][1];\r
 115                     //*pa2++ = -global[i][1] * screen[i][1];\r
 116                     pa2[pa2_ptr_row][ 9]=-global[i][1] * screen[i][1];\r
 117                     //*pa2   = -global[i][2] * screen[i][1];\r
 118                     pa2[pa2_ptr_row][10]=-global[i][2] * screen[i][1];\r
 119                     //*pr++  = screen[i][0] * AR_PARAM_C34;\r
 120                     pr[0][pr_ptr_col]=screen[i][0] * AR_PARAM_C34;pr_ptr_col++;\r
 121                     //*pr++  = screen[i][1] * AR_PARAM_C34;\r
 122                     pr[0][pr_ptr_col]=screen[i][1] * AR_PARAM_C34;pr_ptr_col++;\r
 123                 }\r
 124 \r
 125                 NyARException.trap("未チェックのパス");\r
 126                 NyARMat.matrixTrans(mat_at, mat_a );//if( arMatrixTrans( mat_at, mat_a ) < 0 ){\r
 127 \r
 128                 NyARException.trap("未チェックのパス");\r
 129                 NyARMat.matrixMul( mat_wm1, mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) {\r
 130                 NyARException.trap("未チェックのパス");\r
 131                 NyARMat.matrixSelfInv(mat_wm1 );//if( arMatrixSelfInv( mat_wm1 ) < 0 ) {\r
 132 \r
 133                 NyARException.trap("未チェックのパス");\r
 134                 NyARMat.matrixMul( mat_wm2, mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) {\r
 135 \r
 136                 //mat_cpara.row = AR_PARAM_CDMIN-1;//mat_cpara.row = AR_PARAM_CDMIN-1;\r
 137                 //mat_cpara.clm = 1;\r
 138                 //mat_cpara.m = &(mat[0][0]);\r
 139                 /*1次元行列から3x4行列に転写。高負荷なところじゃないから地道に転写でOK*/\r
 140                 NyARMat mat_cpara=new NyARMat(arParamGet_AR_PARAM_CDMIN-1,1);\r
 141                 double[][] mat_cpara_array=mat_cpara.getArray();\r
 142                 NyARException.trap("未チェックのパス");\r
 143                 NyARMat.matrixMul(mat_cpara, mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) {\r
 144 \r
 145                 for(int i2=0;i<3;i++){\r
 146                     for(int i3=0;i3<4;i3++){\r
 147                         mat[i2][i3]=mat_cpara_array[i2*4+i3][0];\r
 148                     }\r
 149                 }\r
 150                 //ARMat.wrap(mat.array(),arParamGet_AR_PARAM_CDMIN-1,1);}\r
 151 \r
 152                 mat[2][3]=AR_PARAM_C34;//mat[2][3] = AR_PARAM_C34;\r
 153 \r
 154                 return 0;\r
 155         }\r
 156 \r
 157 /*      //int    arsParamChangeSize( ARSParam *source, int xsize, int ysize, ARSParam *newparam );\r
 158         public static int arsParamChangeSize( ARSParam source, int xsize, int ysize, ARSParam newparam)\r
 159         {\r
 160             double  scale;\r
 161 \r
 162             newparam.xsize=xsize;//newparam->xsize = xsize;\r
 163             newparam.ysize=ysize;//newparam->ysize = ysize;\r
 164 \r
 165             scale=(double)xsize/ (double)(source.xsize);//scale = (double)xsize / (double)(source->xsize);\r
 166             for(int i = 0; i < 4; i++ ){\r
 167                 newparam.matL.set(0,i,source.matL.get(0,i)*scale);//newparam->matL[0][i] = source->matL[0][i] * scale;\r
 168                 newparam.matL.set(1,i,source.matL.get(1,i)*scale);//newparam->matL[1][i] = source->matL[1][i] * scale;\r
 169                 newparam.matL.set(2,i,source.matL.get(2,i));//newparam->matL[2][i] = source->matL[2][i];\r
 170             }\r
 171             for(int i = 0; i < 4; i++ ) {\r
 172                 newparam.matR.set(0,i,source.matR.get(0,i)*scale);//newparam->matR[0][i] = source->matR[0][i] * scale;\r
 173                 newparam.matR.set(1,i,source.matR.get(1,i)*scale);//newparam->matR[1][i] = source->matR[1][i] * scale;\r
 174                 newparam.matR.set(2,i,source.matR.get(2,i));//newparam->matR[2][i] = source->matR[2][i];\r
 175             }\r
 176             for(int i = 0; i < 4; i++ ) {\r
 177                 newparam.matL2R.set(0,i,source.matL2R.get(0,i));//newparam->matL2R[0][i] = source->matL2R[0][i];\r
 178                 newparam.matL2R.set(1,i,source.matL2R.get(1,i));//newparam->matL2R[1][i] = source->matL2R[1][i];\r
 179                 newparam.matL2R.set(2,i,source.matL2R.get(2,i));//newparam->matL2R[2][i] = source->matL2R[2][i];\r
 180             }\r
 181 \r
 182             newparam.dist_factorL[0] = source.dist_factorL[0] * scale;//newparam->dist_factorL[0] = source->dist_factorL[0] * scale;\r
 183             newparam.dist_factorL[1] = source.dist_factorL[1] * scale;//newparam->dist_factorL[1] = source->dist_factorL[1] * scale;\r
 184             newparam.dist_factorL[2] = source.dist_factorL[2] / (scale*scale);//newparam->dist_factorL[2] = source->dist_factorL[2] / (scale*scale);\r
 185             newparam.dist_factorL[3] = source.dist_factorL[3];//newparam->dist_factorL[3] = source->dist_factorL[3];\r
 186 \r
 187             newparam.dist_factorR[0] = source.dist_factorR[0] * scale;//newparam->dist_factorR[0] = source->dist_factorR[0] * scale;\r
 188             newparam.dist_factorR[1] = source.dist_factorR[1] * scale;//newparam->dist_factorR[1] = source->dist_factorR[1] * scale;\r
 189             newparam.dist_factorR[2] = source.dist_factorR[2] / (scale*scale);//newparam->dist_factorR[2] = source->dist_factorR[2] / (scale*scale);\r
 190             newparam.dist_factorR[3] = source.dist_factorR[3];//newparam->dist_factorR[3] = source->dist_factorR[3];\r
 191 \r
 192             return 0;\r
 193         }*/\r
 194 /*      //int arsParamSave( char *filename, ARSParam *sparam );\r
 195         public static int arsParamSave(String filename, ARSParam sparam) throws Exception\r
 196         {   \r
 197             //int      xsize, ysize;\r
 198             //Double2dArray   matL=new Double2dArray(3,4);\r
 199             //Double2dArray   matR=new Double2dArray(3,4);\r
 200             //Double2dArray   matL2R=new Double2dArray(3,4);\r
 201             //double   dist_factorL[]=new double[4];\r
 202             //double   dist_factorR[]=new double[4];\r
 203 \r
 204                 byte[] buf=new byte[(4+4+(3*4*8)*3+(4*8)*2)];\r
 205                 \r
 206                 //バッファをラップ\r
 207                 ByteBuffer bb = ByteBuffer.wrap(buf);\r
 208                 bb.order(ByteOrder.BIG_ENDIAN);\r
 209 \r
 210                 //書き込み\r
 211                 bb.putInt(sparam.xsize);\r
 212                 bb.putInt(sparam.ysize);\r
 213                 for(int i=0;i<3;i++){\r
 214                         for(int i2=0;i2<4;i2++){\r
 215                                 bb.putDouble(sparam.matL.get(i, i2));\r
 216                         }\r
 217                 }\r
 218                 for(int i=0;i<3;i++){\r
 219                         for(int i2=0;i2<4;i2++){\r
 220                                 bb.putDouble(sparam.matR.get(i, i2));\r
 221                         }\r
 222                 }\r
 223                 for(int i=0;i<3;i++){\r
 224                         for(int i2=0;i2<4;i2++){\r
 225                                 bb.putDouble(sparam.matL2R.get(i, i2));\r
 226                         }\r
 227                 }\r
 228                 for(int i=0;i<4;i++){\r
 229                         bb.putDouble(sparam.dist_factorL[i]);\r
 230                 }\r
 231                 for(int i=0;i<4;i++){\r
 232                         bb.putDouble(sparam.dist_factorR[i]);\r
 233                 }\r
 234                 //ファイルに保存\r
 235                 FileOutputStream fs=new FileOutputStream(filename);\r
 236                 fs.write(buf);\r
 237                 fs.close();\r
 238                 \r
 239                 return 0;\r
 240         }*/\r
 241 /*      //int    arsParamLoad( char *filename, ARSParam *sparam );\r
 242         public static int arsParamLoad(String filename, ARSParam sparam ) throws Exception\r
 243         {\r
 244                 //ファイルを読んどく\r
 245                 FileInputStream fs=new FileInputStream(filename);\r
 246                 File f=new File(filename);\r
 247                 int file_size=(int)f.length();\r
 248                 byte[] buf=new byte[file_size];\r
 249                 fs.read(buf);\r
 250                 fs.close();\r
 251                 \r
 252                 //バッファを加工\r
 253                 ByteBuffer bb = ByteBuffer.wrap(buf);\r
 254                 bb.order(ByteOrder.BIG_ENDIAN);\r
 255                 \r
 256                 //固定回数パースして配列に格納            \r
 257                 sparam.xsize=bb.getInt();\r
 258                 sparam.ysize=bb.getInt();\r
 259                 for(int i=0;i<3;i++){\r
 260                         for(int i2=0;i2<4;i2++){\r
 261                                 sparam.matL.set(i,i2,bb.getDouble());\r
 262                         }\r
 263                 }\r
 264                 for(int i=0;i<3;i++){\r
 265                         for(int i2=0;i2<4;i2++){\r
 266                                 sparam.matR.set(i,i2,bb.getDouble());\r
 267                         }\r
 268                 }\r
 269                 for(int i=0;i<3;i++){\r
 270                         for(int i2=0;i2<4;i2++){\r
 271                                 sparam.matL2R.set(i,i2,bb.getDouble());\r
 272                         }\r
 273                 }\r
 274                 for(int i=0;i<3;i++){\r
 275                         sparam.dist_factorL[i]=bb.getDouble();\r
 276                 }\r
 277                 for(int i=0;i<3;i++){\r
 278                         sparam.dist_factorR[i]=bb.getDouble();\r
 279                 }\r
 280                 return 0;\r
 281         }*/\r
 282 /*\r
 283         //      int    arsParamGetMat( double matL[3][4], double matR[3][4],double cparaL[3][4], double cparaR[3][4], double matL2R[3][4] );\r
 284         public static int arsParamGetMat(double[][] matL, double[][] matR,double[][] cparaL, double[][] cparaR, double[][] matL2R) throws JartkException\r
 285         {\r
 286             ARMat t1,t2,t3;//ARMat    *t1, *t2, *t3;\r
 287             //double   transL[3][4], transR[3][4];\r
 288             Double2dArray transL=new Double2dArray(3,4);\r
 289             Double2dArray transR=new Double2dArray(3,4);\r
 290         \r
 291             arParamDecompMat( matL,cparaL,transL);\r
 292             arParamDecompMat( matR,cparaR,transR);\r
 293             JartkException.trap("未チェックパス");{\r
 294             t1=new ARMat(4,4);//t1 = arMatrixAlloc( 4, 4 );\r
 295             t2=new ARMat(4,4);//t2 = arMatrixAlloc( 4, 4 );\r
 296             }\r
 297             double[][] t1_array=t1.getArray();\r
 298             double[][] t2_array=t2.getArray();\r
 299             for(int j = 0; j < 3; j++ ){\r
 300                 for(int i = 0; i < 4; i++ ) {\r
 301                     JartkException.trap("未チェックパス");{\r
 302                     t1_array[j][i]=transL.get(j,i);//t1->m[j*4+i] = transL[j][i];\r
 303                     t2_array[j][i]=transL.get(j,i);//t2->m[j*4+i] = transR[j][i];\r
 304                     }\r
 305                 }\r
 306             }\r
 307             JartkException.trap("未チェックパス");{\r
 308             t1_array[3][0]=t1_array[3][1]=t1_array[3][2]=0.0;//t1->m[12] = t1->m[13] = t1->m[14] = 0.0;\r
 309             t1_array[3][3]=1.0;//t1->m[15] = 1.0;\r
 310             t2_array[3][0]=t2_array[3][1]=t2_array[3][2]=0.0;//t2->m[12] = t2->m[13] = t2->m[14] = 0.0;\r
 311             t2_array[3][3]=1.0;//t2->m[15] = 1.0;\r
 312             }\r
 313             JartkException.trap("未チェックのパス");\r
 314             ARMat.matrixSelfInv(t1);//if( arMatrixSelfInv(t1) != 0 ) {\r
 315 \r
 316             JartkException.trap("未チェックのパス");        \r
 317             t3 =ARMat.matrixAllocMul(t2, t1);//t3 = arMatrixAllocMul(t2, t1);\r
 318             double[][] t3_array=t3.getArray();\r
 319             if(t3==null){\r
 320                 return -1;\r
 321             }\r
 322         \r
 323             for(int j = 0; j < 3; j++ ) {\r
 324                for(int i = 0; i < 4; i++ ) {\r
 325                    JartkException.trap("未チェックパス");\r
 326                    matL2R[j][i]=t3_array[j][i];//matL2R[j][i] = t3->m[j*4+i];\r
 327                 }\r
 328             }\r
 329             return 0;\r
 330         }\r
 331 */      //int arsParamDisp( ARSParam *sparam )\r
 332 /*      public static int arsParamDisp( ARSParam sparam)\r
 333         {\r
 334                 System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
 335                 System.out.println("SIZE = "+sparam.xsize+", "+sparam.ysize);// printf("SIZE = %d, %d\n", sparam->xsize, sparam->ysize);\r
 336                 System.out.println("-- Left --");//printf("-- Left --\n");\r
 337                 System.out.println("Distotion factor = "+sparam.dist_factorL[0]+" "+sparam.dist_factorL[1]+" "+sparam.dist_factorL[2]+" "+sparam.dist_factorL[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorL[0],sparam->dist_factorL[1], sparam->dist_factorL[2], sparam->dist_factorL[3] );\r
 338             for(int j = 0; j < 3; j++ ) {\r
 339                 for(int i = 0; i < 4; i++ ){\r
 340                         System.out.print(sparam.matL.get(j,i)+" ");//printf("%7.5f ", sparam->matL[j][i]);\r
 341                 }\r
 342                 System.out.println();//printf("\n");\r
 343             }\r
 344         \r
 345             System.out.println("-- Right --");//printf("-- Right --\n");\r
 346             System.out.println("Distotion factor = "+sparam.dist_factorR[0]+" "+sparam.dist_factorR[1]+" "+sparam.dist_factorR[2]+" "+sparam.dist_factorR[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorR[0],sparam->dist_factorR[1], sparam->dist_factorR[2], sparam->dist_factorR[3]);\r
 347             for(int j = 0; j < 3; j++ ){\r
 348                 for(int i = 0; i < 4; i++ ){\r
 349                         System.out.println(sparam.matR.get(j,i)+" ");//printf("%7.5f ", sparam->matR[j][i]);\r
 350                 }\r
 351                 System.out.println();//printf("\n");\r
 352             }\r
 353         \r
 354             System.out.println("-- Left => Right --");//printf("-- Left => Right --\n");\r
 355             for(int j = 0; j < 3; j++ ) {\r
 356                 for(int i = 0; i < 4; i++ ){\r
 357                         //printf("%7.5f ", sparam->matL2R[j][i]);\r
 358                 }\r
 359                 System.out.println();//printf("\n");\r
 360             }\r
 361         \r
 362             System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
 363         \r
 364             return 0;           \r
 365         }*/\r
 366 }\r