1 package jp.nyatla.nyartoolkit.base;
\r
3 import jp.nyatla.nyartoolkit.NyARException;
\r
4 import jp.nyatla.nyartoolkit.core.NyARMat;
\r
10 private static final int AR_PARAM_NMIN=6;//#define AR_PARAM_NMIN 6
\r
11 private static final int AR_PARAM_NMAX=1000;//#define AR_PARAM_NMAX 1000
\r
12 private static final double AR_PARAM_C34=100.0;//#define AR_PARAM_C34 100.0
\r
19 double matL2R[3][4];
\r
20 double dist_factorL[4];
\r
21 double dist_factorR[4];
\r
23 /* static class ARSParam{
\r
25 Double2dArray matL=new Double2dArray(3,4);
\r
26 Double2dArray matR=new Double2dArray(3,4);
\r
27 Double2dArray matL2R=new Double2dArray(3,4);
\r
28 double[] dist_factorL=new double[4];
\r
29 double[] dist_factorR=new double[4];
\r
31 private static final int arParamGet_AR_PARAM_CDMIN = 12;
\r
32 /*int arParamGet( double global[][3], double screen[][2], int data_num,double mat[3][4] );*/
\r
33 public static int arParamGet( double global[][], double[][] screen, int num,double[][] mat) throws NyARException
\r
35 NyARMat mat_a, mat_at, mat_r;//ARMat *mat_a, *mat_at, *mat_r, mat_cpara;
\r
36 NyARMat mat_wm1,mat_wm2;//ARMat *mat_wm1, *mat_wm2;
\r
39 if(num < AR_PARAM_NMIN){
\r
42 if(num > AR_PARAM_NMAX){
\r
45 NyARException.trap("未チェックのパス");{
\r
46 mat_a = new NyARMat(2*num,arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
\r
47 mat_at =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
\r
48 mat_r = new NyARMat(2*num,1);//mat_r = arMatrixAlloc( 2*num, 1 );
\r
49 mat_wm1 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
\r
50 mat_wm2 =new NyARMat(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
\r
53 mat_a = Matrix.arMatrixAlloc( 2*num, arParamGet_AR_PARAM_CDMIN-1 );//mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
\r
57 mat_at =Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
\r
61 mat_r = Matrix.arMatrixAlloc( 2*num, 1 );//mat_r = arMatrixAlloc( 2*num, 1 );
\r
65 mat_wm1 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, arParamGet_AR_PARAM_CDMIN-1 );//mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
\r
66 if( mat_wm1 == null) {
\r
69 mat_wm2 = Matrix.arMatrixAlloc(arParamGet_AR_PARAM_CDMIN-1, 2*num );//mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
\r
70 if( mat_wm2 == null ) {
\r
74 /* Initializing array */
\r
75 mat_a.zeroClear();//Javaではゼロクリアされるので不要
\r
76 // pa1=mat_a.getPointer();//pa1 = mat_a->m;
\r
77 // for(i = 0; i < 2 * num * (arParamGet_AR_PARAM_CDMIN-1); i++){
\r
82 double[][] pa1 =mat_a.getArray();
\r
83 double[][] pa2 =mat_a.getArray();
\r
84 /* Calculate A,R matrix */
\r
85 double[][] pr=mat_r.getArray();
\r
87 for(i = 0; i < num; i++) {//for(i = 0, pr = mat_r->m; i < num; i++) {
\r
88 int pa1_ptr_row =2*i; //pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1)
\r
89 int pa2_ptr_row =2*i+1;//pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]);
\r
92 // pa1=mat_a.getPointer((2*i)*(arParamGet_AR_PARAM_CDMIN-1)); ]);
\r
93 // pa2=mat_a.getPointer((2*i+1)*(arParamGet_AR_PARAM_CDMIN-1) + 4);
\r
94 //*pa1++ = global[i][0]; *pa1++ = global[i][1];
\r
95 pa1[pa1_ptr_row][ 0]=global[i][0];
\r
96 pa1[pa1_ptr_row][ 1]=global[i][1];
\r
97 //*pa1++ = global[i][2]; *pa1++ = 1.0;
\r
98 pa1[pa1_ptr_row][ 2]=global[i][2];
\r
99 pa1[pa1_ptr_row][ 3]=1.0;
\r
100 //*pa2++ = global[i][0]; *pa2++ = global[i][1];
\r
101 pa2[pa2_ptr_row][ 4]=global[i][0];
\r
102 pa2[pa2_ptr_row][ 5]=global[i][1];
\r
103 //*pa2++ = global[i][2]; *pa2++ = 1.0;
\r
104 pa2[pa2_ptr_row][ 6]=global[i][2];
\r
105 pa2[pa2_ptr_row][ 7]=1.0;
\r
107 //*pa1++ = -global[i][0] * screen[i][0];
\r
108 pa1[pa1_ptr_row][ 8]=-global[i][0] * screen[i][0];
\r
109 //*pa1++ = -global[i][1] * screen[i][0];
\r
110 pa1[pa1_ptr_row][ 9]=-global[i][1] * screen[i][0];
\r
111 //*pa1 = -global[i][2] * screen[i][0];
\r
112 pa1[pa1_ptr_row][10]=-global[i][2]* screen[i][0];
\r
113 //*pa2++ = -global[i][0] * screen[i][1];
\r
114 pa2[pa2_ptr_row][ 8]=-global[i][0] * screen[i][1];
\r
115 //*pa2++ = -global[i][1] * screen[i][1];
\r
116 pa2[pa2_ptr_row][ 9]=-global[i][1] * screen[i][1];
\r
117 //*pa2 = -global[i][2] * screen[i][1];
\r
118 pa2[pa2_ptr_row][10]=-global[i][2] * screen[i][1];
\r
119 //*pr++ = screen[i][0] * AR_PARAM_C34;
\r
120 pr[0][pr_ptr_col]=screen[i][0] * AR_PARAM_C34;pr_ptr_col++;
\r
121 //*pr++ = screen[i][1] * AR_PARAM_C34;
\r
122 pr[0][pr_ptr_col]=screen[i][1] * AR_PARAM_C34;pr_ptr_col++;
\r
125 NyARException.trap("未チェックのパス");
\r
126 NyARMat.matrixTrans(mat_at, mat_a );//if( arMatrixTrans( mat_at, mat_a ) < 0 ){
\r
128 NyARException.trap("未チェックのパス");
\r
129 mat_wm1.matrixMul(mat_at, mat_a );//if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) {
\r
130 NyARException.trap("未チェックのパス");
\r
131 mat_wm1.matrixSelfInv();//if( arMatrixSelfInv( mat_wm1 ) < 0 ) {
\r
133 NyARException.trap("未チェックのパス");
\r
134 mat_wm2.matrixMul(mat_wm1, mat_at );//if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) {
\r
136 //mat_cpara.row = AR_PARAM_CDMIN-1;//mat_cpara.row = AR_PARAM_CDMIN-1;
\r
137 //mat_cpara.clm = 1;
\r
138 //mat_cpara.m = &(mat[0][0]);
\r
139 /*1次元行列から3x4行列に転写。高負荷なところじゃないから地道に転写でOK*/
\r
140 NyARMat mat_cpara=new NyARMat(arParamGet_AR_PARAM_CDMIN-1,1);
\r
141 double[][] mat_cpara_array=mat_cpara.getArray();
\r
142 NyARException.trap("未チェックのパス");
\r
143 mat_cpara.matrixMul(mat_wm2, mat_r );//if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) {
\r
145 for(int i2=0;i<3;i++){
\r
146 for(int i3=0;i3<4;i3++){
\r
147 mat[i2][i3]=mat_cpara_array[i2*4+i3][0];
\r
150 //ARMat.wrap(mat.array(),arParamGet_AR_PARAM_CDMIN-1,1);}
\r
152 mat[2][3]=AR_PARAM_C34;//mat[2][3] = AR_PARAM_C34;
\r
157 /* //int arsParamChangeSize( ARSParam *source, int xsize, int ysize, ARSParam *newparam );
\r
158 public static int arsParamChangeSize( ARSParam source, int xsize, int ysize, ARSParam newparam)
\r
162 newparam.xsize=xsize;//newparam->xsize = xsize;
\r
163 newparam.ysize=ysize;//newparam->ysize = ysize;
\r
165 scale=(double)xsize/ (double)(source.xsize);//scale = (double)xsize / (double)(source->xsize);
\r
166 for(int i = 0; i < 4; i++ ){
\r
167 newparam.matL.set(0,i,source.matL.get(0,i)*scale);//newparam->matL[0][i] = source->matL[0][i] * scale;
\r
168 newparam.matL.set(1,i,source.matL.get(1,i)*scale);//newparam->matL[1][i] = source->matL[1][i] * scale;
\r
169 newparam.matL.set(2,i,source.matL.get(2,i));//newparam->matL[2][i] = source->matL[2][i];
\r
171 for(int i = 0; i < 4; i++ ) {
\r
172 newparam.matR.set(0,i,source.matR.get(0,i)*scale);//newparam->matR[0][i] = source->matR[0][i] * scale;
\r
173 newparam.matR.set(1,i,source.matR.get(1,i)*scale);//newparam->matR[1][i] = source->matR[1][i] * scale;
\r
174 newparam.matR.set(2,i,source.matR.get(2,i));//newparam->matR[2][i] = source->matR[2][i];
\r
176 for(int i = 0; i < 4; i++ ) {
\r
177 newparam.matL2R.set(0,i,source.matL2R.get(0,i));//newparam->matL2R[0][i] = source->matL2R[0][i];
\r
178 newparam.matL2R.set(1,i,source.matL2R.get(1,i));//newparam->matL2R[1][i] = source->matL2R[1][i];
\r
179 newparam.matL2R.set(2,i,source.matL2R.get(2,i));//newparam->matL2R[2][i] = source->matL2R[2][i];
\r
182 newparam.dist_factorL[0] = source.dist_factorL[0] * scale;//newparam->dist_factorL[0] = source->dist_factorL[0] * scale;
\r
183 newparam.dist_factorL[1] = source.dist_factorL[1] * scale;//newparam->dist_factorL[1] = source->dist_factorL[1] * scale;
\r
184 newparam.dist_factorL[2] = source.dist_factorL[2] / (scale*scale);//newparam->dist_factorL[2] = source->dist_factorL[2] / (scale*scale);
\r
185 newparam.dist_factorL[3] = source.dist_factorL[3];//newparam->dist_factorL[3] = source->dist_factorL[3];
\r
187 newparam.dist_factorR[0] = source.dist_factorR[0] * scale;//newparam->dist_factorR[0] = source->dist_factorR[0] * scale;
\r
188 newparam.dist_factorR[1] = source.dist_factorR[1] * scale;//newparam->dist_factorR[1] = source->dist_factorR[1] * scale;
\r
189 newparam.dist_factorR[2] = source.dist_factorR[2] / (scale*scale);//newparam->dist_factorR[2] = source->dist_factorR[2] / (scale*scale);
\r
190 newparam.dist_factorR[3] = source.dist_factorR[3];//newparam->dist_factorR[3] = source->dist_factorR[3];
\r
194 /* //int arsParamSave( char *filename, ARSParam *sparam );
\r
195 public static int arsParamSave(String filename, ARSParam sparam) throws Exception
\r
197 //int xsize, ysize;
\r
198 //Double2dArray matL=new Double2dArray(3,4);
\r
199 //Double2dArray matR=new Double2dArray(3,4);
\r
200 //Double2dArray matL2R=new Double2dArray(3,4);
\r
201 //double dist_factorL[]=new double[4];
\r
202 //double dist_factorR[]=new double[4];
\r
204 byte[] buf=new byte[(4+4+(3*4*8)*3+(4*8)*2)];
\r
207 ByteBuffer bb = ByteBuffer.wrap(buf);
\r
208 bb.order(ByteOrder.BIG_ENDIAN);
\r
211 bb.putInt(sparam.xsize);
\r
212 bb.putInt(sparam.ysize);
\r
213 for(int i=0;i<3;i++){
\r
214 for(int i2=0;i2<4;i2++){
\r
215 bb.putDouble(sparam.matL.get(i, i2));
\r
218 for(int i=0;i<3;i++){
\r
219 for(int i2=0;i2<4;i2++){
\r
220 bb.putDouble(sparam.matR.get(i, i2));
\r
223 for(int i=0;i<3;i++){
\r
224 for(int i2=0;i2<4;i2++){
\r
225 bb.putDouble(sparam.matL2R.get(i, i2));
\r
228 for(int i=0;i<4;i++){
\r
229 bb.putDouble(sparam.dist_factorL[i]);
\r
231 for(int i=0;i<4;i++){
\r
232 bb.putDouble(sparam.dist_factorR[i]);
\r
235 FileOutputStream fs=new FileOutputStream(filename);
\r
241 /* //int arsParamLoad( char *filename, ARSParam *sparam );
\r
242 public static int arsParamLoad(String filename, ARSParam sparam ) throws Exception
\r
245 FileInputStream fs=new FileInputStream(filename);
\r
246 File f=new File(filename);
\r
247 int file_size=(int)f.length();
\r
248 byte[] buf=new byte[file_size];
\r
253 ByteBuffer bb = ByteBuffer.wrap(buf);
\r
254 bb.order(ByteOrder.BIG_ENDIAN);
\r
257 sparam.xsize=bb.getInt();
\r
258 sparam.ysize=bb.getInt();
\r
259 for(int i=0;i<3;i++){
\r
260 for(int i2=0;i2<4;i2++){
\r
261 sparam.matL.set(i,i2,bb.getDouble());
\r
264 for(int i=0;i<3;i++){
\r
265 for(int i2=0;i2<4;i2++){
\r
266 sparam.matR.set(i,i2,bb.getDouble());
\r
269 for(int i=0;i<3;i++){
\r
270 for(int i2=0;i2<4;i2++){
\r
271 sparam.matL2R.set(i,i2,bb.getDouble());
\r
274 for(int i=0;i<3;i++){
\r
275 sparam.dist_factorL[i]=bb.getDouble();
\r
277 for(int i=0;i<3;i++){
\r
278 sparam.dist_factorR[i]=bb.getDouble();
\r
283 // int arsParamGetMat( double matL[3][4], double matR[3][4],double cparaL[3][4], double cparaR[3][4], double matL2R[3][4] );
\r
284 public static int arsParamGetMat(double[][] matL, double[][] matR,double[][] cparaL, double[][] cparaR, double[][] matL2R) throws JartkException
\r
286 ARMat t1,t2,t3;//ARMat *t1, *t2, *t3;
\r
287 //double transL[3][4], transR[3][4];
\r
288 Double2dArray transL=new Double2dArray(3,4);
\r
289 Double2dArray transR=new Double2dArray(3,4);
\r
291 arParamDecompMat( matL,cparaL,transL);
\r
292 arParamDecompMat( matR,cparaR,transR);
\r
293 JartkException.trap("未チェックパス");{
\r
294 t1=new ARMat(4,4);//t1 = arMatrixAlloc( 4, 4 );
\r
295 t2=new ARMat(4,4);//t2 = arMatrixAlloc( 4, 4 );
\r
297 double[][] t1_array=t1.getArray();
\r
298 double[][] t2_array=t2.getArray();
\r
299 for(int j = 0; j < 3; j++ ){
\r
300 for(int i = 0; i < 4; i++ ) {
\r
301 JartkException.trap("未チェックパス");{
\r
302 t1_array[j][i]=transL.get(j,i);//t1->m[j*4+i] = transL[j][i];
\r
303 t2_array[j][i]=transL.get(j,i);//t2->m[j*4+i] = transR[j][i];
\r
307 JartkException.trap("未チェックパス");{
\r
308 t1_array[3][0]=t1_array[3][1]=t1_array[3][2]=0.0;//t1->m[12] = t1->m[13] = t1->m[14] = 0.0;
\r
309 t1_array[3][3]=1.0;//t1->m[15] = 1.0;
\r
310 t2_array[3][0]=t2_array[3][1]=t2_array[3][2]=0.0;//t2->m[12] = t2->m[13] = t2->m[14] = 0.0;
\r
311 t2_array[3][3]=1.0;//t2->m[15] = 1.0;
\r
313 JartkException.trap("未チェックのパス");
\r
314 t1.matrixSelfInv();//if( arMatrixSelfInv(t1) != 0 ) {
\r
316 JartkException.trap("未チェックのパス");
\r
317 t3 =ARMat.matrixAllocMul(t2, t1);//t3 = arMatrixAllocMul(t2, t1);
\r
318 double[][] t3_array=t3.getArray();
\r
323 for(int j = 0; j < 3; j++ ) {
\r
324 for(int i = 0; i < 4; i++ ) {
\r
325 JartkException.trap("未チェックパス");
\r
326 matL2R[j][i]=t3_array[j][i];//matL2R[j][i] = t3->m[j*4+i];
\r
331 */ //int arsParamDisp( ARSParam *sparam )
\r
332 /* public static int arsParamDisp( ARSParam sparam)
\r
334 System.out.println("--------------------------------------");//printf("--------------------------------------\n");
\r
335 System.out.println("SIZE = "+sparam.xsize+", "+sparam.ysize);// printf("SIZE = %d, %d\n", sparam->xsize, sparam->ysize);
\r
336 System.out.println("-- Left --");//printf("-- Left --\n");
\r
337 System.out.println("Distotion factor = "+sparam.dist_factorL[0]+" "+sparam.dist_factorL[1]+" "+sparam.dist_factorL[2]+" "+sparam.dist_factorL[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorL[0],sparam->dist_factorL[1], sparam->dist_factorL[2], sparam->dist_factorL[3] );
\r
338 for(int j = 0; j < 3; j++ ) {
\r
339 for(int i = 0; i < 4; i++ ){
\r
340 System.out.print(sparam.matL.get(j,i)+" ");//printf("%7.5f ", sparam->matL[j][i]);
\r
342 System.out.println();//printf("\n");
\r
345 System.out.println("-- Right --");//printf("-- Right --\n");
\r
346 System.out.println("Distotion factor = "+sparam.dist_factorR[0]+" "+sparam.dist_factorR[1]+" "+sparam.dist_factorR[2]+" "+sparam.dist_factorR[3]);//printf("Distotion factor = %f %f %f %f\n", sparam->dist_factorR[0],sparam->dist_factorR[1], sparam->dist_factorR[2], sparam->dist_factorR[3]);
\r
347 for(int j = 0; j < 3; j++ ){
\r
348 for(int i = 0; i < 4; i++ ){
\r
349 System.out.println(sparam.matR.get(j,i)+" ");//printf("%7.5f ", sparam->matR[j][i]);
\r
351 System.out.println();//printf("\n");
\r
354 System.out.println("-- Left => Right --");//printf("-- Left => Right --\n");
\r
355 for(int j = 0; j < 3; j++ ) {
\r
356 for(int i = 0; i < 4; i++ ){
\r
357 //printf("%7.5f ", sparam->matL2R[j][i]);
\r
359 System.out.println();//printf("\n");
\r
362 System.out.println("--------------------------------------");//printf("--------------------------------------\n");
\r