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);
}
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;
}
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;
}
static int QRM( ARMat *a, ARVec *dv )
{
ARVec *ev, ev1;
ARdouble w, t, s, x, y, c;
ARdouble *v1, *v2;
int dim, iter;
int i, j, k, h;
dim = a->row;
if( dim != a->clm || dim < 2 ) return(-1);
if( dv->clm != dim ) return(-1);
ev = arVecAlloc( dim );
if( ev == NULL ) return(-1);
ev1.clm = dim-1;
ev1.v = &(ev->v[1]);
if( arVecTridiagonalize( a, dv, &ev1 ) < 0 ) {
arVecFree( ev );
return(-1);
}
ev->v[0] = 0.0;
for( h = dim-1; h > 0; h-- ) {
j = h;
while(j>0 && fabs(ev->v[j]) > EPS*(fabs(dv->v[j-1])+fabs(dv->v[j]))) j--;
if( j == h ) continue;
iter = 0;
do{
iter++;
if( iter > MAX_ITER ) break;
w = (dv->v[h-1] - dv->v[h]) / 2;
t = ev->v[h] * ev->v[h];
s = sqrt(w*w+t);
if( w < 0 ) s = -s;
x = dv->v[j] - dv->v[h] + t/(w+s);
y = ev->v[j+1];
for( k = j; k < h; k++ ) {
if( fabs(x) >= fabs(y) ) {
if( fabs(x) > VZERO ) {
t = -y / x;
c = 1 / sqrt(t*t+1);
s = t * c;
}
else{
c = 1.0;
s = 0.0;
}
}
else{
t = -x / y;
s = 1.0 / sqrt(t*t+1);
c = t * s;
}
w = dv->v[k] - dv->v[k+1];
t = (w * s + 2 * c * ev->v[k+1]) * s;
dv->v[k] -= t;
dv->v[k+1] += t;
if( k > j) ev->v[k] = c * ev->v[k] - s * y;
ev->v[k+1] += s * (c * w - 2 * s * ev->v[k+1]);
for( i = 0; i < dim; i++ ) {
x = a->m[k*dim+i];
y = a->m[(k+1)*dim+i];
a->m[k*dim+i] = c * x - s * y;
a->m[(k+1)*dim+i] = s * x + c * y;
}
if( k < h-1 ) {
x = ev->v[k+1];
y = -s * ev->v[k+2];
ev->v[k+2] *= c;
}
}
} while(fabs(ev->v[h]) > EPS*(fabs(dv->v[h-1])+fabs(dv->v[h])));
}
for( k = 0; k < dim-1; k++ ) {
h = k;
t = dv->v[h];
for( i = k+1; i < dim; i++ ) {
if( dv->v[i] > t ) {
h = i;
t = dv->v[h];
}
}
dv->v[h] = dv->v[k];
dv->v[k] = t;
v1 = &(a->m[h*dim]);
v2 = &(a->m[k*dim]);
for( i = 0; i < dim; i++ ) {
w = *v1;
*v1 = *v2;
*v2 = w;
v1++;
v2++;
}
}
arVecFree( ev );
return(0);
}
