Subversion Repositories AndroidProjects

/*******************************************************

 *

 * Author: Hirokazu Kato

 *

 *         kato@sys.im.hiroshima-cu.ac.jp

 *

 * Revision: 3.1

 * Date: 01/12/07

 *

*******************************************************/

#include <stdio.h>

#include <math.h>

#include <string.h>

#ifdef _WIN32

#include <sys/timeb.h>

#include <windows.h>

#else

#include <sys/time.h>

#endif

#include <AR/param.h>

#include <AR/matrix.h>

#include <AR/ar.h>

int        arDebug                 = 0;

ARUint8*   arImage                 = NULL;

int        arFittingMode           = DEFAULT_FITTING_MODE;

int        arImageProcMode         = DEFAULT_IMAGE_PROC_MODE;

ARParam    arParam;

int        arImXsize, arImYsize;

int        arTemplateMatchingMode  = DEFAULT_TEMPLATE_MATCHING_MODE;

int        arMatchingPCAMode       = DEFAULT_MATCHING_PCA_MODE;

ARUint8*   arImageL                = NULL;

ARUint8*   arImageR                = NULL;

ARSParam   arsParam;

double     arsMatR2L[3][4];

ARUint32 arGetVersion(char **versionStringRef)

{

        const char version[] = AR_HEADER_VERSION_STRING;

        char *s;

        if (versionStringRef) {

                arMalloc(s, char, sizeof(version));

                strncpy(s, version, sizeof(version));

                *versionStringRef = s;

        }

        // Represent full version number (major, minor, tiny, build) in

        // binary coded decimal. N.B: Integer division.

        return (0x10000000u * ((unsigned int)AR_HEADER_VERSION_MAJOR / 10u) +

                        0x01000000u * ((unsigned int)AR_HEADER_VERSION_MAJOR % 10u) +

                        0x00100000u * ((unsigned int)AR_HEADER_VERSION_MINOR / 10u) +

                        0x00010000u * ((unsigned int)AR_HEADER_VERSION_MINOR % 10u) +

                        0x00001000u * ((unsigned int)AR_HEADER_VERSION_TINY / 10u) +

                        0x00000100u * ((unsigned int)AR_HEADER_VERSION_TINY % 10u) +

                        0x00000010u * ((unsigned int)AR_HEADER_VERSION_BUILD / 10u) +

                        0x00000001u * ((unsigned int)AR_HEADER_VERSION_BUILD % 10u)

                        );

}

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

int arInitCparam( ARParam *param )

{

    arImXsize = param->xsize;

    arImYsize = param->ysize;

    arParam = *param;

    return(0);

}

int arsInitCparam( ARSParam *sparam )

{  

    arImXsize = sparam->xsize;

    arImYsize = sparam->ysize;

    arsParam = *sparam;

    arUtilMatInv( arsParam.matL2R, arsMatR2L );

    return(0);

}

int arGetLine(int x_coord[], int y_coord[], int coord_num,

              int vertex[], double line[4][3], double v[4][2])

{

    return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arParam.dist_factor );

}

int arsGetLine(int x_coord[], int y_coord[], int coord_num,

               int vertex[], double line[4][3], double v[4][2], int LorR)

{  

    if( LorR )

        return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arsParam.dist_factorL );

    else

        return arGetLine2( x_coord, y_coord, coord_num, vertex, line, v, arsParam.dist_factorR );

}

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

}

int arUtilMatMul( double s1[3][4], double s2[3][4], double d[3][4] )

{

    int     i, j;

    for( j = 0; j < 3; j++ ) {

        for( i = 0; i < 4; i++) {

            d[j][i] = s1[j][0] * s2[0][i]

                    + s1[j][1] * s2[1][i]

                    + s1[j][2] * s2[2][i];

        }

        d[j][3] += s1[j][3];

    }

    return 0;

}

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 arUtilMat2QuatPos( double m[3][4], double q[4], double p[3] )

{

    double   w;

    w = m[0][0] + m[1][1] + m[2][2] + 1;

    if( w < 0.0 ) return -1;

    w = sqrt( w );

    q[0] = (m[1][2] - m[2][1]) / (w*2.0);

    q[1] = (m[2][0] - m[0][2]) / (w*2.0);

    q[2] = (m[0][1] - m[1][0]) / (w*2.0);

    q[3] = w / 2.0;

    p[0] = m[0][3];

    p[1] = m[1][3];

    p[2] = m[2][3];

    return 0;

}

int arUtilQuatPos2Mat( double q[4], double p[3], double m[3][4] )

{

    double    x2, y2, z2;

    double    xx, xy, xz;

    double    yy, yz, zz;

    double    wx, wy, wz;

    x2 = q[0] * 2.0;

    y2 = q[1] * 2.0;

    z2 = q[2] * 2.0;

    xx = q[0] * x2;

    xy = q[0] * y2;

    xz = q[0] * z2;

    yy = q[1] * y2;

    yz = q[1] * z2;

    zz = q[2] * z2;

    wx = q[3] * x2;

    wy = q[3] * y2;

    wz = q[3] * z2;

    m[0][0] = 1.0 - (yy + zz);

    m[1][1] = 1.0 - (xx + zz);

    m[2][2] = 1.0 - (xx + yy);

    m[1][0] = xy - wz;

    m[0][1] = xy + wz;

    m[2][0] = xz + wy;

    m[0][2] = xz - wy;

    m[2][1] = yz - wx;

    m[1][2] = yz + wx;

    m[0][3] = p[0];

    m[1][3] = p[1];

    m[2][3] = p[2];

    return 0;

}

static int      ss, sms;

double arUtilTimer(void)

{

#ifdef _WIN32

    struct _timeb sys_time;

    double             tt;

    int                s1, s2;

    _ftime(&sys_time);

    s1 = sys_time.time  - ss;

    s2 = sys_time.millitm - sms;

#else

    struct timeval     time;

    double             tt;

    int                s1, s2;

#if defined(__linux) || defined(__APPLE__)

    gettimeofday( &time, NULL );

#else

    gettimeofday( &time );

#endif

    s1 = time.tv_sec  - ss;

    s2 = time.tv_usec/1000 - sms;

#endif

    tt = (double)s1 + (double)s2 / 1000.0;

    return( tt );

}

void arUtilTimerReset(void)

{

#ifdef _WIN32

    struct _timeb sys_time;

    _ftime(&sys_time);

    ss  = sys_time.time;

    sms = sys_time.millitm;

#else

    struct timeval     time;

#if defined(__linux) || defined(__APPLE__)

    gettimeofday( &time, NULL );

#else

    gettimeofday( &time );

#endif

    ss  = time.tv_sec;

    sms = time.tv_usec / 1000;

#endif

}

void arUtilSleep( int msec )

{

#ifndef _WIN32

    struct timespec  req;

    req.tv_sec = 0;

    req.tv_nsec = msec* 1000;

    nanosleep( &req, NULL );

#else

        Sleep(msec);

#endif

    return;

}