static void draw_line2( double *x, double *y, int num ) { ARMat *input, *evec; ARVec *ev, *mean; double a, b, c; int i; ev = arVecAlloc( 2 ); mean = arVecAlloc( 2 ); evec = arMatrixAlloc( 2, 2 ); input = arMatrixAlloc( num, 2 ); for( i = 0; i < num; i++ ) { arParamObserv2Ideal( dist_factor, x[i], y[i], &(input->m[i*2+0]), &(input->m[i*2+1]) ); } if( arMatrixPCA(input, evec, ev, mean) < 0 ) exit(0); a = evec->m[1]; b = -evec->m[0]; c = -(a*mean->v[0] + b*mean->v[1]); arMatrixFree( input ); arMatrixFree( evec ); arVecFree( mean ); arVecFree( ev ); draw_warp_line(a, b, c); }
static int arGetLine2(int x_coord[], int y_coord[], int coord_num, int vertex[], double line[4][3], double v[4][2], double *dist_factor) { ARMat *input, *evec; ARVec *ev, *mean; double w1; int st, ed, n; int i, j; ev = arVecAlloc( 2 ); mean = arVecAlloc( 2 ); evec = arMatrixAlloc( 2, 2 ); for( i = 0; i < 4; i++ ) { w1 = (double)(vertex[i+1]-vertex[i]+1) * 0.05 + 0.5; st = (int)(vertex[i] + w1); ed = (int)(vertex[i+1] - w1); n = ed - st + 1; input = arMatrixAlloc( n, 2 ); for( j = 0; j < n; j++ ) { arParamObserv2Ideal( dist_factor, x_coord[st+j], y_coord[st+j], &(input->m[j*2+0]), &(input->m[j*2+1]) ); } if( arMatrixPCA(input, evec, ev, mean) < 0 ) { arMatrixFree( input ); arMatrixFree( evec ); arVecFree( mean ); arVecFree( ev ); return(-1); } line[i][0] = evec->m[1]; line[i][1] = -evec->m[0]; line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]); arMatrixFree( input ); } arMatrixFree( evec ); arVecFree( mean ); arVecFree( ev ); for( i = 0; i < 4; i++ ) { w1 = line[(i+3)%4][0] * line[i][1] - line[i][0] * line[(i+3)%4][1]; if( w1 == 0.0 ) return(-1); v[i][0] = ( line[(i+3)%4][1] * line[i][2] - line[i][1] * line[(i+3)%4][2] ) / w1; v[i][1] = ( line[i][0] * line[(i+3)%4][2] - line[(i+3)%4][0] * line[i][2] ) / w1; } return(0); }
/* find the position of the paddle card relative to the base and set the dropped blob position to this */ static void findPaddlePosition(float curPaddlePos[], double card_trans[3][4],double base_trans[3][4]) { int i,j; ARMat *mat_a, *mat_b, *mat_c; double x, y, z; //get card position relative to base pattern mat_a = arMatrixAlloc( 4, 4 ); mat_b = arMatrixAlloc( 4, 4 ); mat_c = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_b->m[j*4+i] = base_trans[j][i]; } } mat_b->m[3*4+0] = 0.0; mat_b->m[3*4+1] = 0.0; mat_b->m[3*4+2] = 0.0; mat_b->m[3*4+3] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat_a->m[j*4+i] = card_trans[j][i]; } } mat_a->m[3*4+0] = 0.0; mat_a->m[3*4+1] = 0.0; mat_a->m[3*4+2] = 0.0; mat_a->m[3*4+3] = 1.0; arMatrixSelfInv( mat_b ); arMatrixMul( mat_c, mat_b, mat_a ); //x,y,z is card position relative to base pattern x = mat_c->m[0*4+3]; y = mat_c->m[1*4+3]; z = mat_c->m[2*4+3]; curPaddlePos[0] = x; curPaddlePos[1] = y; curPaddlePos[2] = z; // printf("Position: %3.2f %3.2f %3.2f\n",x,y,z); arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); }
static void get_cpara( double world[4][2], double vertex[4][2], double para[3][3] ) { ARMat *a, *b, *c; int i; a = arMatrixAlloc( 8, 8 ); b = arMatrixAlloc( 8, 1 ); c = arMatrixAlloc( 8, 1 ); for( i = 0; i < 4; i++ ) { a->m[i*16+0] = world[i][0]; a->m[i*16+1] = world[i][1]; a->m[i*16+2] = 1.0; a->m[i*16+3] = 0.0; a->m[i*16+4] = 0.0; a->m[i*16+5] = 0.0; a->m[i*16+6] = -world[i][0] * vertex[i][0]; a->m[i*16+7] = -world[i][1] * vertex[i][0]; a->m[i*16+8] = 0.0; a->m[i*16+9] = 0.0; a->m[i*16+10] = 0.0; a->m[i*16+11] = world[i][0]; a->m[i*16+12] = world[i][1]; a->m[i*16+13] = 1.0; a->m[i*16+14] = -world[i][0] * vertex[i][1]; a->m[i*16+15] = -world[i][1] * vertex[i][1]; b->m[i*2+0] = vertex[i][0]; b->m[i*2+1] = vertex[i][1]; } arMatrixSelfInv( a ); arMatrixMul( c, a, b ); for( i = 0; i < 2; i++ ) { para[i][0] = c->m[i*3+0]; para[i][1] = c->m[i*3+1]; para[i][2] = c->m[i*3+2]; } para[2][0] = c->m[2*3+0]; para[2][1] = c->m[2*3+1]; para[2][2] = 1.0; arMatrixFree( a ); arMatrixFree( b ); arMatrixFree( c ); }
ARMat *arMatrixAllocInv(ARMat *source) { ARMat *dest; dest = arMatrixAlloc(source->row, source->row); if( dest == NULL ) return NULL; if( arMatrixInv(dest, source) < 0 ) { arMatrixFree( dest ); return NULL; } return dest; }
ARMat *arMatrixAllocTrans(ARMat *source) { ARMat *dest; dest = arMatrixAlloc(source->clm, source->row); if( dest == NULL ) return NULL; if( arMatrixTrans(dest, source) < 0 ) { arMatrixFree(dest); return NULL; } return dest; }
AR_DLL_API ARMat *arMatrixAllocUnit(int dim) { ARMat *m; m = arMatrixAlloc(dim, dim); if( m == NULL ) return NULL; if( arMatrixUnit(m) < 0 ) { arMatrixFree(m); return NULL; } return m; }
static int PCA( ARMat *input, ARMat *output, ARVec *ev ) { ARMat *u; ARdouble *m1, *m2; int row, clm, min; int i, j; row = input->row; clm = input->clm; min = (clm < row)? clm: row; if( row < 2 || clm < 2 ) return(-1); if( output->clm != input->clm ) return(-1); if( output->row != min ) return(-1); if( ev->clm != min ) return(-1); u = arMatrixAlloc( min, min ); if( u->row != min || u->clm != min ) return(-1); if( row < clm ) { if( x_by_xt( input, u ) < 0 ) { arMatrixFree(u); return(-1); } } else { if( xt_by_x( input, u ) < 0 ) { arMatrixFree(u); return(-1); } } if( QRM( u, ev ) < 0 ) { arMatrixFree(u); return(-1); } if( row < clm ) { if( EV_create( input, u, output, ev ) < 0 ) { arMatrixFree(u); return(-1); } } else{ m1 = u->m; m2 = output->m; for( i = 0; i < min; i++){ if( ev->v[i] < VZERO ) break; for( j = 0; j < min; j++ ) *(m2++) = *(m1++); } for( ; i < min; i++){ ev->v[i] = 0.0; for( j = 0; j < min; j++ ) *(m2++) = 0.0; } } arMatrixFree(u); return( 0 ); }
static double check_error( double *x, double *y, int num, double dist_factor[4] ) { ARMat *input, *evec; ARVec *ev, *mean; double a, b, c; double error; int i; ev = arVecAlloc( 2 ); mean = arVecAlloc( 2 ); evec = arMatrixAlloc( 2, 2 ); input = arMatrixAlloc( num, 2 ); for( i = 0; i < num; i++ ) { arParamObserv2Ideal( dist_factor, x[i], y[i], &(input->m[i*2+0]), &(input->m[i*2+1]) ); } if( arMatrixPCA(input, evec, ev, mean) < 0 ) exit(0); a = evec->m[1]; b = -evec->m[0]; c = -(a*mean->v[0] + b*mean->v[1]); error = 0.0; for( i = 0; i < num; i++ ) { error += (a*input->m[i*2+0] + b*input->m[i*2+1] + c) * (a*input->m[i*2+0] + b*input->m[i*2+1] + c); } error /= (a*a + b*b); arMatrixFree( input ); arMatrixFree( evec ); arVecFree( mean ); arVecFree( ev ); return error; }
int arUtilMatInv( double s[3][4], double d[3][4] ) { ARMat *mat; int i, j; mat = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat->m[j*4+i] = s[j][i]; } } mat->m[3*4+0] = 0; mat->m[3*4+1] = 0; mat->m[3*4+2] = 0; mat->m[3*4+3] = 1; arMatrixSelfInv( mat ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { d[j][i] = mat->m[j*4+i]; } } arMatrixFree( mat ); return 0; }
int arUtilMatInvf( const float s[3][4], float d[3][4] ) { ARMat *mat; int i, j; mat = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { mat->m[j*4+i] = (ARdouble)s[j][i]; } } mat->m[3*4+0] = 0; mat->m[3*4+1] = 0; mat->m[3*4+2] = 0; mat->m[3*4+3] = 1; arMatrixSelfInv( mat ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { d[j][i] = (float)mat->m[j*4+i]; } } arMatrixFree( mat ); return 0; }
ARdouble arGetStereoMatchingError( AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2] ) { ARMat *cL, *cR, *cLi, *cRi; ARMat *tL, *tR, *tLi, *tRi; ARMat *w1, *w2, *w3; ARdouble q1, q2, q3, t1, t2, t3; ARdouble a1, b1, c1, a2, b2, c2; ARdouble lL2, lR2; int i, j; cL = arMatrixAlloc( 4, 4 ); cR = arMatrixAlloc( 4, 4 ); tL = arMatrixAlloc( 4, 4 ); tR = arMatrixAlloc( 4, 4 ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { cL->m[j*4+i] = handle->icpStereoHandle->matXcl2Ul[j][i]; } } cL->m[12] = cL->m[13] = cL->m[14] = 0.0; cL->m[15] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { cR->m[j*4+i] = handle->icpStereoHandle->matXcr2Ur[j][i]; } } cR->m[12] = cR->m[13] = cR->m[14] = 0.0; cR->m[15] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { tL->m[j*4+i] = handle->icpStereoHandle->matC2L[j][i]; } } tL->m[12] = tL->m[13] = tL->m[14] = 0.0; tL->m[15] = 1.0; for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) { tR->m[j*4+i] = handle->icpStereoHandle->matC2R[j][i]; } } tR->m[12] = tR->m[13] = tR->m[14] = 0.0; tR->m[15] = 1.0; cLi = arMatrixAllocInv( cL ); cRi = arMatrixAllocInv( cR ); tLi = arMatrixAllocInv( tL ); tRi = arMatrixAllocInv( tR ); w1 = arMatrixAllocMul( cR, tR ); w2 = arMatrixAllocMul( w1, tLi ); w3 = arMatrixAllocMul( w2, cLi ); q1 = w3->m[0*4+0]*pos2dL[0] + w3->m[0*4+1]*pos2dL[1] + w3->m[0*4+2]; q2 = w3->m[1*4+0]*pos2dL[0] + w3->m[1*4+1]*pos2dL[1] + w3->m[1*4+2]; q3 = w3->m[2*4+0]*pos2dL[0] + w3->m[2*4+1]*pos2dL[1] + w3->m[2*4+2]; t1 = w3->m[0*4+3]; t2 = w3->m[1*4+3]; t3 = w3->m[2*4+3]; a1 = q3*t2 - q2*t3; b1 = q1*t3 - q3*t1; c1 = q2*t1 - q1*t2; arMatrixMul( w1, cL, tL ); arMatrixMul( w2, w1, tRi ); arMatrixMul( w3, w2, cRi ); q1 = w3->m[0*4+0]*pos2dR[0] + w3->m[0*4+1]*pos2dR[1] + w3->m[0*4+2]; q2 = w3->m[1*4+0]*pos2dR[0] + w3->m[1*4+1]*pos2dR[1] + w3->m[1*4+2]; q3 = w3->m[2*4+0]*pos2dR[0] + w3->m[2*4+1]*pos2dR[1] + w3->m[2*4+2]; t1 = w3->m[0*4+3]; t2 = w3->m[1*4+3]; t3 = w3->m[2*4+3]; a2 = q3*t2 - q2*t3; b2 = q1*t3 - q3*t1; c2 = q2*t1 - q1*t2; arMatrixFree( w3 ); arMatrixFree( w2 ); arMatrixFree( w1 ); arMatrixFree( tL ); arMatrixFree( tR ); arMatrixFree( tLi ); arMatrixFree( tRi ); arMatrixFree( cR ); arMatrixFree( cL ); arMatrixFree( cRi ); arMatrixFree( cLi ); lL2 = (a1*pos2dR[0]+b1*pos2dR[1]+c1)*(a1*pos2dR[0]+b1*pos2dR[1]+c1) / (a1*a1+b1*b1); lR2 = (a2*pos2dL[0]+b2*pos2dL[1]+c2)*(a2*pos2dL[0]+b2*pos2dL[1]+c2) / (a2*a2+b2*b2); return (lL2 + lR2)/2.0; }
int arGetStereoMatching(AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2], ARdouble pos3d[3]) { ARMat *matP, *matQ; ARMat *matPt, *matR, *matS, *matT; ARdouble wL[3][4], wR[3][4]; arUtilMatMul( handle->icpStereoHandle->matXcl2Ul, handle->icpStereoHandle->matC2L, wL ); arUtilMatMul( handle->icpStereoHandle->matXcr2Ur, handle->icpStereoHandle->matC2R, wR ); matP = arMatrixAlloc(4,3); matPt = arMatrixAlloc(3,4); matQ = arMatrixAlloc(4,1); matR = arMatrixAlloc(3,3); matS = arMatrixAlloc(3,1); matT = arMatrixAlloc(3,1); matP->m[0*3+0] = matPt->m[0*4+0] = wL[0][0] - wL[2][0] * pos2dL[0]; matP->m[0*3+1] = matPt->m[1*4+0] = wL[0][1] - wL[2][1] * pos2dL[0]; matP->m[0*3+2] = matPt->m[2*4+0] = wL[0][2] - wL[2][2] * pos2dL[0]; matP->m[1*3+0] = matPt->m[0*4+1] = wL[1][0] - wL[2][0] * pos2dL[1]; matP->m[1*3+1] = matPt->m[1*4+1] = wL[1][1] - wL[2][1] * pos2dL[1]; matP->m[1*3+2] = matPt->m[2*4+1] = wL[1][2] - wL[2][2] * pos2dL[1]; matP->m[2*3+0] = matPt->m[0*4+2] = wR[0][0] - wR[2][0] * pos2dR[0]; matP->m[2*3+1] = matPt->m[1*4+2] = wR[0][1] - wR[2][1] * pos2dR[0]; matP->m[2*3+2] = matPt->m[2*4+2] = wR[0][2] - wR[2][2] * pos2dR[0]; matP->m[3*3+0] = matPt->m[0*4+3] = wR[1][0] - wR[2][0] * pos2dR[1]; matP->m[3*3+1] = matPt->m[1*4+3] = wR[1][1] - wR[2][1] * pos2dR[1]; matP->m[3*3+2] = matPt->m[2*4+3] = wR[1][2] - wR[2][2] * pos2dR[1]; matQ->m[0] = wL[2][3] * pos2dL[0] - wL[0][3]; matQ->m[1] = wL[2][3] * pos2dL[1] - wL[1][3]; matQ->m[2] = wR[2][3] * pos2dR[0] - wR[0][3]; matQ->m[3] = wR[2][3] * pos2dR[1] - wR[1][3]; arMatrixMul( matR, matPt, matP ); arMatrixMul( matS, matPt, matQ ); if( arMatrixSelfInv( matR ) < 0 ) { arMatrixFree( matP ); arMatrixFree( matPt); arMatrixFree( matQ ); arMatrixFree( matR ); arMatrixFree( matS ); arMatrixFree( matT ); return -1; } arMatrixMul( matT, matR, matS ); pos3d[0] = matT->m[0]; pos3d[1] = matT->m[1]; pos3d[2] = matT->m[2]; arMatrixFree( matP ); arMatrixFree( matPt); arMatrixFree( matQ ); arMatrixFree( matR ); arMatrixFree( matS ); arMatrixFree( matT ); return 0; }
int arParamGet( double global[][3], double screen[][2], int num, double mat[3][4] ) { ARMat *mat_a, *mat_at, *mat_r, mat_cpara; ARMat *mat_wm1, *mat_wm2; double *pa1, *pa2, *pr; /* working pointer */ int i; /* working variables */ if(num < AR_PARAM_NMIN) return( -1 ); if(num > AR_PARAM_NMAX) return( -1 ); mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 ); if( mat_a == NULL ) { return -1; } mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); if( mat_at == NULL ) { arMatrixFree( mat_a ); return -1; } mat_r = arMatrixAlloc( 2*num, 1 ); if( mat_r == NULL ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); return -1; } mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 ); if( mat_wm1 == NULL ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); return -1; } mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); if( mat_wm2 == NULL ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); return -1; } /* Initializing array */ pa1 = mat_a->m; for(i = 0; i < 2 * num * (AR_PARAM_CDMIN-1); i++) *pa1++ = 0.0; /* Calculate A,R matrix */ for(i = 0, pr = mat_r->m; i < num; i++) { pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1) ]); pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]); *pa1++ = global[i][0]; *pa1++ = global[i][1]; *pa1++ = global[i][2]; *pa1++ = 1.0; *pa2++ = global[i][0]; *pa2++ = global[i][1]; *pa2++ = global[i][2]; *pa2++ = 1.0; pa1 += 4; *pa1++ = -global[i][0] * screen[i][0]; *pa1++ = -global[i][1] * screen[i][0]; *pa1 = -global[i][2] * screen[i][0]; *pa2++ = -global[i][0] * screen[i][1]; *pa2++ = -global[i][1] * screen[i][1]; *pa2 = -global[i][2] * screen[i][1]; *pr++ = screen[i][0] * AR_PARAM_C34; *pr++ = screen[i][1] * AR_PARAM_C34; } if( arMatrixTrans( mat_at, mat_a ) < 0 ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return -1; } if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return -1; } if( arMatrixSelfInv( mat_wm1 ) < 0 ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return -1; } if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return -1; } mat_cpara.row = AR_PARAM_CDMIN-1; mat_cpara.clm = 1; mat_cpara.m = &(mat[0][0]); if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) { arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return -1; } mat[2][3] = AR_PARAM_C34; arMatrixFree( mat_a ); arMatrixFree( mat_at ); arMatrixFree( mat_r ); arMatrixFree( mat_wm1 ); arMatrixFree( mat_wm2 ); return 0; }
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] ) { ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f; double trans[3]; double wx, wy, wz; double ret; int i, j; mat_a = arMatrixAlloc( num*2, 3 ); mat_b = arMatrixAlloc( 3, num*2 ); mat_c = arMatrixAlloc( num*2, 1 ); mat_d = arMatrixAlloc( 3, 3 ); mat_e = arMatrixAlloc( 3, 1 ); mat_f = arMatrixAlloc( 3, 1 ); if( arFittingMode == AR_FITTING_TO_INPUT ) { for( i = 0; i < num; i++ ) { arParamIdeal2Observ(dist_factor, ppos2d[i][0], ppos2d[i][1], &pos2d[i][0], &pos2d[i][1]); } } else { for( i = 0; i < num; i++ ) { pos2d[i][0] = ppos2d[i][0]; pos2d[i][1] = ppos2d[i][1]; } } for( j = 0; j < num; j++ ) { wx = rot[0][0] * pos3d[j][0] + rot[0][1] * pos3d[j][1] + rot[0][2] * pos3d[j][2]; wy = rot[1][0] * pos3d[j][0] + rot[1][1] * pos3d[j][1] + rot[1][2] * pos3d[j][2]; wz = rot[2][0] * pos3d[j][0] + rot[2][1] * pos3d[j][1] + rot[2][2] * pos3d[j][2]; mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; mat_c->m[j*2+0] = wz * pos2d[j][0] - cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0; mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; mat_c->m[j*2+1] = wz * pos2d[j][1] - cpara[1][1]*wy - cpara[1][2]*wz; } arMatrixMul( mat_d, mat_b, mat_a ); arMatrixMul( mat_e, mat_b, mat_c ); arMatrixSelfInv( mat_d ); arMatrixMul( mat_f, mat_d, mat_e ); trans[0] = mat_f->m[0]; trans[1] = mat_f->m[1]; trans[2] = mat_f->m[2]; ret = arModifyMatrix( rot, trans, cpara, pos3d, pos2d, num ); for( j = 0; j < num; j++ ) { wx = rot[0][0] * pos3d[j][0] + rot[0][1] * pos3d[j][1] + rot[0][2] * pos3d[j][2]; wy = rot[1][0] * pos3d[j][0] + rot[1][1] * pos3d[j][1] + rot[1][2] * pos3d[j][2]; wz = rot[2][0] * pos3d[j][0] + rot[2][1] * pos3d[j][1] + rot[2][2] * pos3d[j][2]; mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = cpara[0][0]; mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1]; mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0]; mat_c->m[j*2+0] = wz * pos2d[j][0] - cpara[0][0]*wx - cpara[0][1]*wy - cpara[0][2]*wz; mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0; mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = cpara[1][1]; mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1]; mat_c->m[j*2+1] = wz * pos2d[j][1] - cpara[1][1]*wy - cpara[1][2]*wz; } arMatrixMul( mat_d, mat_b, mat_a ); arMatrixMul( mat_e, mat_b, mat_c ); arMatrixSelfInv( mat_d ); arMatrixMul( mat_f, mat_d, mat_e ); trans[0] = mat_f->m[0]; trans[1] = mat_f->m[1]; trans[2] = mat_f->m[2]; ret = arModifyMatrix( rot, trans, cpara, pos3d, pos2d, num ); arMatrixFree( mat_a ); arMatrixFree( mat_b ); arMatrixFree( mat_c ); arMatrixFree( mat_d ); arMatrixFree( mat_e ); arMatrixFree( mat_f ); for( j = 0; j < 3; j++ ) { for( i = 0; i < 3; i++ ) conv[j][i] = rot[j][i]; conv[j][3] = trans[j]; } return ret; }
static void gen_evec(void) { int i, j, k, ii, jj; ARMat *input, *wevec; ARVec *wev; double sum, sum2; int dim; if( pattern_num < 4 ) { evecf = 0; evecBWf = 0; return; } #if DEBUG printf("------------------------------------------\n"); #endif dim = (pattern_num*4 < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3)? pattern_num*4: AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; input = arMatrixAlloc( pattern_num*4, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 ); wevec = arMatrixAlloc( dim, AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3 ); wev = arVecAlloc( dim ); for( j = jj = 0; jj < AR_PATT_NUM_MAX; jj++ ) { if( patf[jj] == 0 ) continue; for( k = 0; k < 4; k++ ) { for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) { input->m[(j*4+k)*AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3+i] = pat[j][k][i] / patpow[j][k]; } } j++; } if( arMatrixPCA2(input, wevec, wev) < 0 ) { arMatrixFree( input ); arMatrixFree( wevec ); arVecFree( wev ); evecf = 0; evecBWf = 0; return; } sum = 0.0; for( i = 0; i < dim; i++ ) { sum += wev->v[i]; #if DEBUG printf("%2d(%10.7f): \n", i+1, sum); #endif if( sum > 0.90 ) break; if( i == EVEC_MAX-1 ) break; } evec_dim = i+1; for( j = 0; j < evec_dim; j++ ) { for( i = 0; i < AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3; i++ ) { evec[j][i] = wevec->m[j*AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3+i]; } } for( i = 0; i < AR_PATT_NUM_MAX; i++ ) { if(patf[i] == 0) continue; for( j = 0; j < 4; j++ ) { #if DEBUG printf("%2d[%d]: ", i+1, j+1); #endif sum2 = 0.0; for( k = 0; k < evec_dim; k++ ) { sum = 0.0; for(ii=0;ii<AR_PATT_SIZE_Y*AR_PATT_SIZE_X*3;ii++) { sum += evec[k][ii] * pat[i][j][ii] / patpow[i][j]; } #if DEBUG printf("%10.7f ", sum); #endif epat[i][j][k] = sum; sum2 += sum*sum; } #if DEBUG printf(":: %10.7f\n", sqrt(sum2)); #endif } } arMatrixFree( input ); arMatrixFree( wevec ); arVecFree( wev ); evecf = 1; evecBWf = 0; return; }