ARToolkit Java class library NyARToolkit.\r
Copyright (C)2008 R.Iizuka\r
\r
-version 1.0.0.20080428.0\r
+version 1.1.1.20080503.0\r
\r
http://nyatla.jp/\r
airmail(at)ebony.plala.or.jp\r
クラスは関数機能毎にまとめた作りになっていますので、オリジナルの\r
コード読んだことがあれば、なんとなく判ると思います。\r
\r
-演算性能は、Windows環境下でVCリリース版の約85％です。\r
+演算性能は、Windows環境下でVCリリース版の約101.5％です。\r
マーカー認識部分はネイティブ版よりも低速ですが、変換行列計算部分\r
-はネイティブ版よりも高速に動作します。\r
+はネイティブ版よりも圧倒的に高速に動作します。\r
\r
\r
・足りない機能等\r
\r
ではでは、楽しく遊んでくださいネ。\r
\r
-2008.04.28 R.Iizuka A虎＠nyatla.jp\r
+2008.05.03 R.Iizuka A虎＠nyatla.jp\r
//NyARToolkitの準備\r
try{\r
//キャプチャの準備\r
- capture=new JmfCameraCapture(320,240,15f,JmfCameraCapture.PIXEL_FORMAT_RGB);\r
+ capture=new JmfCameraCapture(SCREEN_X,SCREEN_Y,15f,JmfCameraCapture.PIXEL_FORMAT_RGB);\r
capture.setCaptureListener(this);\r
//NyARToolkitの準備\r
ar_param=new GLNyARParam();\r
+/* \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
import jp.nyatla.nyartoolkit.NyARException;\r
+/* \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
import jp.nyatla.nyartoolkit.NyARException;\r
+/* \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
private int[] wk_updateExtpat_x_rgb_index;\r
private int[] wk_updateExtpat_y_rgb_index;\r
private int[] wk_updateExtpat_i_rgb_index;\r
+ private int wk_updateExtpat_buffer_size=0;\r
\r
/**\r
* ワークバッファを予約する\r
*/\r
private void reservWorkBuffers(int i_xdiv2)\r
{\r
- int[] buf=this.wk_updateExtpat_rgb_buf;\r
- if(this.wk_updateExtpat_rgb_buf==null){\r
- wk_updateExtpat_para00_xw=new double[i_xdiv2];\r
- wk_updateExtpat_para10_xw=new double[i_xdiv2];\r
- wk_updateExtpat_para20_xw=new double[i_xdiv2];\r
- wk_updateExtpat_rgb_buf=new int[i_xdiv2*3];\r
- wk_updateExtpat_x_rgb_index=new int[i_xdiv2];\r
- wk_updateExtpat_y_rgb_index=new int[i_xdiv2];\r
- wk_updateExtpat_i_rgb_index=new int[i_xdiv2];\r
- }else{\r
- if(buf.length<i_xdiv2){\r
- wk_updateExtpat_para00_xw=new double[i_xdiv2];\r
- wk_updateExtpat_para10_xw=new double[i_xdiv2];\r
- wk_updateExtpat_para20_xw=new double[i_xdiv2];\r
- wk_updateExtpat_rgb_buf=new int[i_xdiv2*3];\r
- wk_updateExtpat_x_rgb_index=new int[i_xdiv2];\r
- wk_updateExtpat_y_rgb_index=new int[i_xdiv2];\r
- wk_updateExtpat_i_rgb_index=new int[i_xdiv2];\r
- }\r
- //十分なら何もしない。\r
- }\r
+ if(this.wk_updateExtpat_buffer_size<i_xdiv2){\r
+ wk_updateExtpat_para00_xw=new double[i_xdiv2];\r
+ wk_updateExtpat_para10_xw=new double[i_xdiv2];\r
+ wk_updateExtpat_para20_xw=new double[i_xdiv2];\r
+ wk_updateExtpat_rgb_buf=new int[i_xdiv2*3];\r
+ wk_updateExtpat_x_rgb_index=new int[i_xdiv2];\r
+ wk_updateExtpat_y_rgb_index=new int[i_xdiv2];\r
+ wk_updateExtpat_i_rgb_index=new int[i_xdiv2];\r
+ this.wk_updateExtpat_buffer_size=i_xdiv2;\r
+ }\r
+ //十分なら何もしない。\r
return;\r
}\r
\r
{\r
int img_x=image.getWidth();\r
int img_y=image.getHeight();\r
-\r
+ final int[][][] L_extpat=this.extpat;\r
+ final int L_WIDTH=this.width;\r
+ final int L_HEIGHT=this.height;\r
/*wk_pickFromRaster_ext_pat2ワーク変数を初期化する。*/\r
//int[][][] ext_pat2=wk_pickFromRaster_ext_pat2;//ARUint32 ext_pat2[AR_PATT_SIZE_Y][AR_PATT_SIZE_X][3];\r
int extpat_j[][],extpat_j_i[];\r
//int ext_pat2_j[][],ext_pat2_j_i[];\r
\r
- initValue_wk_pickFromRaster_ext_pat2(extpat,this.width,this.height);\r
+ initValue_wk_pickFromRaster_ext_pat2(L_extpat,L_WIDTH,L_HEIGHT);\r
\r
double[][] cpara_array=i_cpara.getArray();\r
double para21_x_yw,para01_x_yw,para11_x_yw;\r
int xc, yc;\r
int i,j;\r
// arGetCode_put_zero(ext_pat2);//put_zero( (ARUint8 *)ext_pat2, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3*sizeof(ARUint32) );\r
- int xdiv = i_xdiv2/width;//xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
- int ydiv = i_ydiv2/height;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
+ int xdiv = i_xdiv2/L_WIDTH;//xdiv = xdiv2/Config.AR_PATT_SIZE_X;\r
+ int ydiv = i_ydiv2/L_HEIGHT;//ydiv = ydiv2/Config.AR_PATT_SIZE_Y;\r
\r
//計算バッファを予約する\r
this.reservWorkBuffers(i_xdiv2); \r
}\r
\r
int index_num;\r
+\r
\r
for(j = 0; j < i_ydiv2; j++ ) {\r
yw = 102.5 + 5.0 * ((double)j+0.5) /i_ydiv2;\r
para21_x_yw=para21*yw+1.0;\r
para11_x_yw=para11*yw+para12;\r
para01_x_yw=para01*yw+para02;\r
- extpat_j=extpat[j/ydiv];\r
+ extpat_j=L_extpat[j/ydiv];\r
index_num=0;\r
//ステップ１．RGB取得用のマップを作成\r
for(i = 0; i < i_xdiv2; i++ ) {\r
}\r
/*<Optimize>*/\r
int xdiv_x_ydiv=xdiv*ydiv;\r
- for(j =this.height-1; j>=0; j--){\r
- extpat_j=extpat[j];\r
- for(i = this.width-1; i>=0; i--){ // PRL 2006-06-08.\r
+ for(j =L_HEIGHT-1; j>=0; j--){\r
+ extpat_j=L_extpat[j];\r
+ for(i = L_WIDTH-1; i>=0; i--){ // PRL 2006-06-08.\r
extpat_j_i=extpat_j[i];\r
extpat_j_i[0]/=(xdiv_x_ydiv);//ext_pat[j][i][0] = (byte)(ext_pat2[j][i][0] / (xdiv*ydiv));\r
extpat_j_i[1]/=(xdiv_x_ydiv);//ext_pat[j][i][1] = (byte)(ext_pat2[j][i][1] / (xdiv*ydiv));\r
* detectMarker関数から使う関数です。o_markerにlabelとclipで示される1個のマーカーを格納します。\r
* marker_holder[i_holder_num]にオブジェクトが無ければまず新規に作成し、もし\r
* 既に存在すればそこにマーカー情報を上書きして記録します。\r
- * Optimize:STEP[591->543]\r
+ * Optimize:STEP[369->336]\r
* @param o_marker\r
* @param limage\r
* @param label_ref\r
dir = 5;\r
\r
int r,c;\r
-\r
+ c=xcoord[0];\r
+ r=ycoord[0];\r
+ dmax=0;\r
+ //本家はdmaxの作成とxcoordの作成を別のループでやってるけど、非効率なので統合\r
for(;;){\r
- r=ycoord[coord_num-1];\r
- c=xcoord[coord_num-1];\r
+ //xcoord[1]-xcoord[n]までのデータを作る。\r
+ \r
+// 1個前のxcoordとycoordはループ後半で格納される。\r
+// c=xcoord[coord_num-1];\r
+// r=ycoord[coord_num-1];\r
//p1 = &(limage[marker_info2->y_coord[marker_info2->coord_num-1] * xsize+ marker_info2->x_coord[marker_info2->coord_num-1]]);\r
dir = (dir+5)%8;\r
for(i=0;i<8;i++) {\r
System.out.println("??? 2");//printf("??? 2\n");\r
throw new NyARException();//return(-1);\r
}\r
- xcoord[coord_num]= xcoord[coord_num-1] + xdir[dir];//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir];\r
- ycoord[coord_num]= ycoord[coord_num-1] + ydir[dir];//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir];\r
- if(xcoord[coord_num] == sx && ycoord[coord_num] == sy ){\r
+// xcoordとycoordをc,rにも保存\r
+ c= c + xdir[dir];//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir];\r
+ r= r + ydir[dir];//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir];\r
+ xcoord[coord_num]=c;//marker_info2->x_coord[marker_info2->coord_num]= marker_info2->x_coord[marker_info2->coord_num-1] + xdir[dir];\r
+ ycoord[coord_num]=r;//marker_info2->y_coord[marker_info2->coord_num]= marker_info2->y_coord[marker_info2->coord_num-1] + ydir[dir];\r
+ if(c == sx && r == sy ){\r
break;\r
}\r
+ //dmaxの計算\r
+ d=(c-sx)*(c-sx)+(r-sy)*(r-sy);\r
+ if( d > dmax ) {\r
+ dmax = d;\r
+ v1 = coord_num;\r
+ }\r
+ //終了条件判定\r
coord_num++;\r
- if(coord_num == xcoord.length-1){//if( marker_info2.coord_num == Config.AR_CHAIN_MAX-1 ){\r
+ if(coord_num == AR_CHAIN_MAX-1){//if( marker_info2.coord_num == Config.AR_CHAIN_MAX-1 ){\r
System.out.println("??? 3");//printf("??? 3\n");\r
throw new NyARException();//return(-1);\r
}\r
}\r
-\r
- dmax = 0;\r
- for(i=1;i<coord_num;i++) {// for(i=1;i<marker_info2->coord_num;i++) {\r
- d = (xcoord[i]-sx)*(xcoord[i]-sx)+ (ycoord[i]-sy)*(ycoord[i]-sy);// d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)+ (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy);\r
- if( d > dmax ) {\r
- dmax = d;\r
- v1 = i;\r
- }\r
- }\r
+//\r
+// dmax = 0;\r
+// for(i=1;i<coord_num;i++) {// for(i=1;i<marker_info2->coord_num;i++) {\r
+// d = (xcoord[i]-sx)*(xcoord[i]-sx)+ (ycoord[i]-sy)*(ycoord[i]-sy);// d = (marker_info2->x_coord[i]-sx)*(marker_info2->x_coord[i]-sx)+ (marker_info2->y_coord[i]-sy)*(marker_info2->y_coord[i]-sy);\r
+// if( d > dmax ) {\r
+// dmax = d;\r
+// v1 = i;\r
+// }\r
+// }\r
//NyARMarkerへcoord情報をセット\r
//coordの並び替えと保存はNyARMarkerへ移動\r
o_marker.setCoordXY(v1,coord_num,xcoord,ycoord);\r
-\r
-// int[] wx=new int[v1];//new int[Config.AR_CHAIN_MAX];\r
-// int[] wy=new int[v1]; //new int[Config.AR_CHAIN_MAX]; \r
-// for(i=0;i<v1;i++) {\r
-// wx[i] = marker_ref.x_coord[i];//wx[i] = marker_info2->x_coord[i];\r
-// wy[i] = marker_ref.y_coord[i];//wy[i] = marker_info2->y_coord[i];\r
-// }\r
-// for(i=0;i<marker_ref.coord_num-v1;i++) {//for(i=v1;i<marker_info2->coord_num;i++) {\r
-// marker_ref.x_coord[i] = marker_ref.x_coord[i+v1];//marker_info2->x_coord[i-v1] = marker_info2->x_coord[i];\r
-// marker_ref.y_coord[i] = marker_ref.y_coord[i+v1];//marker_info2->y_coord[i-v1] = marker_info2->y_coord[i];\r
-// }\r
-// for(i=0;i<v1;i++) {\r
-// marker_ref.x_coord[i-v1+marker_ref.coord_num] = wx[i];//marker_info2->x_coord[i-v1+marker_info2->coord_num] = wx[i];\r
-// marker_ref.y_coord[i-v1+marker_ref.coord_num] = wy[i];//marker_info2->y_coord[i-v1+marker_info2->coord_num] = wy[i];\r
-// }\r
-// marker_ref.x_coord[marker_ref.coord_num] = marker_ref.x_coord[0];//marker_info2->x_coord[marker_info2->coord_num] = marker_info2->x_coord[0];\r
-// marker_ref.y_coord[marker_ref.coord_num] = marker_ref.y_coord[0];//marker_info2->y_coord[marker_info2->coord_num] = marker_info2->y_coord[0];\r
-// marker_ref.coord_num++;//marker_info2->coord_num++;\r
return;\r
}\r
\r
*/\r
public final void detectMarker(NyARLabeling i_labeling,double i_factor,NyARMarkerList o_marker_list) throws NyARException\r
{\r
+ int label_area;\r
int i;\r
- int xsize, ysize;\r
+ int xsize, ysize;\r
NyARLabel[] labels=i_labeling.getLabel();\r
// int[] warea =i_labeling.getArea();\r
int label_num =i_labeling.getLabelNum();\r
NyARLabel label_pt;\r
for(i=0; i<label_num; i++ ){\r
label_pt=labels[i];\r
- if( label_pt.area < AR_AREA_MIN || label_pt.area > AR_AREA_MAX ){\r
+ label_area=label_pt.area;\r
+ if(label_area < AR_AREA_MIN || label_area > AR_AREA_MAX ){\r
continue;\r
}\r
if( label_pt.clip0 == 1 || label_pt.clip1 == xsize-2 ){//if( wclip[i*4+0] == 1 || wclip[i*4+1] == xsize-2 ){\r
//ret = arGetContour( limage, label_ref, i+1,&(wclip[i*4]), &(marker_info2[marker_num2]));\r
arGetContour(current_marker,limage, label_ref, i+1,label_pt);\r
\r
- if(!current_marker.checkSquare(label_pt.area,i_factor,label_pt.pos_x,label_pt.pos_y)){\r
+ if(!current_marker.checkSquare(label_area,i_factor,label_pt.pos_x,label_pt.pos_y)){\r
//後半で整理するからここはいらない。// marker_holder[marker_num2]=null;\r
continue;\r
}\r
public void detectSquare(NyARRaster i_image,int i_thresh,NyARSquareList o_square_holder) throws NyARException\r
{\r
// number_of_square=0;\r
+ \r
labeling.labeling(i_image, i_thresh);\r
if(labeling.getLabelNum()<1){\r
return;\r
\r
private int label_num;\r
//\r
- private int width;\r
- private int height;\r
+ private final int width;\r
+ private final int height;\r
/**\r
* @param i_width\r
* ラベリング画像の幅。解析するラスタの幅より大きいこと。\r
width =i_width;\r
height=i_height;\r
glabel_img=new int[height][width];\r
+ this.wk_reservLineBuffer_buf=new int[width];\r
label_num=0;\r
+\r
+\r
+ //ワークイメージに枠を書く\r
+ int[][] label_img=this.glabel_img;\r
+ for(int i = 0; i < i_width; i++){\r
+ label_img[0][i]=0;\r
+ label_img[i_height-1][i]=0;\r
+ }\r
+ //</Optimize>\r
+ for(int i = 0; i < i_height; i++) {\r
+ label_img[i][0]=0;\r
+ label_img[i][i_width-1]=0; \r
+ }\r
+ \r
+ \r
+ \r
+ \r
}\r
/**\r
* 検出したラベルの数を返す\r
{\r
return glabel_img;\r
}\r
- \r
+ //コンストラクタで作ること\r
private int[] wk_reservLineBuffer_buf=null;\r
- /**\r
- * ラベリング関数から使うラインスキャン用メモリの予約関数です。\r
- * i_widthピクセル分のラインスキャン用バッファを保障します。\r
- * @param i_width\r
- * @return\r
- */\r
- private int[] reservWorkLineBuffer(int i_width)\r
- {\r
- int[] buf=this.wk_reservLineBuffer_buf;\r
- if(buf==null){\r
- buf=new int[i_width];\r
- this.wk_reservLineBuffer_buf=buf;\r
- }else{\r
- if(buf.length<i_width){\r
- buf=new int[i_width];\r
- this.wk_reservLineBuffer_buf=buf;\r
- }\r
- //十分なら何もしない。\r
- }\r
- return buf;\r
- }\r
+\r
/**\r
* static ARInt16 *labeling2( ARUint8 *image, int thresh,int *label_num, int **area, double **pos, int **clip,int **label_ref, int LorR )\r
* 関数の代替品\r
int lxsize, lysize;\r
int thresht3 = thresh * 3;\r
int i,j,k;\r
-\r
+ lxsize=image.getWidth();//lxsize = arUtil_c.arImXsize;\r
+ lysize=image.getHeight();//lysize = arUtil_c.arImYsize;\r
+ //画素数の一致チェック\r
+ if(lxsize!=this.width || lysize!=this.height){\r
+ throw new NyARException();\r
+ } \r
//ラベル数を0に初期化\r
this.label_num=0;\r
-// int[][] work2=this.wk_labeling_work2;\r
-// int[] work=this.gwork;\r
+\r
+\r
\r
int[][] label_img=this.glabel_img;\r
\r
- lxsize=image.getWidth();//lxsize = arUtil_c.arImXsize;\r
- lysize=image.getHeight();//lysize = arUtil_c.arImYsize;\r
\r
- int[] label_img_pt0,label_img_pt1;\r
+ //枠作成はインスタンスを作った直後にやってしまう。\r
\r
- //<Optimize>label_img_pt0,label_img_pt1\r
- label_img_pt0=label_img[0];\r
- label_img_pt1=label_img[lysize-1];\r
- for(i = 0; i < lxsize; i++){\r
- label_img_pt0[i]=0;\r
- label_img_pt1[i]=0;\r
- }\r
- //</Optimize>\r
- for(i = 0; i < lysize; i++) {\r
- label_img[i][0]=0;\r
- label_img[i][lxsize-1]=0; \r
- }\r
-\r
int[] work2_pt;\r
wk_max = 0;\r
\r
\r
int[] work=this.work_holder.work;\r
int[][] work2=this.work_holder.work2;\r
- //1ライン分のメモリを予約\r
- int[] line_bufferr=reservWorkLineBuffer(lxsize);\r
-\r
+ int[] line_bufferr=this.wk_reservLineBuffer_buf;\r
+ \r
+ int[] label_img_pt0,label_img_pt1;\r
for(j = 1; j < lysize - 1; j++) {//for (int j = 1; j < lysize - 1; j++, pnt += poff*2, pnt2 += 2) {\r
label_img_pt0=label_img[j];\r
label_img_pt1=label_img[j-1];\r
if( label_pt.clip3 < work2_pt[6] ){\r
label_pt.clip3 = work2_pt[6];\r
}\r
- \r
-// warea[j] += work2_pt[0];\r
-// wpos[j*2+0] += work2_pt[1];\r
-// wpos[j*2+1] += work2_pt[2];\r
-// if( wclip[j][0] > work2_pt[3] ){\r
-// wclip[j][0] = work2_pt[3];\r
-// }\r
-// if( wclip[j][1] < work2_pt[4] ){\r
-// wclip[j][1] = work2_pt[4];\r
-// }\r
-// if( wclip[j][2] > work2_pt[5] ){\r
-// wclip[j][2] = work2_pt[5];\r
-// }\r
-// if( wclip[j][3] < work2_pt[6] ){\r
-// wclip[j][3] = work2_pt[6];\r
-// }\r
}\r
\r
for(i = 0; i < wlabel_num; i++ ) {//for(int i = 0; i < *label_num; i++ ) {\r
/**\r
* static int arDetectMarker2_check_square( int area, ARMarkerInfo2 *marker_info2, double factor )\r
* 関数の代替関数\r
- * OPTIMIZED STEP [450->397]\r
+ * OPTIMIZED STEP [450->415]\r
* @param i_area\r
* @param i_factor\r
* @return\r
final int[] l_y_coord=y_coord;\r
final NyARVertexCounter wv1=wk_checkSquare_wv1;\r
final NyARVertexCounter wv2=wk_checkSquare_wv2;\r
- int sx, sy;\r
- int dmax, d, v1;\r
+ int sx,sy;\r
+ int dmax,d,v1;\r
\r
int v2;// int wvnum1,wvnum2,v2;\r
- double thresh;\r
- int i;\r
+ int i;\r
\r
+ final int L_coord_num_m1=this.coord_num-1;\r
dmax = 0;\r
v1 = 0;\r
sx = l_x_coord[0];//sx = marker_info2->x_coord[0];\r
sy = l_y_coord[0];//sy = marker_info2->y_coord[0];\r
- for(i=1;i<this.coord_num-1;i++){//for(i=1;i<marker_info2->coord_num-1;i++) {\r
- d = (l_x_coord[i]-sx)*(l_x_coord[i]-sx)+ (l_y_coord[i]-sy)*(l_y_coord[i]-sy);\r
+ for(i=1;i<L_coord_num_m1;i++){//for(i=1;i<marker_info2->coord_num-1;i++) {\r
+ d = (l_x_coord[i]-sx)*(l_x_coord[i]-sx)+ (l_y_coord[i]-sy)*(l_y_coord[i]-sy);\r
if( d > dmax ) {\r
dmax = d;\r
v1 = i;\r
}\r
}\r
\r
- thresh = (i_area/0.75) * 0.01 * i_factor;\r
+ final double thresh = (i_area/0.75) * 0.01 * i_factor;\r
\r
l_vertex[0] = 0;\r
\r
if(!wv1.getVertex(l_x_coord,l_y_coord, 0, v1,thresh)){ //if( get_vertex(marker_info2->x_coord, marker_info2->y_coord, 0, v1,thresh, wv1, &wvnum1) < 0 ) {\r
return false;\r
}\r
- if(!wv2.getVertex(l_x_coord,l_y_coord,v1, this.coord_num-1, thresh)) {//if(get_vertex(marker_info2->x_coord, marker_info2->y_coord,v1, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) {\r
+ if(!wv2.getVertex(l_x_coord,l_y_coord,v1,L_coord_num_m1, thresh)) {//if(get_vertex(marker_info2->x_coord, marker_info2->y_coord,v1, marker_info2->coord_num-1, thresh, wv2, &wvnum2) < 0 ) {\r
return false;\r
}\r
\r
if(!wv1.getVertex(l_x_coord,l_y_coord,v1, v2, thresh)) {\r
return false;\r
}\r
- if(!wv2.getVertex(l_x_coord,l_y_coord,v2, this.coord_num-1, thresh)) {\r
+ if(!wv2.getVertex(l_x_coord,l_y_coord,v2,L_coord_num_m1, thresh)) {\r
return false;\r
}\r
if( wv1.number_of_vertex==1 && wv2.number_of_vertex==1 ) {\r
else {\r
return false;\r
}\r
- l_vertex[4] = this.coord_num-1;//この値使ってるの？\r
+ l_vertex[4] =L_coord_num_m1;//この値使ってるの？\r
//\r
area = i_area;\r
pos[0] = i_pos_x;\r
}\r
\r
\r
+\r
/**\r
* get_vertex関数を切り離すためのクラス\r
*\r
//double epsl;\r
double p,pbuf,work;\r
\r
-// epsl = 1.0e-10; /* Threshold value */\r
/* check size */\r
switch(dimen){\r
case 0:\r
* ループ内で0初期化していいかが判らない。\r
*/\r
ip=0;\r
-// int wap_ptr,wbp_ptr;\r
//For順変更禁止\r
for(int n=0; n<dimen;n++)\r
{\r
ap_n =ap[n];//wcp = ap + n * rowa;\r
p=0.0;\r
-// wap_ptr=0;//wap = DoublePointer.wrap(wcp);\r
for(int i = n; i<dimen ; i++){//for(i = n, wap = wcp, p = 0.0; i < dimen ; i++, wap += rowa)\r
-// wap=ap[i];\r
if( p < ( pbuf = Math.abs(ap[i][0]))) {\r
p = pbuf;\r
ip = i;\r
nos[n] = nwork;\r
\r
ap_ip=ap[ip];\r
-// wbp=ap_n;\r
-// wap_ptr=0;\r
-// wbp_ptr=0;\r
for(j=0; j< dimen ; j++){//for(j = 0, wap = ap + ip * rowa, wbp = wcp; j < dimen ; j++) {\r
work = ap_ip[j]; //work = *wap;\r
ap_ip[j]=ap_n[j];\r
-// wap_ptr++;//*wap++ = *wbp;\r
ap_n[j]=work;\r
-// wbp_ptr++; //*wbp++ = work;\r
}\r
\r
-// wap=ap_n;\r
-// wap_ptr=0;\r
work=ap_n[0];\r
for(j = 0; j < dimen_1 ; j++){//for(j = 1, wap = wcp, work = *wcp; j < dimen ; j++, wap++)\r
ap_n[j]=ap_n[j+1]/work;//*wap = *(wap + 1) / work;\r
-// wap_ptr++;\r
}\r
ap_n[j]=1.0/work;//*wap = 1.0 / work;\r
-// wbp=ap_n;\r
for(int i = 0; i < dimen ; i++) {\r
if(i != n) {\r
ap_i =ap[i];//wap = ap + i * rowa;\r
\r
-// wap_ptr=0;\r
-// wbp_ptr=0;\r
work=ap_i[0];\r
for(j = 0;j < dimen_1 ; j++){//for(j = 1, wbp = wcp, work = *wap;j < dimen ; j++, wap++, wbp++)\r
ap_i[j]=ap_i[j+1]-work*ap_n[j];//wap = *(wap + 1) - work * (*wbp);\r
-// wap_ptr++;\r
-// wbp_ptr++;\r
}\r
ap_i[j]=-work*ap_n[j];//*wap = -work * (*wbp);\r
}\r
nos[j] = nos[n];\r
for(int i = 0; i < dimen ;i++){//for(i = 0, wap = ap + j, wbp = ap + n; i < dimen ;i++, wap += rowa, wbp += rowa) {\r
ap_i=ap[i];\r
-// wbp=ap[i];\r
work =ap_i[j];//work = *wap;\r
ap_i[j]=ap_i[n];//*wap = *wbp;\r
ap_i[n]=work;//*wbp = work;\r
if(mean.getClm()!= clm){\r
throw new NyARException();\r
}\r
- \r
+ double[][] im=inout.m;\r
+ double[] im_i;\r
+ double w0,w1;\r
v = mean.getArray();\r
- for(int i = 0; i < row; i++ ) {\r
- for(int j = 0; j < clm; j++ ){\r
- //*(m++) -= *(v++);\r
- inout.m[i][j]-=v[j];\r
+ //特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。\r
+ switch(clm){\r
+ case 1:\r
+ w0=v[0];\r
+ for(int i = 0; i < row; i++ ){\r
+ im[i][0]-=w0;\r
+ }\r
+ break;\r
+ case 2:\r
+ w0=v[0];\r
+ w1=v[1];\r
+ for(int i = 0; i < row; i++ ){\r
+ im_i=im[i];\r
+ im_i[0]-=w0;\r
+ im_i[1]-=w1;\r
+ }\r
+ break;\r
+ default:\r
+ for(int i = 0; i < row; i++ ){\r
+ im_i=im[i];\r
+ for(int j = 0; j < clm; j++ ){\r
+ //*(m++) -= *(v++);\r
+ im_i[j]-=v[j];\r
+ }\r
}\r
}\r
+ return;\r
}\r
/**\r
* int x_by_xt( ARMat *input, ARMat *output )の代替関数\r
if( ev == null ){\r
throw new NyARException();\r
}\r
-\r
+ final double[][] L_m=this.m;\r
this.vecTridiagonalize(dv,ev,1);\r
\r
ev_array[0]=0.0;//ev->v[0] = 0.0;\r
ev_array[k+1]+=s * (c * w - 2 * s * ev_array[k+1]);//ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]);\r
\r
for(int i = 0; i < dim; i++ ){\r
- x = this.m[k][i];//x = a->m[k*dim+i];\r
- y = this.m[k+1][i];//y = a->m[(k+1)*dim+i];\r
- this.m[k][i]=c * x - s * y;//a->m[k*dim+i] = c * x - s * y;\r
- this.m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y;\r
+ x = L_m[k][i];//x = a->m[k*dim+i];\r
+ y = L_m[k+1][i];//y = a->m[(k+1)*dim+i];\r
+ L_m[k][i]=c * x - s * y;//a->m[k*dim+i] = c * x - s * y;\r
+ L_m[k+1][i]=s * x + c * y;//a->m[(k+1)*dim+i] = s * x + c * y;\r
}\r
if( k < h-1 ) {\r
NyARException.trap("未チェックパス");{\r
\r
\r
import jp.nyatla.nyartoolkit.NyARException;\r
-import jp.nyatla.util.DoubleValue;\r
+\r
\r
/**\r
* This class calculates ARMatrix from square information and holds it.\r
* 変換行列を計算して、結果を保持するクラス。\r
*\r
*/\r
-public class NyARTransMat{\r
- private final static int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0\r
- private final static int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1\r
- private final static int arFittingMode =AR_FITTING_TO_INPUT;\r
-\r
- private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
- private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
- private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX 500\r
- private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
- private final NyARTransRot transrot;\r
- private final double[] center={0.0,0.0};\r
- private final NyARParam param;\r
- private final NyARMat result_mat=new NyARMat(3,4);\r
- public NyARTransMat(NyARParam i_param)throws NyARException\r
- {\r
- param=i_param;\r
- transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
-\r
- }\r
- public void setCenter(double i_x,double i_y)\r
- {\r
- center[0]=i_x;\r
- center[1]=i_x;\r
- }\r
- public NyARMat getTransformationMatrix()\r
- {\r
- return result_mat;\r
- }\r
-\r
- private final double[] wk_transMat_pos3d=new double[P_MAX*3];//pos3d[P_MAX][3];\r
- private final double[][] wk_transMat_ppos2d=new double[4][2];\r
- private final double[][] wk_transMat_ppos3d=new double[4][2];\r
- private final double[] wk_transMat_off=new double[3];\r
- /**\r
- * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
- * 関数の置き換え。\r
- * 保持している変換行列を更新する。\r
- * @param square\r
- * 計算対象のNyARSquareオブジェクト\r
- * @param i_direction\r
- * @param width\r
- * @return\r
- * @throws NyARException\r
- */\r
- public double transMat( NyARSquare square,int i_direction, double width)throws NyARException\r
- {\r
- double[][] ppos2d=wk_transMat_ppos2d;\r
- double[][] ppos3d=wk_transMat_ppos3d;\r
- double[] off=wk_transMat_off;\r
- double[] pos3d=wk_transMat_pos3d;\r
- int dir;\r
- double err=-1;\r
-\r
-\r
-\r
- transrot.initRot(square,i_direction);\r
-\r
- dir = i_direction;\r
- ppos2d[0][0] = square.sqvertex[(4-dir)%4][0];\r
- ppos2d[0][1] = square.sqvertex[(4-dir)%4][1];\r
- ppos2d[1][0] = square.sqvertex[(5-dir)%4][0];\r
- ppos2d[1][1] = square.sqvertex[(5-dir)%4][1];\r
- ppos2d[2][0] = square.sqvertex[(6-dir)%4][0];\r
- ppos2d[2][1] = square.sqvertex[(6-dir)%4][1];\r
- ppos2d[3][0] = square.sqvertex[(7-dir)%4][0];\r
- ppos2d[3][1] = square.sqvertex[(7-dir)%4][1];\r
- ppos3d[0][0] = center[0] - width/2.0;\r
- ppos3d[0][1] = center[1] + width/2.0;\r
- ppos3d[1][0] = center[0] + width/2.0;\r
- ppos3d[1][1] = center[1] + width/2.0;\r
- ppos3d[2][0] = center[0] + width/2.0;\r
- ppos3d[2][1] = center[1] - width/2.0;\r
- ppos3d[3][0] = center[0] - width/2.0;\r
- ppos3d[3][1] = center[1] - width/2.0;\r
-\r
- //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
- arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
-\r
-\r
-\r
- for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {\r
- err = arGetTransMat3(ppos2d, pos3d,off);\r
- if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
- break;\r
- }\r
- }\r
- return err;\r
- }\r
- private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
- private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
- /**\r
- * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
- * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
- * @param i_ppos3d\r
- * 入力配列[num][3]\r
- * @param o_pos3d\r
- * 出力配列[P_MAX][3]\r
- * @param o_off\r
- * [3]\r
- * @throws NyARException\r
- */\r
- private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[] o_pos3d,double[] o_off)throws NyARException\r
- {\r
- double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
- double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double off[3], pmax[3], pmin[3];\r
- pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
- pmin[0]=pmin[1]=pmin[2] = 10000000000.0;\r
- for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
- if( i_ppos3d[i][0] > pmax[0] ){\r
- pmax[0] = i_ppos3d[i][0];\r
- }\r
- if( i_ppos3d[i][0] < pmin[0] ){\r
- pmin[0] = i_ppos3d[i][0];\r
- }\r
- if( i_ppos3d[i][1] > pmax[1] ){\r
- pmax[1] = i_ppos3d[i][1];\r
- }\r
- if( i_ppos3d[i][1] < pmin[1] ){\r
- pmin[1] = i_ppos3d[i][1];\r
- }\r
- }\r
- o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
- o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
- o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
- for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
- o_pos3d[i*3+0] = i_ppos3d[i][0] + o_off[0];\r
- o_pos3d[i*3+1] = i_ppos3d[i][1] + o_off[1];\r
- o_pos3d[i*3+2] = 0.0;\r
- }\r
- } \r
- \r
- \r
- /**\r
- * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double ppos3d[][2], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
- * STEP 414->\r
- * @param ppos2d\r
- * @param i_pos3d\r
- * @param i_off\r
- * @param num\r
- * @return\r
- * @throws NyARException\r
- */\r
- private final double arGetTransMat3(\r
- double ppos2d[][],\r
- final double i_pos3d[],\r
- final double i_off[])throws NyARException{\r
- \r
- double ret;\r
- ret = arGetTransMatSub(ppos2d, i_pos3d);\r
- double[][] conv=result_mat.getArray();\r
- conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + conv[0][2]*i_off[2] + conv[0][3];\r
- conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + conv[1][2]*i_off[2] + conv[1][3];\r
- conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + conv[2][2]*i_off[2] + conv[2][3];\r
- return ret;\r
- }\r
- private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
- private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
- private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
- private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 );\r
- private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
- private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
- private final double[] wk_arGetTransMatSub_trans=new double[3];\r
- private final double[] wk_arGetTransMatSub_pos2d=new double[P_MAX*2];//pos2d[P_MAX][2];\r
- private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
- private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
- \r
-\r
- /**\r
- * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
- * Optimize:2008.04.20:STEP[1033→1004]\r
- * @param rot\r
- * @param ppos2d\r
- * @param pos3d\r
- * @param num\r
- * @return\r
- * @throws NyARException\r
- */\r
- private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[]) throws NyARException\r
- {\r
- double[] pos2d=wk_arGetTransMatSub_pos2d;\r
- double cpara[]=param.get34Array();\r
- NyARMat mat_a,mat_b,mat_c,mat_d,mat_e,mat_f;//ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
-\r
- double wx, wy, wz;\r
- double ret;\r
- int i, j;\r
-\r
- mat_a =this.wk_arGetTransMatSub_mat_a;\r
-// mat_a.realloc(NUMBER_OF_VERTEX*2,3);\r
- double[][] a_array=mat_a.getArray();\r
-\r
- mat_b =this.wk_arGetTransMatSub_mat_b;\r
-// mat_b.realloc(3,NUMBER_OF_VERTEX*2);\r
- double[][] b_array=mat_b.getArray();\r
-\r
-\r
- DoubleValue a1=wk_arGetTransMatSub_a1;\r
- DoubleValue a2=wk_arGetTransMatSub_a2;\r
- if(arFittingMode == AR_FITTING_TO_INPUT ){\r
- for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
- param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);\r
- pos2d[i*2+0]=a1.value;\r
- pos2d[i*2+1]=a2.value;\r
- }\r
- }else{\r
- for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
- pos2d[i*2+0] = i_ppos2d[i][0];\r
- pos2d[i*2+1] = i_ppos2d[i][1];\r
- }\r
- }\r
- mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
-// mat_c.realloc(NUMBER_OF_VERTEX*2,1);\r
- double[][] c_array=mat_c.getArray();\r
- double[] rot=transrot.getArray();\r
- for( j = 0; j < NUMBER_OF_VERTEX; j++ ) {\r
- int x2=j*2;\r
- //<Optimize> \r
- //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
- //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
- //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
- wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2];\r
- wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2];\r
- wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2];\r
- //</Optimize>\r
- a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
- a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
- a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
- a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
- a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
- a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
- c_array[x2][0] =wz * pos2d[j*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
- c_array[x2+1][0]=wz * pos2d[j*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
- }\r
- mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。\r
- mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
- mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
- double[][] f_array=mat_f.getArray();\r
-\r
- mat_d.matrixMul(mat_b, mat_a );\r
- mat_e.matrixMul(mat_b, mat_c );\r
- mat_d.matrixSelfInv();\r
- mat_f.matrixMul(mat_d, mat_e );\r
-\r
- double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; \r
- trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
- trans[1] = f_array[1][0];\r
- trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
-\r
- ret =transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
- for( j = 0; j < NUMBER_OF_VERTEX; j++ ) {\r
- int x2=j*2;\r
- //<Optimize> \r
- //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
- //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
- //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
- wx = rot[0] * i_pos3d[j*3+0]+ rot[1] * i_pos3d[j*3+1]+ rot[2] * i_pos3d[j*3+2];\r
- wy = rot[3] * i_pos3d[j*3+0]+ rot[4] * i_pos3d[j*3+1]+ rot[5] * i_pos3d[j*3+2];\r
- wz = rot[6] * i_pos3d[j*3+0]+ rot[7] * i_pos3d[j*3+1]+ rot[8] * i_pos3d[j*3+2];\r
- //</Optimize>\r
- a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
- a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
- a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - pos2d[j*2+0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
- a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
- a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
- a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[j*2+1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
- c_array[x2][0] =wz * pos2d[j*2+0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
- c_array[x2+1][0]=wz * pos2d[j*2+1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
- }\r
-// JartkException.trap("未チェックのパス");{\r
- mat_d.matrixMul(mat_b, mat_a );\r
- mat_e.matrixMul(mat_b, mat_c );\r
- mat_d.matrixSelfInv();\r
- mat_f.matrixMul(mat_d, mat_e );\r
-// }\r
- trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
- trans[1] = f_array[1][0];\r
- trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
-\r
-\r
- ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
- double[][] conv=result_mat.getArray();\r
- for( j = 2; j >=0; j-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
- //<Optimize>\r
- //for( i = 0; i < 3; i++ ){\r
- // conv[j][i] = rot[j][i];\r
- //}\r
- conv[j][0] = rot[j*3+0];\r
- conv[j][1] = rot[j*3+1];\r
- conv[j][2] = rot[j*3+2];\r
- //</Optimize>\r
- conv[j][3] = trans[j];\r
- }\r
- return ret;\r
- }\r
+public interface NyARTransMat{\r
+ public void setCenter(double i_x,double i_y);\r
+ public NyARMat getTransformationMatrix();\r
+ public double transMat( NyARSquare square,int i_direction, double width)throws NyARException;\r
}\r
--- /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
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.util.DoubleValue;\r
+\r
+public class NyARTransMat_O1 implements NyARTransMat{\r
+ private final static int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0\r
+ private final static int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1\r
+ private final static int arFittingMode =AR_FITTING_TO_INPUT;\r
+\r
+ private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
+ private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
+ private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX 500\r
+ private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
+ private final NyARTransRot transrot;\r
+ private final double[] center={0.0,0.0};\r
+ private final NyARParam param;\r
+ private final NyARMat result_mat=new NyARMat(3,4);\r
+ public NyARTransMat_O1(NyARParam i_param)throws NyARException\r
+ {\r
+ param=i_param;\r
+ transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
+\r
+ }\r
+ public void setCenter(double i_x,double i_y)\r
+ {\r
+ center[0]=i_x;\r
+ center[1]=i_x;\r
+ }\r
+ public NyARMat getTransformationMatrix()\r
+ {\r
+ return result_mat;\r
+ }\r
+\r
+ private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];\r
+ private final double[][] wk_transMat_ppos2d=new double[4][2];\r
+ private final double[][] wk_transMat_ppos3d=new double[4][2];\r
+ private final double[] wk_transMat_off=new double[3];\r
+ /**\r
+ * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
+ * 関数の置き換え。\r
+ * 保持している変換行列を更新する。\r
+ * @param square\r
+ * 計算対象のNyARSquareオブジェクト\r
+ * @param i_direction\r
+ * @param width\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public double transMat( NyARSquare square,int i_direction, double width)throws NyARException\r
+ {\r
+ double[][] ppos2d=wk_transMat_ppos2d;\r
+ double[][] ppos3d=wk_transMat_ppos3d;\r
+ double[] off=wk_transMat_off;\r
+ double[][] pos3d=wk_transMat_pos3d;\r
+ int dir;\r
+ double err=-1;\r
+\r
+\r
+\r
+ transrot.initRot(square,i_direction);\r
+\r
+ dir = i_direction;\r
+ ppos2d[0][0] = square.sqvertex[(4-dir)%4][0];\r
+ ppos2d[0][1] = square.sqvertex[(4-dir)%4][1];\r
+ ppos2d[1][0] = square.sqvertex[(5-dir)%4][0];\r
+ ppos2d[1][1] = square.sqvertex[(5-dir)%4][1];\r
+ ppos2d[2][0] = square.sqvertex[(6-dir)%4][0];\r
+ ppos2d[2][1] = square.sqvertex[(6-dir)%4][1];\r
+ ppos2d[3][0] = square.sqvertex[(7-dir)%4][0];\r
+ ppos2d[3][1] = square.sqvertex[(7-dir)%4][1];\r
+ ppos3d[0][0] = center[0] - width/2.0;\r
+ ppos3d[0][1] = center[1] + width/2.0;\r
+ ppos3d[1][0] = center[0] + width/2.0;\r
+ ppos3d[1][1] = center[1] + width/2.0;\r
+ ppos3d[2][0] = center[0] + width/2.0;\r
+ ppos3d[2][1] = center[1] - width/2.0;\r
+ ppos3d[3][0] = center[0] - width/2.0;\r
+ ppos3d[3][1] = center[1] - width/2.0;\r
+\r
+ //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
+ arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
+\r
+\r
+\r
+ for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ) {\r
+ err = arGetTransMat3(ppos2d, pos3d,off);\r
+ if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
+ break;\r
+ }\r
+ }\r
+ return err;\r
+ }\r
+ private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
+ private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
+ /**\r
+ * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
+ * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
+ * @param i_ppos3d\r
+ * 入力配列[num][3]\r
+ * @param o_pos3d\r
+ * 出力配列[P_MAX][3]\r
+ * @param o_off\r
+ * [3]\r
+ * @throws NyARException\r
+ */\r
+ private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[][] o_pos3d,double[] o_off)throws NyARException\r
+ {\r
+ double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
+ double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double off[3], pmax[3], pmin[3];\r
+ int i;\r
+ pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
+ pmin[0]=pmin[1]=pmin[2] = 10000000000.0;\r
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ if( i_ppos3d[i][0] > pmax[0] ){\r
+ pmax[0] = i_ppos3d[i][0];\r
+ }\r
+ if( i_ppos3d[i][0] < pmin[0] ){\r
+ pmin[0] = i_ppos3d[i][0];\r
+ }\r
+ if( i_ppos3d[i][1] > pmax[1] ){\r
+ pmax[1] = i_ppos3d[i][1];\r
+ }\r
+ if( i_ppos3d[i][1] < pmin[1] ){\r
+ pmin[1] = i_ppos3d[i][1];\r
+ }\r
+ }\r
+ o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
+ o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
+ o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
+\r
+ \r
+ double[] o_pos3d_pt;\r
+ double[] i_pos_pd_pt; \r
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ o_pos3d_pt =o_pos3d[i];\r
+ i_pos_pd_pt=i_ppos3d[i];\r
+ o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
+ o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
+ o_pos3d_pt[2] = 0.0;\r
+ }\r
+ } \r
+ \r
+ \r
+ /**\r
+ * double arGetTransMat3( double rot[3][3], double ppos2d[][2],double ppos3d[][2], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
+ * STEP 414->\r
+ * @param ppos2d\r
+ * @param i_pos3d\r
+ * @param i_off\r
+ * @param num\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ private final double arGetTransMat3(\r
+ double ppos2d[][],\r
+ final double i_pos3d[][],\r
+ final double i_off[])throws NyARException{\r
+ \r
+ double ret;\r
+ ret = arGetTransMatSub(ppos2d, i_pos3d);\r
+ double[][] conv=result_mat.getArray();\r
+ conv[0][3] = conv[0][0]*i_off[0] + conv[0][1]*i_off[1] + conv[0][2]*i_off[2] + conv[0][3];\r
+ conv[1][3] = conv[1][0]*i_off[0] + conv[1][1]*i_off[1] + conv[1][2]*i_off[2] + conv[1][3];\r
+ conv[2][3] = conv[2][0]*i_off[0] + conv[2][1]*i_off[1] + conv[2][2]*i_off[2] + conv[2][3];\r
+ return ret;\r
+ }\r
+ private final NyARMat wk_arGetTransMatSub_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
+ private final NyARMat wk_arGetTransMatSub_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
+ private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
+ private final NyARMat wk_arGetTransMatSub_mat_d=new NyARMat( 3, 3 );\r
+ private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
+ private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
+ private final double[] wk_arGetTransMatSub_trans=new double[3];\r
+ private final double[][] wk_arGetTransMatSub_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];\r
+ private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
+ private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
+ \r
+\r
+ /**\r
+ * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
+ * Optimize:2008.04.20:STEP[1033→1004]\r
+ * @param rot\r
+ * @param ppos2d\r
+ * @param pos3d\r
+ * @param num\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[][]) throws NyARException\r
+ {\r
+ double[][] pos2d=wk_arGetTransMatSub_pos2d;\r
+ double cpara[]=param.get34Array();\r
+ NyARMat mat_a,mat_b,mat_c,mat_d,mat_e,mat_f;//ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
+\r
+ double wx, wy, wz;\r
+ double ret;\r
+ int i;\r
+ double[] po2d_pt;\r
+\r
+ mat_a =this.wk_arGetTransMatSub_mat_a;\r
+// mat_a.realloc(NUMBER_OF_VERTEX*2,3);\r
+ double[][] a_array=mat_a.getArray();\r
+\r
+ mat_b =this.wk_arGetTransMatSub_mat_b;\r
+// mat_b.realloc(3,NUMBER_OF_VERTEX*2);\r
+ double[][] b_array=mat_b.getArray();\r
+\r
+\r
+ DoubleValue a1=wk_arGetTransMatSub_a1;\r
+ DoubleValue a2=wk_arGetTransMatSub_a2;\r
+ if(arFittingMode == AR_FITTING_TO_INPUT ){\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ param.ideal2Observ(i_ppos2d[i][0], i_ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);\r
+ po2d_pt=pos2d[i];\r
+ po2d_pt[0]=a1.value;\r
+ po2d_pt[1]=a2.value;\r
+ }\r
+ }else{\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ){\r
+ po2d_pt=pos2d[i];\r
+ pos2d[i][0] = i_ppos2d[i][0];\r
+ pos2d[i][1] = i_ppos2d[i][1];\r
+ }\r
+ }\r
+ mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
+// mat_c.realloc(NUMBER_OF_VERTEX*2,1);\r
+ double[][] c_array=mat_c.getArray();\r
+ double[] rot=transrot.getArray();\r
+ double[] i_pos3d_pt;\r
+ int x2;\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ x2=i*2;\r
+ i_pos3d_pt=i_pos3d[i];\r
+ po2d_pt=pos2d[i];\r
+ wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
+ wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
+ wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
+ //</Optimize>\r
+ a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
+ a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
+ a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
+ a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
+ a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
+ a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
+ c_array[x2][0] =wz * po2d_pt[0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
+ c_array[x2+1][0]=wz * po2d_pt[1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
+ }\r
+ mat_d = this.wk_arGetTransMatSub_mat_d;//次処理で値をもらうので、初期化の必要は無い。\r
+ mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
+ mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
+ double[][] f_array=mat_f.getArray();\r
+\r
+ mat_d.matrixMul(mat_b, mat_a );\r
+ mat_e.matrixMul(mat_b, mat_c );\r
+ mat_d.matrixSelfInv();\r
+ mat_f.matrixMul(mat_d, mat_e );\r
+\r
+ double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; \r
+ trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
+ trans[1] = f_array[1][0];\r
+ trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
+\r
+ ret =transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ x2=i*2;\r
+ i_pos3d_pt=i_pos3d[i];\r
+ po2d_pt=pos2d[i];\r
+ //<Optimize> \r
+ //wx = rot[0][0] * pos3d[j][0]+ rot[0][1] * pos3d[j][1]+ rot[0][2] * pos3d[j][2];\r
+ //wy = rot[1][0] * pos3d[j][0]+ rot[1][1] * pos3d[j][1]+ rot[1][2] * pos3d[j][2];\r
+ //wz = rot[2][0] * pos3d[j][0]+ rot[2][1] * pos3d[j][1]+ rot[2][2] * pos3d[j][2];\r
+ wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
+ wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
+ wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
+ //</Optimize>\r
+ a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
+ a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
+ a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2] - po2d_pt[0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
+ a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
+ a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
+ a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - po2d_pt[1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
+ c_array[x2][0] =wz * po2d_pt[0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
+ c_array[x2+1][0]=wz * po2d_pt[1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
+ }\r
+// JartkException.trap("未チェックのパス");{\r
+ mat_d.matrixMul(mat_b, mat_a );\r
+ mat_e.matrixMul(mat_b, mat_c );\r
+ mat_d.matrixSelfInv();\r
+ mat_f.matrixMul(mat_d, mat_e );\r
+// }\r
+ trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
+ trans[1] = f_array[1][0];\r
+ trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
+\r
+\r
+ ret = transrot.modifyMatrix(trans, i_pos3d, pos2d);\r
+ double[][] conv=result_mat.getArray();\r
+ for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
+ //<Optimize>\r
+ //for( i = 0; i < 3; i++ ){\r
+ // conv[j][i] = rot[j][i];\r
+ //}\r
+ conv[i][0] = rot[i*3+0];\r
+ conv[i][1] = rot[i*3+1];\r
+ conv[i][2] = rot[i*3+2];\r
+ //</Optimize>\r
+ conv[i][3] = trans[i];\r
+ }\r
+ return ret;\r
+ }\r
+}\r
--- /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
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.util.DoubleValue;\r
+\r
+\r
+\r
+/**\r
+ * This class calculates ARMatrix from square information and holds it.\r
+ * --\r
+ * 変換行列を計算して、結果を保持するクラス。\r
+ *\r
+ */\r
+public class NyARTransMat_O2 implements NyARTransMat{\r
+ private final static int AR_FITTING_TO_IDEAL=0;//#define AR_FITTING_TO_IDEAL 0\r
+ private final static int AR_FITTING_TO_INPUT=1;//#define AR_FITTING_TO_INPUT 1\r
+ private final static int arFittingMode =AR_FITTING_TO_INPUT;\r
+\r
+ private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT=5;//#define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5\r
+ private final static double AR_GET_TRANS_MAT_MAX_FIT_ERROR=1.0;//#define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0\r
+ private final static int P_MAX=10;//頂点の数(4で十分だけどなんとなく10)//#define P_MAX 500\r
+ private final static int NUMBER_OF_VERTEX=4;//処理対象の頂点数\r
+ private final NyARTransRot transrot;\r
+ private final double[] center={0.0,0.0};\r
+ private final NyARParam param;\r
+ private final NyARMat result_mat=new NyARMat(3,4);\r
+ public NyARTransMat_O2(NyARParam i_param)throws NyARException\r
+ {\r
+ param=i_param;\r
+ transrot=new NyARTransRot_O3(i_param,NUMBER_OF_VERTEX);\r
+\r
+ }\r
+ public void setCenter(double i_x,double i_y)\r
+ {\r
+ center[0]=i_x;\r
+ center[1]=i_x;\r
+ }\r
+ public NyARMat getTransformationMatrix()\r
+ {\r
+ return result_mat;\r
+ }\r
+\r
+ private final double[][] wk_transMat_pos3d=new double[P_MAX][3];//pos3d[P_MAX][3];\r
+ private final double[][] wk_transMat_ppos2d=new double[4][2];\r
+ private final double[][] wk_transMat_ppos3d=new double[4][2];\r
+ private final double[] wk_transMat_off=new double[3];\r
+ private final double[][] wk_transMat_pos2d=new double[P_MAX][2];//pos2d[P_MAX][2];\r
+\r
+\r
+ private final DoubleValue wk_arGetTransMatSub_a1=new DoubleValue();\r
+ private final DoubleValue wk_arGetTransMatSub_a2=new DoubleValue();\r
+ private final NyARMat wk_transMat_mat_a=new NyARMat(NUMBER_OF_VERTEX*2,3);\r
+ private final NyARMat wk_transMat_mat_b=new NyARMat(3,NUMBER_OF_VERTEX*2);\r
+ private final NyARMat wk_transMat_mat_d=new NyARMat( 3, 3 ); \r
+ private final double[] wk_transMat_mat_trans=new double[3];\r
+\r
+ /**\r
+ * double arGetTransMat( ARMarkerInfo *marker_info,double center[2], double width, double conv[3][4] )\r
+ * 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。\r
+ * 保持している変換行列を更新する。\r
+ * @param square\r
+ * 計算対象のNyARSquareオブジェクト\r
+ * @param i_direction\r
+ * @param width\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ public double transMat( NyARSquare square,int i_direction, double width)throws NyARException\r
+ {\r
+ double[][] ppos2d=wk_transMat_ppos2d;\r
+ double[][] ppos3d=wk_transMat_ppos3d;\r
+ double[] off=wk_transMat_off;\r
+ double[][] pos3d=wk_transMat_pos3d;\r
+\r
+\r
+ ppos2d[0][0] = square.sqvertex[(4-i_direction)%4][0];\r
+ ppos2d[0][1] = square.sqvertex[(4-i_direction)%4][1];\r
+ ppos2d[1][0] = square.sqvertex[(5-i_direction)%4][0];\r
+ ppos2d[1][1] = square.sqvertex[(5-i_direction)%4][1];\r
+ ppos2d[2][0] = square.sqvertex[(6-i_direction)%4][0];\r
+ ppos2d[2][1] = square.sqvertex[(6-i_direction)%4][1];\r
+ ppos2d[3][0] = square.sqvertex[(7-i_direction)%4][0];\r
+ ppos2d[3][1] = square.sqvertex[(7-i_direction)%4][1];\r
+ ppos3d[0][0] = center[0] - width/2.0;\r
+ ppos3d[0][1] = center[1] + width/2.0;\r
+ ppos3d[1][0] = center[0] + width/2.0;\r
+ ppos3d[1][1] = center[1] + width/2.0;\r
+ ppos3d[2][0] = center[0] + width/2.0;\r
+ ppos3d[2][1] = center[1] - width/2.0;\r
+ ppos3d[3][0] = center[0] - width/2.0;\r
+ ppos3d[3][1] = center[1] - width/2.0;\r
+\r
+ transrot.initRot(square,i_direction);\r
+\r
+ //arGetTransMat3の前段処理(pos3dとoffを初期化)\r
+ arGetTransMat3_initPos3d(ppos3d,pos3d,off);\r
+\r
+\r
+ //arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。\r
+ double[][] pos2d=this.wk_transMat_pos2d;\r
+ DoubleValue a1=this.wk_arGetTransMatSub_a1;\r
+ DoubleValue a2=this.wk_arGetTransMatSub_a2;\r
+ if(arFittingMode == AR_FITTING_TO_INPUT ){\r
+ for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ param.ideal2Observ(ppos2d[i][0], ppos2d[i][1],a1,a2);//arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1],&pos2d[i][0], &pos2d[i][1]);\r
+ pos2d[i][0]=a1.value;\r
+ pos2d[i][1]=a2.value;\r
+ }\r
+ }else{\r
+ for(int i = 0; i < NUMBER_OF_VERTEX; i++ ){\r
+ pos2d[i][0] = ppos2d[i][0];\r
+ pos2d[i][1] = ppos2d[i][1];\r
+ }\r
+ }\r
+\r
+ //変換マトリクスdとbの準備(arGetTransMatSubの一部)\r
+ final double cpara[]=param.get34Array();\r
+ final NyARMat mat_a =this.wk_transMat_mat_a;\r
+ final double[][] a_array=mat_a.getArray();\r
+\r
+ final NyARMat mat_b =this.wk_transMat_mat_b;\r
+ final double[][] b_array=mat_b.getArray();\r
+\r
+ int x2;\r
+ for(int i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ x2=i*2;\r
+ //</Optimize>\r
+ a_array[x2 ][0]=b_array[0][x2]=cpara[0*4+0];//mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0];\r
+ a_array[x2 ][1]=b_array[1][x2]=cpara[0*4+1];//mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];\r
+ a_array[x2 ][2]=b_array[2][x2]=cpara[0*4+2]-pos2d[i][0];//mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];\r
+ a_array[x2+1][0]=b_array[0][x2+1]=0.0;//mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;\r
+ a_array[x2+1][1]=b_array[1][x2+1]=cpara[1*4+1];//mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1];\r
+ a_array[x2+1][2]=b_array[2][x2+1]=cpara[1*4+2] - pos2d[i][1];//mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];\r
+ }\r
+ final NyARMat mat_d =this.wk_transMat_mat_d;\r
+ mat_d.matrixMul(mat_b,mat_a);\r
+ mat_d.matrixSelfInv();\r
+\r
+ double err=-1;\r
+ double[] rot=transrot.getArray();\r
+ double[][] conv=result_mat.getArray();\r
+ double[] trans=this.wk_transMat_mat_trans;\r
+ for(int i=0;i<AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++ ){\r
+ //<arGetTransMat3>\r
+ err = arGetTransMatSub(pos2d, pos3d,mat_b,mat_d,trans);\r
+ conv[0][0] = rot[0*3+0];\r
+ conv[0][1] = rot[0*3+1];\r
+ conv[0][2] = rot[0*3+2];\r
+ conv[1][0] = rot[1*3+0];\r
+ conv[1][1] = rot[1*3+1];\r
+ conv[1][2] = rot[1*3+2];\r
+ conv[2][0] = rot[2*3+0];\r
+ conv[2][1] = rot[2*3+1];\r
+ conv[2][2] = rot[2*3+2];\r
+ conv[0][3] = rot[0*3+0]*off[0] + rot[0*3+1]*off[1] + rot[0*3+2]*off[2] + trans[0];\r
+ conv[1][3] = rot[1*3+0]*off[0] + rot[1*3+1]*off[1] + rot[1*3+2]*off[2] + trans[1];\r
+ conv[2][3] = rot[2*3+0]*off[0] + rot[2*3+1]*off[1] + rot[2*3+2]*off[2] + trans[2];\r
+\r
+ //</arGetTransMat3>\r
+ if( err < AR_GET_TRANS_MAT_MAX_FIT_ERROR ){\r
+ break;\r
+ }\r
+ }\r
+ return err;\r
+ }\r
+ private final double[] wk_arGetTransMat3_initPos3d_pmax=new double[3];\r
+ private final double[] wk_arGetTransMat3_initPos3d_pmin=new double[3];\r
+ /**\r
+ * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
+ * 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
+ * @param i_ppos3d\r
+ * 入力配列[num][3]\r
+ * @param o_pos3d\r
+ * 出力配列[P_MAX][3]\r
+ * @param o_off\r
+ * [3]\r
+ * @throws NyARException\r
+ */\r
+ private final void arGetTransMat3_initPos3d(double i_ppos3d[][],double[][] o_pos3d,double[] o_off)throws NyARException\r
+ {\r
+ final double[] pmax=wk_arGetTransMat3_initPos3d_pmax;//new double[3];\r
+ final double[] pmin=wk_arGetTransMat3_initPos3d_pmin;//new double[3];//double off[3], pmax[3], pmin[3];\r
+ int i;\r
+ pmax[0]=pmax[1]=pmax[2] = -10000000000.0;\r
+ pmin[0]=pmin[1]=pmin[2] = 10000000000.0;\r
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ if( i_ppos3d[i][0] > pmax[0] ){\r
+ pmax[0] = i_ppos3d[i][0];\r
+ }\r
+ if( i_ppos3d[i][0] < pmin[0] ){\r
+ pmin[0] = i_ppos3d[i][0];\r
+ }\r
+ if( i_ppos3d[i][1] > pmax[1] ){\r
+ pmax[1] = i_ppos3d[i][1];\r
+ }\r
+ if( i_ppos3d[i][1] < pmin[1] ){\r
+ pmin[1] = i_ppos3d[i][1];\r
+ }\r
+ }\r
+ o_off[0] = -(pmax[0] + pmin[0]) / 2.0;\r
+ o_off[1] = -(pmax[1] + pmin[1]) / 2.0;\r
+ o_off[2] = -(pmax[2] + pmin[2]) / 2.0;\r
+\r
+\r
+ double[] o_pos3d_pt;\r
+ double[] i_pos_pd_pt; \r
+ for(i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ o_pos3d_pt =o_pos3d[i];\r
+ i_pos_pd_pt=i_ppos3d[i];\r
+ o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
+ o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
+ o_pos3d_pt[2] = 0.0;\r
+ }\r
+ } \r
+\r
+ private final NyARMat wk_arGetTransMatSub_mat_c=new NyARMat(NUMBER_OF_VERTEX*2,1);\r
+ private final NyARMat wk_arGetTransMatSub_mat_e=new NyARMat( 3, 1 );\r
+ private final NyARMat wk_arGetTransMatSub_mat_f=new NyARMat( 3, 1 );\r
+\r
+ /**\r
+ * static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double *dist_factor, double cpara[3][4] )\r
+ * Optimize:2008.04.20:STEP[1033→1004]\r
+ * @param i_ppos2d\r
+ * @param i_pos3d\r
+ * @param i_mat_b\r
+ * 演算用行列b\r
+ * @param i_mat_d\r
+ * 演算用行列d\r
+ * @return\r
+ * @throws NyARException\r
+ */\r
+ private final double arGetTransMatSub(double i_ppos2d[][],double i_pos3d[][],NyARMat i_mat_b,NyARMat i_mat_d,double[] o_trans) throws NyARException\r
+ {\r
+ double cpara[]=param.get34Array();\r
+ NyARMat mat_c,mat_e,mat_f;//ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;\r
+\r
+ double wx, wy, wz;\r
+ double ret;\r
+ int i;\r
+\r
+ mat_c =this.wk_arGetTransMatSub_mat_c;//次処理で値をもらうので、初期化の必要は無い。\r
+ double[][] c_array=mat_c.getArray();\r
+ double[] rot=transrot.getArray();\r
+ double[] i_pos3d_pt;\r
+ int x2;\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ x2=i*2;\r
+ i_pos3d_pt=i_pos3d[i];\r
+ wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
+ wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
+ wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
+ c_array[x2][0] =wz * i_ppos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
+ c_array[x2+1][0]=wz * i_ppos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
+ }\r
+ mat_e = this.wk_arGetTransMatSub_mat_e;//次処理で値をもらうので、初期化の必要は無い。\r
+ mat_f = this.wk_arGetTransMatSub_mat_f;//次処理で値をもらうので、初期化の必要は無い。\r
+ double[][] f_array=mat_f.getArray();\r
+\r
+ mat_e.matrixMul(i_mat_b, mat_c );\r
+ mat_f.matrixMul(i_mat_d, mat_e );\r
+\r
+// double[] trans=wk_arGetTransMatSub_trans;//double trans[3]; \r
+ o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
+ o_trans[1] = f_array[1][0];\r
+ o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
+ ret =transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d);\r
+ for( i = 0; i < NUMBER_OF_VERTEX; i++ ) {\r
+ x2=i*2;\r
+ i_pos3d_pt=i_pos3d[i];\r
+ wx = rot[0] * i_pos3d_pt[0]+ rot[1] * i_pos3d_pt[1]+ rot[2] * i_pos3d_pt[2];\r
+ wy = rot[3] * i_pos3d_pt[0]+ rot[4] * i_pos3d_pt[1]+ rot[5] * i_pos3d_pt[2];\r
+ wz = rot[6] * i_pos3d_pt[0]+ rot[7] * i_pos3d_pt[1]+ rot[8] * i_pos3d_pt[2];\r
+ c_array[x2][0] =wz * i_ppos2d[i][0]- cpara[0*4+0]*wx - cpara[0*4+1]*wy - cpara[0*4+2]*wz;//mat_c->m[j*2+0] = wz * pos2d[j][0]- cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz;\r
+ c_array[x2+1][0]=wz * i_ppos2d[i][1]- cpara[1*4+1]*wy - cpara[1*4+2]*wz;//mat_c->m[j*2+1] = wz * pos2d[j][1]- cpara[1][1]*wy - cpara[1][2]*wz;\r
+ }\r
+\r
+ mat_e.matrixMul(i_mat_b, mat_c );\r
+ mat_f.matrixMul(i_mat_d, mat_e );\r
+ o_trans[0] = f_array[0][0];//trans[0] = mat_f->m[0];\r
+ o_trans[1] = f_array[1][0];\r
+ o_trans[2] = f_array[2][0];//trans[2] = mat_f->m[2];\r
+ ret = transrot.modifyMatrix(o_trans, i_pos3d, i_ppos2d);\r
+\r
+// double[][] conv=result_mat.getArray();\r
+// for( i = 2; i >=0; i-- ) {//<Optimize/>for( j = 0; j < 3; j++ ) {\r
+// //<Optimize>\r
+// //for( i = 0; i < 3; i++ ){\r
+// // conv[j][i] = rot[j][i];\r
+// //}\r
+// conv[i][0] = rot[i*3+0];\r
+// conv[i][1] = rot[i*3+1];\r
+// conv[i][2] = rot[i*3+2];\r
+// //</Optimize>\r
+// conv[i][3] = trans[i];\r
+// }\r
+ return ret;\r
+ }\r
+}\r
* @return\r
* @throws NyARException\r
*/\r
- public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException;\r
+ public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException;\r
public void initRot(NyARSquare marker_info,int i_direction) throws NyARException; \r
}\r
\r
*/\r
protected final int arGetAngle(double[] o_abc)\r
{\r
- double a, b, c;\r
+ double a, b, c,tmp;\r
double sina, cosa, sinb, cosb, sinc, cosc;\r
double[] rot=array;\r
if( rot[8] > 1.0 ) {//<Optimize/>if( rot[2][2] > 1.0 ) {\r
//<Optimize>\r
//sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
//cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/ (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
- sinc = (rot[7]*rot[2]-rot[6]*rot[5])/ (rot[2]*rot[2]+rot[5]*rot[5]);\r
- cosc = -(rot[2]*rot[6]+rot[5]*rot[7])/ (rot[2]*rot[2]+rot[5]*rot[5]);\r
+ tmp = (rot[2]*rot[2]+rot[5]*rot[5]);\r
+ sinc = (rot[7]*rot[2]-rot[6]*rot[5])/ tmp;\r
+ cosc = -(rot[2]*rot[6]+rot[5]*rot[7])/ tmp;\r
//</Optimize>\r
\r
if( cosc > 1.0 ) {\r
* @param num\r
* @return\r
*/\r
- public final double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException\r
+ public final double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
{\r
int num=this.number_of_vertex;\r
double factor;\r
arGetNewMatrix(rot,trans, null, combo );\r
err = 0.0;\r
for( i = 0; i < num; i++ ) {\r
- hx = combo[0] * vertex[i*3+0]+ combo[1] * vertex[i*3+1]+ combo[2] * vertex[i*3+2]+ combo[3];\r
- hy = combo[4] * vertex[i*3+0]+ combo[5] * vertex[i*3+1]+ combo[6] * vertex[i*3+2]+ combo[7];\r
- h = combo[8] * vertex[i*3+0]+ combo[9] * vertex[i*3+1]+ combo[10] * vertex[i*3+2]+ combo[11];\r
+ hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3];\r
+ hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7];\r
+ h = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11];\r
x = hx / h;\r
y = hy / h;\r
- err += (pos2d[i*2+0] - x) * (pos2d[i*2+0] - x)+ (pos2d[i*2+1] - y) * (pos2d[i*2+1] - y);\r
+ err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y);\r
}\r
if( err < minerr ) {\r
minerr = err;\r
* @return\r
* @throws NyARException\r
*/\r
- public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException\r
+ public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
{\r
int num=this.number_of_vertex;\r
double factor;\r
nyatla_arGetNewMatrix_row012(rot,trans,combo);//第二パラメタは常にnull//arGetNewMatrix(trans, null, combo );\r
err = 0.0;\r
for( i = 0; i < num; i++ ) {\r
- hx = combo[0] * vertex[i*3+0]+ combo[1] * vertex[i*3+1]+ combo[2] * vertex[i*3+2]+ combo[3];\r
- hy = combo[4] * vertex[i*3+0]+ combo[5] * vertex[i*3+1]+ combo[6] * vertex[i*3+2]+ combo[7];\r
- h = combo[8] * vertex[i*3+0]+ combo[9] * vertex[i*3+1]+ combo[10] * vertex[i*3+2]+ combo[11];\r
+ hx = combo[0] * vertex[i][0]+ combo[1] * vertex[i][1]+ combo[2] * vertex[i][2]+ combo[3];\r
+ hy = combo[4] * vertex[i][0]+ combo[5] * vertex[i][1]+ combo[6] * vertex[i][2]+ combo[7];\r
+ h = combo[8] * vertex[i][0]+ combo[9] * vertex[i][1]+ combo[10] * vertex[i][2]+ combo[11];\r
x = hx / h;\r
y = hy / h;\r
- err += (pos2d[i*2+0] - x) * (pos2d[i*2+0] - x)+ (pos2d[i*2+1] - y) * (pos2d[i*2+1] - y);\r
+ err += (pos2d[i][0] - x) * (pos2d[i][0] - x)+ (pos2d[i][1] - y) * (pos2d[i][1] - y);\r
}\r
if( err < minerr ) {\r
minerr = err;\r
\r
/**\r
* NyARModifyMatrixの最適化バージョン3\r
+ * O3版の演算テーブル版\r
* 計算速度のみを追求する\r
*\r
*/\r
rot[8] = wdir[2][2];\r
//</Optimize> \r
}\r
- private final double[] wk_arModifyMatrix_abc=new double[3];\r
+ private final double[][] wk_arModifyMatrix_double1D=new double[8][3];\r
/**\r
* arGetRot計算を階層化したModifyMatrix\r
* 896\r
* @param nyrot\r
* @param trans\r
* @param vertex\r
+ * [m][3]\r
* @param pos2d\r
- * @param num\r
+ * [n][2]\r
* @return\r
* @throws NyARException\r
*/\r
- public double modifyMatrix(double trans[],double vertex[], double pos2d[]) throws NyARException\r
+ public double modifyMatrix(double trans[],double vertex[][], double pos2d[][]) throws NyARException\r
{\r
- double CACA,SASA,SACA,CA,SA;\r
- double CACACB,SACACB,SASACB,CASB,SASB;\r
- double SACASC,SACACBSC,SACACBCC,SACACC; \r
double factor;\r
- double a1, b1, c1;\r
double a2, b2, c2;\r
double ma = 0.0, mb = 0.0, mc = 0.0;\r
double h, x, y;\r
double err, minerr=0;\r
int t1, t2, t3;\r
int s1 = 0, s2 = 0, s3 = 0;\r
- int i;\r
- final double[] abc=wk_arModifyMatrix_abc;\r
\r
- arGetAngle(abc);//arGetAngle( rot, &a, &b, &c );\r
- a2 = abc[0];\r
- b2 = abc[1];\r
- c2 = abc[2];\r
factor = 10.0*Math.PI/180.0;\r
double rot0,rot1,rot3,rot4,rot6,rot7;\r
double combo00,combo01,combo02,combo03,combo10,combo11,combo12,combo13,combo20,combo21,combo22,combo23;\r
double combo02_2,combo02_5,combo02_8,combo02_11;\r
double combo22_2,combo22_5,combo22_8,combo22_11;\r
double combo12_2,combo12_5,combo12_8,combo12_11;\r
+ //vertex展開\r
+ final double VX00,VX01,VX02,VX10,VX11,VX12,VX20,VX21,VX22,VX30,VX31,VX32;\r
+ double[] d_pt;\r
+ d_pt=vertex[0];VX00=d_pt[0];VX01=d_pt[1];VX02=d_pt[2];\r
+ d_pt=vertex[1];VX10=d_pt[0];VX11=d_pt[1];VX12=d_pt[2];\r
+ d_pt=vertex[2];VX20=d_pt[0];VX21=d_pt[1];VX22=d_pt[2];\r
+ d_pt=vertex[3];VX30=d_pt[0];VX31=d_pt[1];VX32=d_pt[2];\r
+ final double P2D00,P2D01,P2D10,P2D11,P2D20,P2D21,P2D30,P2D31;\r
+ d_pt=pos2d[0];P2D00=d_pt[0];P2D01=d_pt[1];\r
+ d_pt=pos2d[1];P2D10=d_pt[0];P2D11=d_pt[1];\r
+ d_pt=pos2d[2];P2D20=d_pt[0];P2D21=d_pt[1];\r
+ d_pt=pos2d[3];P2D30=d_pt[0];P2D31=d_pt[1];\r
final double cpara[]=cparam.get34Array();\r
- combo03 = cpara[0] * trans[0]+ cpara[1] * trans[1]+ cpara[2] * trans[2]+ cpara[3];\r
- combo13 = cpara[4] * trans[0]+ cpara[5] * trans[1]+ cpara[6] * trans[2]+ cpara[7];\r
- combo23 = cpara[8] * trans[0]+ cpara[9] * trans[1]+ cpara[10] * trans[2]+ cpara[11];\r
+ final double CP0,CP1,CP2,CP3,CP4,CP5,CP6,CP7,CP8,CP9,CP10;\r
+ CP0=cpara[0];CP1=cpara[1];CP2=cpara[2];CP3=cpara[3];\r
+ CP4=cpara[4];CP5=cpara[5];CP6=cpara[6];CP7=cpara[7];\r
+ CP8=cpara[8];CP9=cpara[9];CP10=cpara[10];\r
+ combo03 = CP0 * trans[0]+ CP1 * trans[1]+ CP2 * trans[2]+ CP3;\r
+ combo13 = CP4 * trans[0]+ CP5 * trans[1]+ CP6 * trans[2]+ CP7;\r
+ combo23 = CP8 * trans[0]+ CP9 * trans[1]+ CP10 * trans[2]+ cpara[11];\r
+ double CACA,SASA,SACA,CA,SA;\r
+ double CACACB,SACACB,SASACB,CASB,SASB;\r
+ double SACASC,SACACBSC,SACACBCC,SACACC; \r
+ final double[][] double1D=this.wk_arModifyMatrix_double1D;\r
+\r
+ final double[] abc =double1D[0];\r
+ final double[] a_factor=double1D[1];\r
+ final double[] sinb =double1D[2];\r
+ final double[] cosb =double1D[3];\r
+ final double[] b_factor=double1D[4];\r
+ final double[] sinc =double1D[5];\r
+ final double[] cosc =double1D[6];\r
+ final double[] c_factor=double1D[7];\r
+ double w,w2;\r
double wsin,wcos;\r
+\r
+ arGetAngle(abc);//arGetAngle( rot, &a, &b, &c );\r
+ a2 = abc[0];\r
+ b2 = abc[1];\r
+ c2 = abc[2];\r
+ \r
//comboの3行目を先に計算\r
- for( i = 0; i < 10; i++ ) {\r
+ for(int i = 0; i < 10; i++ ) {\r
minerr = 1000000000.0;\r
- for(t1=-1;t1<=1;t1++) {\r
- a1 = a2 + factor*t1;\r
- wsin = Math.sin(a1);\r
- wcos = Math.cos(a1);\r
+ //sin-cosテーブルを計算(これが外に出せるとは…。)\r
+ for(int j=0;j<3;j++){\r
+ w2=factor*(j-1);\r
+ w= a2 + w2;\r
+ a_factor[j]=w;\r
+ w= b2 + w2;\r
+ b_factor[j]=w;\r
+ sinb[j]=Math.sin(w);\r
+ cosb[j]=Math.cos(w);\r
+ w= c2 + w2;\r
+ c_factor[j]=w;\r
+ sinc[j]=Math.sin(w);\r
+ cosc[j]=Math.cos(w);\r
+ }\r
+ //\r
+ for(t1=0;t1<3;t1++) {\r
+ SA = Math.sin(a_factor[t1]);\r
+ CA = Math.cos(a_factor[t1]);\r
//Optimize\r
- CACA=wcos*wcos;\r
- SASA=wsin*wsin;\r
- SACA=wsin*wcos;\r
- CA=wcos;\r
- SA=wsin;\r
- for(t2=-1;t2<=1;t2++) {\r
- b1 = b2 + factor*t2;\r
- wsin = Math.sin(b1);\r
- wcos = Math.cos(b1);\r
+ CACA=CA*CA;\r
+ SASA=SA*SA;\r
+ SACA=SA*CA;\r
+ for(t2=0;t2<3;t2++) {\r
+ wsin=sinb[t2];\r
+ wcos=cosb[t2];\r
CACACB=CACA*wcos;\r
SACACB=SACA*wcos;\r
SASACB=SASA*wcos;\r
CASB=CA*wsin;\r
SASB=SA*wsin;\r
//comboの計算1\r
- combo02 = cpara[0] * CASB+ cpara[1] * SASB+ cpara[2] * wcos;\r
- combo12 = cpara[4] * CASB+ cpara[5] * SASB+ cpara[6] * wcos;\r
- combo22 = cpara[8] * CASB+ cpara[9] * SASB+ cpara[10] * wcos;\r
- combo02_2 =combo02 * vertex[2]+combo03;\r
- combo02_5 =combo02 * vertex[5]+combo03;\r
- combo02_8 =combo02 * vertex[8]+combo03;\r
- combo02_11=combo02 * vertex[11]+combo03;\r
- combo12_2 =combo12 * vertex[2]+ combo13;\r
- combo12_5 =combo12 * vertex[5]+ combo13;\r
- combo12_8 =combo12 * vertex[8]+ combo13;\r
- combo12_11=combo12 * vertex[11]+ combo13;\r
- combo22_2 =combo22 * vertex[2]+ combo23;\r
- combo22_5 =combo22 * vertex[5]+ combo23;\r
- combo22_8 =combo22 * vertex[8]+ combo23;\r
- combo22_11=combo22 * vertex[11]+ combo23;\r
- \r
- for(t3=-1;t3<=1;t3++) {\r
- c1 = c2 + factor*t3;\r
- wsin = Math.sin(c1);\r
- wcos = Math.cos(c1);\r
+ combo02 = CP0 * CASB+ CP1 * SASB+ CP2 * wcos;\r
+ combo12 = CP4 * CASB+ CP5 * SASB+ CP6 * wcos;\r
+ combo22 = CP8 * CASB+ CP9 * SASB+ CP10 * wcos;\r
+\r
+ combo02_2 =combo02 * VX02 + combo03;\r
+ combo02_5 =combo02 * VX12 + combo03;\r
+ combo02_8 =combo02 * VX22 + combo03;\r
+ combo02_11=combo02 * VX32 + combo03;\r
+ combo12_2 =combo12 * VX02 + combo13;\r
+ combo12_5 =combo12 * VX12 + combo13;\r
+ combo12_8 =combo12 * VX22 + combo13;\r
+ combo12_11=combo12 * VX32 + combo13;\r
+ combo22_2 =combo22 * VX02 + combo23;\r
+ combo22_5 =combo22 * VX12 + combo23;\r
+ combo22_8 =combo22 * VX22 + combo23;\r
+ combo22_11=combo22 * VX32 + combo23; \r
+ for(t3=0;t3<3;t3++){\r
+ wsin=sinc[t3];\r
+ wcos=cosc[t3]; \r
SACASC=SACA*wsin;\r
SACACC=SACA*wcos;\r
SACACBSC=SACACB*wsin;\r
SACACBCC=SACACB*wcos;\r
- \r
+\r
rot0 = CACACB*wcos+SASA*wcos+SACACBSC-SACASC;\r
rot3 = SACACBCC-SACACC+SASACB*wsin+CACA*wsin;\r
rot6 = -CASB*wcos-SASB*wsin;\r
- combo00 = cpara[0] * rot0+ cpara[1] * rot3+ cpara[2] * rot6;\r
- combo10 = cpara[4] * rot0+ cpara[5] * rot3+ cpara[6] * rot6;\r
- combo20 = cpara[8] * rot0+ cpara[9] * rot3+ cpara[10] * rot6;\r
- \r
+\r
+ combo00 = CP0 * rot0+ CP1 * rot3+ CP2 * rot6;\r
+ combo10 = CP4 * rot0+ CP5 * rot3+ CP6 * rot6;\r
+ combo20 = CP8 * rot0+ CP9 * rot3+ CP10 * rot6;\r
+\r
rot1 = -CACACB*wsin-SASA*wsin+SACACBCC-SACACC;\r
rot4 = -SACACBSC+SACASC+SASACB*wcos+CACA*wcos;\r
rot7 = CASB*wsin-SASB*wcos;\r
- combo01 = cpara[0] * rot1+ cpara[1] * rot4+ cpara[2] * rot7;\r
- combo11 = cpara[4] * rot1+ cpara[5] * rot4+ cpara[6] * rot7;\r
- combo21 = cpara[8] * rot1+ cpara[9] * rot4+ cpara[10] * rot7;\r
- //\r
- err = 0.0;\r
- h = combo20 * vertex[0]+ combo21 * vertex[1]+ combo22_2;\r
- x = pos2d[0] - (combo00 * vertex[0]+ combo01 * vertex[1]+ combo02_2) / h;\r
- y = pos2d[1] - (combo10 * vertex[0]+ combo11 * vertex[1]+ combo12_2) / h;\r
- err += x*x+y*y;\r
- h = combo20 * vertex[3]+ combo21 * vertex[4]+ combo22_5;\r
- x = pos2d[2] - (combo00 * vertex[3]+ combo01 * vertex[4]+ combo02_5) / h;\r
- y = pos2d[3] - (combo10 * vertex[3]+ combo11 * vertex[4]+ combo12_5) / h;\r
- err += x*x+y*y;\r
- h = combo20 * vertex[6]+ combo21 * vertex[7]+ combo22_8;\r
- x = pos2d[4] - (combo00 * vertex[6]+ combo01 * vertex[7]+ combo02_8) / h;\r
- y = pos2d[5] - (combo10 * vertex[6]+ combo11 * vertex[7]+ combo12_8) / h;\r
- err += x*x+y*y;\r
- h = combo20 * vertex[9]+ combo21 * vertex[10]+ combo22_11;\r
- x = pos2d[6] - (combo00 * vertex[9]+ combo01 * vertex[10]+ combo02_11) / h;\r
- y = pos2d[7] - (combo10 * vertex[9]+ combo11 * vertex[10]+ combo12_11) / h;\r
- err += x*x+y*y;\r
+ combo01 = CP0 * rot1+ CP1 * rot4+ CP2 * rot7;\r
+ combo11 = CP4 * rot1+ CP5 * rot4+ CP6 * rot7;\r
+ combo21 = CP8 * rot1+ CP9 * rot4+ CP10 * rot7;\r
+ //\r
+ err = 0.0;\r
+ h = combo20 * VX00+ combo21 * VX01+ combo22_2;\r
+ x = P2D00 - (combo00 * VX00+ combo01 * VX01+ combo02_2) / h;\r
+ y = P2D01 - (combo10 * VX00+ combo11 * VX01+ combo12_2) / h;\r
+ err += x*x+y*y;\r
+ h = combo20 * VX10+ combo21 * VX11+ combo22_5;\r
+ x = P2D10 - (combo00 * VX10+ combo01 * VX11+ combo02_5) / h;\r
+ y = P2D11 - (combo10 * VX10+ combo11 * VX11+ combo12_5) / h;\r
+ err += x*x+y*y;\r
+ h = combo20 * VX20+ combo21 * VX21+ combo22_8;\r
+ x = P2D20 - (combo00 * VX20+ combo01 * VX21+ combo02_8) / h;\r
+ y = P2D21 - (combo10 * VX20+ combo11 * VX21+ combo12_8) / h;\r
+ err += x*x+y*y;\r
+ h = combo20 * VX30+ combo21 * VX31+ combo22_11;\r
+ x = P2D30 - (combo00 * VX30+ combo01 * VX31+ combo02_11) / h;\r
+ y = P2D31 - (combo10 * VX30+ combo11 * VX31+ combo12_11) / h;\r
+ err += x*x+y*y;\r
if( err < minerr ) {\r
minerr = err;\r
- ma = a1;\r
- mb = b1;\r
- mc = c1;\r
- s1 = t1;\r
- s2 = t2;\r
- s3 = t3;\r
+ ma = a_factor[t1];\r
+ mb = b_factor[t2];\r
+ mc = c_factor[t3];\r
+ s1 = t1-1;\r
+ s2 = t2-1;\r
+ s3 = t3-1;\r
}\r
}\r
}\r
* pattern_match関数を分解した３種類のパターン検出クラスを定義します。\r
*\r
*/\r
-interface ARMatchPatt{\r
+public interface NyARMatchPatt{\r
public double getConfidence();\r
public int getDirection();\r
public void evaluate(NyARCode i_code);\r
* マーカーを評価します。\r
*\r
*/\r
-public class NyARMatchPatt_BlackWhite implements ARMatchPatt{\r
+public class NyARMatchPatt_BlackWhite implements NyARMatchPatt{\r
private double datapow;\r
private int width;\r
private int height;\r
* マーカーを評価します。\r
*\r
*/\r
-public class NyARMatchPatt_Color_WITHOUT_PCA implements ARMatchPatt{\r
+public class NyARMatchPatt_Color_WITHOUT_PCA implements NyARMatchPatt{\r
private int[][][] input=new int[1][1][3];\r
private double datapow;\r
\r
* マーカーを評価します。\r
*\r
*/\r
-public class NyARMatchPatt_Color_WITH_PCA implements ARMatchPatt{\r
+public class NyARMatchPatt_Color_WITH_PCA implements NyARMatchPatt{\r
private final int EVEC_MAX=10;//#define EVEC_MAX 10\r
private int evec_dim;//static int evec_dim;\r
private int[][][] input;\r
{\r
//解析オブジェクトを作る\r
this.square=new NyARDetectSquare(i_param);\r
- this.transmat=new NyARTransMat(i_param);\r
+ this.transmat=new NyARTransMat_O2(i_param);\r
//比較コードを保存\r
this.code=i_code;\r
this.marker_width=i_marker_width;\r
}\r
public void Test_arDetectMarkerLite() throws Exception\r
{\r
- //AR用カメラパラメタファイルをロード\r
- NyARParam ap =new NyARParam();\r
- ap.loadFromARFile(camera_file);\r
- ap.changeSize(320,240);\r
- \r
- //AR用のパターンコードを読み出し \r
- NyARCode code=new NyARCode(16,16);\r
- code.loadFromARFile(code_file);\r
- \r
- //試験イメージの読み出し(320x240 BGRAのRAWデータ)\r
- File f=new File(data_file);\r
- FileInputStream fs=new FileInputStream(data_file);\r
- byte[] buf=new byte[(int)f.length()];\r
- fs.read(buf);\r
- NyARRaster_BGRA ra=NyARRaster_BGRA.wrap(buf, 320, 240);\r
- // Blank_Raster ra=new Blank_Raster(320, 240);\r
- \r
- //１パターンのみを追跡するクラスを作成\r
- NyARSingleDetectMarker ar=new NyARSingleDetectMarker(ap,code,80.0);\r
- ar.detectMarkerLite(ra,100);\r
- ar.getTransmationMatrix();\r
- \r
- //マーカーを検出\r
- double[][] tm;\r
- Date d2=new Date();\r
- for(int i=0;i<1000;i++){\r
- //変換行列を取得\r
- ar.detectMarkerLite(ra,100);\r
- ar.getTransmationMatrix();\r
- }\r
- Date d=new Date();\r
- tm=null;\r
- System.out.println(d.getTime()-d2.getTime()); \r
+ //AR用カメラパラメタファイルをロード\r
+ NyARParam ap =new NyARParam();\r
+ ap.loadFromARFile(camera_file);\r
+ ap.changeSize(320,240);\r
+\r
+ //AR用のパターンコードを読み出し \r
+ NyARCode code=new NyARCode(16,16);\r
+ code.loadFromARFile(code_file);\r
+\r
+ //試験イメージの読み出し(320x240 BGRAのRAWデータ)\r
+ File f=new File(data_file);\r
+ FileInputStream fs=new FileInputStream(data_file);\r
+ byte[] buf=new byte[(int)f.length()];\r
+ fs.read(buf);\r
+ NyARRaster_BGRA ra=NyARRaster_BGRA.wrap(buf, 320, 240);\r
+ // Blank_Raster ra=new Blank_Raster(320, 240);\r
+\r
+ //１パターンのみを追跡するクラスを作成\r
+ NyARSingleDetectMarker ar=new NyARSingleDetectMarker(ap,code,80.0);\r
+ ar.detectMarkerLite(ra,100);\r
+ ar.getTransmationMatrix();\r
+\r
+ //マーカーを検出\r
+ double[][] tm;\r
+ Date d2=new Date();\r
+ for(int i=0;i<1000;i++){\r
+ //変換行列を取得\r
+ ar.detectMarkerLite(ra,100);\r
+ ar.getTransmationMatrix();\r
+ }\r
+ Date d=new Date();\r
+ tm=null;\r
+ System.out.println(d.getTime()-d2.getTime()); \r
}\r
public static void main(String[] args)\r
{\r
- \r
- try{\r
- RawFileTest t=new RawFileTest();\r
- //t.Test_arGetVersion();\r
- t.Test_arDetectMarkerLite();\r
- }catch(Exception e){\r
- e.printStackTrace();\r
- }\r
+\r
+ try{\r
+ RawFileTest t=new RawFileTest();\r
+ //t.Test_arGetVersion();\r
+ t.Test_arDetectMarkerLite();\r
+ }catch(Exception e){\r
+ e.printStackTrace();\r
+ }\r
}\r
- \r
+\r
}\r
OSDN Git Service
