--- /dev/null
+/* \r
+ * PROJECT: NyARToolkit\r
+ * --------------------------------------------------------------------------------\r
+ * This work is based on the original ARToolKit developed by\r
+ * Hirokazu Kato\r
+ * Mark Billinghurst\r
+ * HITLab, University of Washington, Seattle\r
+ * http://www.hitl.washington.edu/artoolkit/\r
+ *\r
+ * The NyARToolkit is Java version ARToolkit class library.\r
+ * Copyright (C)2008 R.Iizuka\r
+ *\r
+ * This program is free software; you can redistribute it and/or\r
+ * modify it under the terms of the GNU General Public License\r
+ * as published by the Free Software Foundation; either version 2\r
+ * of the License, or (at your option) any later version.\r
+ * \r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ * \r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this framework; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ * \r
+ * For further information please contact.\r
+ * http://nyatla.jp/nyatoolkit/\r
+ * <airmail(at)ebony.plala.or.jp>\r
+ * \r
+ */\r
+package jp.nyatla.nyartoolkit.core;\r
+\r
+\r
+import java.io.*;\r
+import java.nio.*;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.util.DoubleValue;\r
+\r
+/*typedef struct {\r
+ int xsize, ysize;\r
+ double mat[3][4];\r
+ double dist_factor[4];\r
+} ARParam;*/\r
+public class NyARParam{\r
+ private static final int SIZE_OF_PARAM_SET=4+4+(3*4*8)+(4*8);\r
+ private static final int PD_LOOP = 3;\r
+ protected int xsize, ysize;\r
+ private double[] array34=new double[3*4];//Double2dArray mat=new Double2dArray(3,4);\r
+ private double[] dist_factor=new double[4];\r
+ public int getX()\r
+ {\r
+ return xsize;\r
+ }\r
+ public int getY()\r
+ {\r
+ return ysize;\r
+ }\r
+ public double[] getDistFactor()\r
+ {\r
+ return dist_factor;\r
+ }\r
+ /**\r
+ * パラメタを格納した[4x3]配列を返します。\r
+ * @return\r
+ */\r
+ public final double[] get34Array()\r
+ {\r
+ return array34;\r
+ }\r
+ /**\r
+ * ARToolKit標準ファイルから1個目の設定をロードする。\r
+ * @param i_filename\r
+ * @throws NyARException\r
+ */\r
+ public void loadFromARFile(String i_filename) throws NyARException\r
+ {\r
+ try {\r
+ loadFromARFile(new FileInputStream(i_filename));\r
+ } catch (Exception e) {\r
+ throw new NyARException(e);\r
+ }\r
+ }\r
+ public void loadFromARFile(InputStream i_stream) throws NyARException\r
+ {\r
+ try {\r
+ NyARParam new_inst[] = arParamLoad(i_stream, 1);\r
+ i_stream.close();\r
+ xsize = new_inst[0].xsize;\r
+ ysize = new_inst[0].ysize;\r
+ array34 = new_inst[0].array34;\r
+ dist_factor = new_inst[0].dist_factor;\r
+ } catch (Exception e) {\r
+ throw new NyARException(e);\r
+ }\r
+ }\r
+ /*static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )*/\r
+ private final static double dot( double a1, double a2, double a3,double b1, double b2, double b3 )\r
+ {\r
+ return( a1 * b1 + a2 * b2 + a3 * b3 );\r
+ }\r
+ /* static double norm( double a, double b, double c )*/\r
+ private final static double norm( double a, double b, double c )\r
+ {\r
+ return Math.sqrt( a*a + b*b + c*c );\r
+ }\r
+ /**\r
+ * int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] );\r
+ * 関数の置き換え\r
+ * Optimize STEP[754->665]\r
+ * @param o_cpara\r
+ * 戻り引数。3x4のマトリクスを指定すること。\r
+ * @param o_trans\r
+ * 戻り引数。3x4のマトリクスを指定すること。\r
+ * @return\r
+ */\r
+ public void decompMat(NyARMat o_cpara, NyARMat o_trans)\r
+ {\r
+ double[] source=array34;\r
+ double[] Cpara=new double[3*4];//double Cpara[3][4];\r
+ double rem1, rem2, rem3;\r
+ int i;\r
+ if(source[2*4+3]>= 0 ){//if( source[2][3] >= 0 ) {\r
+ //<Optimize>\r
+ //for(int r = 0; r < 3; r++ ){\r
+ // for(int c = 0; c < 4; c++ ){\r
+ // Cpara[r][c]=source[r][c];//Cpara[r][c] = source[r][c];\r
+ // }\r
+ //}\r
+ for(i=0;i<12;i++){\r
+ Cpara[i]=source[i];//Cpara[r][c] = source[r][c];\r
+ }\r
+ //</Optimize>\r
+ }else {\r
+ //<Optimize>\r
+ //for(int r = 0; r < 3; r++ ){\r
+ // for(int c = 0; c < 4; c++ ){\r
+ // Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);\r
+ // }\r
+ //}\r
+ for(i=0;i<12;i++){\r
+ Cpara[i]=source[i];//Cpara[r][c] = source[r][c];\r
+ }\r
+ //</Optimize>\r
+ }\r
+ \r
+ double[][] cpara=o_cpara.getArray();\r
+ double[][] trans=o_trans.getArray();\r
+ for(int r = 0; r < 3; r++ ){\r
+ for(int c = 0; c < 4; c++ ){\r
+ cpara[r][c]=0.0;//cpara[r][c] = 0.0;\r
+ }\r
+ }\r
+ cpara[2][2]=norm(Cpara[2*4+0],Cpara[2*4+1], Cpara[2*4+2]);//cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );\r
+ trans[2][0]=Cpara[2*4+0] / cpara[2][2];//trans[2][0] = Cpara[2][0] / cpara[2][2];\r
+ trans[2][1]=Cpara[2*4+1]/ cpara[2][2];//trans[2][1] = Cpara[2][1] / cpara[2][2];\r
+ trans[2][2]=Cpara[2*4+2]/ cpara[2][2];//trans[2][2] = Cpara[2][2] / cpara[2][2];\r
+ trans[2][3]=Cpara[2*4+3] / cpara[2][2];//trans[2][3] = Cpara[2][3] / cpara[2][2];\r
+ \r
+ cpara[1][2]=dot(trans[2][0], trans[2][1], trans[2][2],Cpara[1*4+0], Cpara[1*4+1], Cpara[1*4+2]);//cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[1][0], Cpara[1][1], Cpara[1][2] );\r
+ rem1=Cpara[1*4+0]- cpara[1][2] * trans[2][0];//rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];\r
+ rem2=Cpara[1*4+1] - cpara[1][2] * trans[2][1];//rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];\r
+ rem3=Cpara[1*4+2] - cpara[1][2] * trans[2][2];//rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];\r
+ cpara[1][1]=norm(rem1, rem2, rem3 );//cpara[1][1] = norm( rem1, rem2, rem3 );\r
+ trans[1][0]= rem1/cpara[1][1];//trans[1][0] = rem1 / cpara[1][1];\r
+ trans[1][1]=rem2/cpara[1][1];//trans[1][1] = rem2 / cpara[1][1];\r
+ trans[1][2]=rem3 / cpara[1][1];//trans[1][2] = rem3 / cpara[1][1];\r
+ \r
+ cpara[0][2]=dot(trans[2][0],trans[2][1], trans[2][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2] );//cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );\r
+ cpara[0][1]=dot(trans[1][0],trans[1][1], trans[1][2],Cpara[0*4+0], Cpara[0*4+1], Cpara[0*4+2]);//cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],Cpara[0][0], Cpara[0][1], Cpara[0][2] );\r
+ rem1=Cpara[0*4+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
+ rem2 = Cpara[0*4+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
+ rem3 = Cpara[0*4+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
+ cpara[0][0]=norm(rem1, rem2, rem3);//cpara[0][0] = norm( rem1, rem2, rem3 );\r
+ trans[0][0]=rem1 / cpara[0][0];//trans[0][0] = rem1 / cpara[0][0];\r
+ trans[0][1]= rem2 / cpara[0][0];//trans[0][1] = rem2 / cpara[0][0];\r
+ trans[0][2]= rem3 / cpara[0][0];//trans[0][2] = rem3 / cpara[0][0];\r
+ \r
+ trans[1][3]=(Cpara[1*4+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
+ trans[0][3]=(Cpara[0*4+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
+ \r
+ for(int r = 0; r < 3; r++ ){\r
+ for(int c = 0; c < 3; c++ ){\r
+ cpara[r][c]/=cpara[2][2];//cpara[r][c] /= cpara[2][2];\r
+ }\r
+ }\r
+ }\r
+\r
+\r
+ /*int arParamDisp( ARParam *param );*/\r
+ public int paramDisp()\r
+ {\r
+ System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
+ System.out.print("SIZE = "+xsize+", "+ysize);//printf("SIZE = %d, %d\n", param->xsize, param->ysize);\r
+ 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
+ for(int j = 0; j < 3; j++ ) {//for(j = 0; j < 3; j++ ) {\r
+ for(int i = 0; i < 4; i++ ){\r
+ System.out.print(array34[j*4+i]+" ");//printf("%7.5f ", param->mat[j][i]);\r
+ }\r
+ System.out.println();// printf("\n");\r
+ }//}\r
+ System.out.println("--------------------------------------");//printf("--------------------------------------\n");\r
+ return 0;\r
+ }\r
+// /*int arParamDecomp( ARParam *source, ARParam *icpara, double trans[3][4] );*/\r
+// private static int arParamDecomp( NyARParam source, NyARParam icpara, double[][] trans)\r
+// {\r
+// icpara.xsize = source.xsize;//icpara->xsize = source->xsize;\r
+// icpara.ysize = source.ysize;//icpara->ysize = source->ysize;\r
+// icpara.dist_factor[0] = source.dist_factor[0];//icpara->dist_factor[0] = source->dist_factor[0];\r
+// icpara.dist_factor[1] = source.dist_factor[1];// icpara->dist_factor[1] = source->dist_factor[1];\r
+// icpara.dist_factor[2] = source.dist_factor[2];//icpara->dist_factor[2] = source->dist_factor[2];\r
+// icpara.dist_factor[3] = source.dist_factor[3];//icpara->dist_factor[3] = source->dist_factor[3];\r
+// return arParamDecompMat(source.mat, icpara.mat, trans );\r
+// }\r
+ /**\r
+ * int arParamChangeSize( ARParam *source, int xsize, int ysize, ARParam *newparam );\r
+ * 関数の代替関数\r
+ * サイズプロパティをi_xsize,i_ysizeに変更します。\r
+ * @param xsize\r
+ * @param ysize\r
+ * @param newparam\r
+ * @return\r
+ * \r
+ */\r
+ public void changeSize(int i_xsize, int i_ysize)\r
+ {\r
+ double scale; \r
+ scale = (double)i_xsize / (double)(xsize);//scale = (double)xsize / (double)(source->xsize);\r
+ \r
+ for(int i = 0; i < 4; i++ ) {\r
+ array34[0*4+i]=array34[0*4+i]*scale;//newparam->mat[0][i] = source->mat[0][i] * scale;\r
+ array34[1*4+i]=array34[1*4+i]*scale;//newparam->mat[1][i] = source->mat[1][i] * scale;\r
+ array34[2*4+i]=array34[2*4+i];//newparam->mat[2][i] = source->mat[2][i];\r
+ }\r
+ \r
+ dist_factor[0] = dist_factor[0] * scale;//newparam->dist_factor[0] = source->dist_factor[0] * scale;\r
+ dist_factor[1] = dist_factor[1] * scale;//newparam->dist_factor[1] = source->dist_factor[1] * scale;\r
+ dist_factor[2] = dist_factor[2] / (scale*scale);//newparam->dist_factor[2] = source->dist_factor[2] / (scale*scale);\r
+ dist_factor[3] = dist_factor[3];//newparam->dist_factor[3] = source->dist_factor[3];\r
+ \r
+ xsize = i_xsize;//newparam->xsize = xsize;\r
+ ysize = i_ysize;//newparam->ysize = ysize;\r
+ }\r
+ /**\r
+ * int arParamIdeal2Observ( const double dist_factor[4], const double ix, const double iy,double *ox, double *oy )\r
+ * 関数の代替関数\r
+ * @param ix\r
+ * @param iy\r
+ * @param ox\r
+ * @param oy\r
+ */\r
+ public void ideal2Observ(double ix,double iy,DoubleValue ox, DoubleValue oy)\r
+ {\r
+\r
+ double x, y, d;\r
+ final double d0,d1,d3;\r
+ final double df[]=this.dist_factor;\r
+ d0=df[0];\r
+ d1=df[1];\r
+ d3=df[3];\r
+ x = (ix - d0) * d3;\r
+ y = (iy - d1) * d3;\r
+ if( x == 0.0 && y == 0.0 ) {\r
+ ox.value=d0;\r
+ oy.value=d1;\r
+ }else{\r
+ d = 1.0 - df[2]/100000000.0 * (x*x+y*y);\r
+ ox.value=x * d + d0;\r
+ oy.value=y * d + d1;\r
+ }\r
+ }\r
+ /**\r
+ * ideal2Observをまとめて実行します。\r
+ * @param i_in\r
+ * double[][2]\r
+ * @param o_out\r
+ * double[][2]\r
+ */\r
+ public void ideal2ObservBatch(double[][] i_in,double[][] o_out,int i_size)\r
+ {\r
+\r
+ double x, y, d;\r
+ final double d0,d1,d3,d2_w;\r
+ final double df[]=this.dist_factor;\r
+ d0=df[0];\r
+ d1=df[1];\r
+ d3=df[3];\r
+ d2_w=df[2]/100000000.0;\r
+ for(int i=0;i<i_size;i++){\r
+ x = (i_in[i][0] - d0) * d3;\r
+ y = (i_in[i][1] - d1) * d3;\r
+ if( x == 0.0 && y == 0.0 ) {\r
+ o_out[i][0]=d0;\r
+ o_out[i][1]=d1;\r
+ }else{\r
+ d = 1.0 - d2_w * (x*x+y*y);\r
+ o_out[i][0]=x * d + d0;\r
+ o_out[i][1]=y * d + d1;\r
+ }\r
+ }\r
+ return;\r
+ } \r
+ \r
+ \r
+ \r
+ \r
+ /**\r
+ * int arParamObserv2Ideal( const double dist_factor[4], const double ox, const double oy,double *ix, double *iy );\r
+ * \r
+ * @param ox\r
+ * @param oy\r
+ * @param ix\r
+ * @param iy\r
+ * @return\r
+ */\r
+ public int observ2Ideal(double ox,double oy,DoubleValue ix,DoubleValue iy)\r
+ {\r
+ double z02, z0, p, q, z, px, py,opttmp_1;\r
+ final double d0,d1,d3;\r
+ final double df[]=this.dist_factor;\r
+ d0=df[0];\r
+ d1=df[1];\r
+ \r
+ px = ox - d0;\r
+ py = oy - d1;\r
+ p = df[2]/100000000.0;\r
+ z02 = px*px+py*py;\r
+ q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
+ \r
+ for(int i = 1; ; i++ ) {\r
+ if( z0 != 0.0 ) {\r
+ //Optimize opttmp_1\r
+ opttmp_1=p*z02;\r
+ z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1);\r
+ px = px*z/z0;\r
+ py = py*z/z0;\r
+ }else {\r
+ px = 0.0;\r
+ py = 0.0;\r
+ break;\r
+ }\r
+ if( i == PD_LOOP ){\r
+ break;\r
+ }\r
+ z02 = px*px+ py*py;\r
+ z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py);\r
+ }\r
+ d3=df[3];\r
+ ix.value=px / d3 + d0;\r
+ iy.value=py / d3 + d1;\r
+ return 0;\r
+ }\r
+ /**\r
+ * 指定範囲のobserv2Idealをまとめて実行して、結果をo_idealに格納します。\r
+ * @param i_x_coord\r
+ * @param i_y_coord\r
+ * @param i_start\r
+ * coord開始点\r
+ * @param i_num\r
+ * 計算数\r
+ * @param o_ideal\r
+ * 出力バッファ[i_num][2]であること。\r
+ */\r
+ public void observ2IdealBatch(int[] i_x_coord,int[] i_y_coord,int i_start,int i_num,double[][] o_ideal)\r
+ {\r
+ double z02, z0,q, z, px, py,opttmp_1;\r
+ final double df[]=this.dist_factor;\r
+ final double d0=df[0];\r
+ final double d1=df[1];\r
+ final double d3=df[3];\r
+ final double p = df[2]/100000000.0;\r
+ for(int j = 0; j < i_num; j++ ){\r
+\r
+ px = i_x_coord[i_start+j] - d0;\r
+ py = i_y_coord[i_start+j] - d1;\r
+\r
+ z02 = px*px+py*py;\r
+ q = z0 = Math.sqrt(z02);//Optimize//q = z0 = Math.sqrt(px*px+ py*py);\r
+\r
+ for(int i = 1; ; i++ ) {\r
+ if( z0 != 0.0 ) {\r
+ //Optimize opttmp_1\r
+ opttmp_1=p*z02;\r
+ z = z0 - ((1.0 - opttmp_1)*z0 - q) / (1.0 - 3.0*opttmp_1);\r
+ px = px*z/z0;\r
+ py = py*z/z0;\r
+ }else {\r
+ px = 0.0;\r
+ py = 0.0;\r
+ break;\r
+ }\r
+ if( i == PD_LOOP ){\r
+ break;\r
+ }\r
+ z02 = px*px+ py*py;\r
+ z0 = Math.sqrt(z02);//Optimize//z0 = Math.sqrt(px*px+ py*py);\r
+ }\r
+ o_ideal[j][0]=px / d3 + d0;\r
+ o_ideal[j][1]=py / d3 + d1;\r
+ } \r
+ } \r
+ \r
+ \r
+ \r
+ \r
+ /**\r
+ * int arParamLoad( const char *filename, int num, ARParam *param, ...);\r
+ * i_streamの入力ストリームからi_num個の設定を読み込み、パラメタを配列にして返します。\r
+ * @param filename\r
+ * @param num\r
+ * @param param\r
+ * @return\r
+ * 設定を格納した配列を返します。\r
+ * @throws Exception\r
+ * i_num個の設定が読み出せない場合、JartkExceptionを発生します。\r
+ */\r
+ private static NyARParam[] arParamLoad(InputStream i_stream, int i_num) throws NyARException\r
+ {\r
+ try{\r
+ int read_size=SIZE_OF_PARAM_SET*i_num;\r
+ byte[] buf=new byte[read_size];\r
+ i_stream.read(buf);\r
+ //返却配列を確保\r
+ NyARParam[] result=new NyARParam[i_num];\r
+ \r
+ //バッファを加工\r
+ ByteBuffer bb = ByteBuffer.wrap(buf);\r
+ bb.order(ByteOrder.BIG_ENDIAN);\r
+ \r
+ //固定回数パースして配列に格納\r
+ for(int i=0;i<i_num;i++){\r
+ NyARParam new_param=new NyARParam();;\r
+ new_param.xsize=bb.getInt();\r
+ new_param.ysize=bb.getInt();\r
+ for(int i2=0;i2<3;i2++){\r
+ for(int i3=0;i3<4;i3++){\r
+ new_param.array34[i2*4+i3]=bb.getDouble();\r
+ }\r
+ }\r
+ for(int i2=0;i2<4;i2++){\r
+ new_param.dist_factor[i2]=bb.getDouble();\r
+ }\r
+ result[i]=new_param;\r
+ }\r
+ return result;\r
+ }catch(Exception e){\r
+ throw new NyARException(e);\r
+ }\r
+ }\r
+ public static int arParamSave(String filename,int num, NyARParam param[]) throws Exception\r
+ {\r
+ NyARException.trap("未チェックの関数");\r
+ byte buf[]=new byte[SIZE_OF_PARAM_SET*param.length];\r
+ //バッファをラップ\r
+ ByteBuffer bb = ByteBuffer.wrap(buf);\r
+ bb.order(ByteOrder.BIG_ENDIAN);\r
+\r
+ //書き込み\r
+ for(int i=0;i<param.length;i++){\r
+ bb.putInt(param[i].xsize);\r
+ bb.putInt(param[i].ysize);\r
+ for(int i2=0;i2<3;i2++){\r
+ for(int i3=0;i3<4;i3++){\r
+ bb.putDouble(param[i].array34[i2*4+i3]);\r
+ }\r
+ }\r
+ for(int i2=0;i2<4;i2++){\r
+ bb.putDouble(param[i].dist_factor[i2]);\r
+ }\r
+ }\r
+ //ファイルに保存\r
+ FileOutputStream fs=new FileOutputStream(filename);\r
+ fs.write(buf);\r
+ fs.close();\r
+\r
+ return 0;\r
+ }\r
+}\r
OSDN Git Service
