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;
}
