Subversion Repositories AndroidProjects

#include <math.h>
#include <stdio.h>
#include <AR/ar.h>
#include <AR/param.h>
#include <AR/matrix.h>
#include "calib_camera.h"

static int calc_inp2( CALIB_PATT_T *patt, CALIB_COORD_T *screen, double *pos2d, double *pos3d,
                      double dist_factor[4], double x0, double y0, double f[2], double *err );
static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, double para[3][3] );
static int  get_fl( double *p , double *q, int num, double f[2] );
static int  check_rotation( double rot[2][3] );

int calc_inp( CALIB_PATT_T *patt, double dist_factor[4], int xsize, int ysize, double mat[3][4] )
{
    CALIB_COORD_T  *screen, *sp;
    double         *pos2d, *pos3d, *pp;
    double         f[2];
    double         x0, y0;
    double         err, minerr;
    int            res;
    int            i, j, k;

    sp = screen = (CALIB_COORD_T *)malloc(sizeof(CALIB_COORD_T) * patt->h_num * patt->v_num * patt->loop_num);
    pp = pos2d = (double *)malloc(sizeof(double) * patt->h_num * patt->v_num * patt->loop_num * 2);
    pos3d = (double *)malloc(sizeof(double) * patt->h_num * patt->v_num * 2);
    for( k = 0; k < patt->loop_num; k++ ) {
        for( j = 0; j < patt->v_num; j++ ) {
            for( i = 0; i < patt->h_num; i++ ) {
                arParamObserv2Ideal( dist_factor,
                                     patt->point[k][j*patt->h_num+i].x_coord,
                                     patt->point[k][j*patt->h_num+i].y_coord,
                                     &(sp->x_coord), &(sp->y_coord) );
                *(pp++) = sp->x_coord;
                *(pp++) = sp->y_coord;
                sp++;
            }
        }
    }
    pp = pos3d;
    for( j = 0; j < patt->v_num; j++ ) {
        for( i = 0; i < patt->h_num; i++ ) {
            *(pp++) = patt->world_coord[j*patt->h_num+i].x_coord;
            *(pp++) = patt->world_coord[j*patt->h_num+i].y_coord;
        }
    }

    minerr = 100000000000000000000000.0;
    for( j = -50; j <= 50; j++ ) {
        printf("-- loop:%d --\n", j);
        y0 = dist_factor[1] + j;
/*      y0 = ysize/2 + j;   */
        if( y0 < 0 || y0 >= ysize ) continue;

        for( i = -50; i <= 50; i++ ) {
            x0 = dist_factor[0] + i;
/*          x0 = xsize/2 + i;  */
            if( x0 < 0 || x0 >= xsize ) continue;

            res = calc_inp2( patt, screen, pos2d, pos3d, dist_factor, x0, y0, f, &err );
            if( res < 0 ) continue;
            if( err < minerr ) {
                printf("F = (%f,%f), Center = (%f,%f): err = %f\n", f[0], f[1], x0, y0, err);
                minerr = err;

                mat[0][0] = f[0];
                mat[0][1] = 0.0;
                mat[0][2] = x0;
                mat[0][3] = 0.0;
                mat[1][0] = 0.0;
                mat[1][1] = f[1];
                mat[1][2] = y0;
                mat[1][3] = 0.0;
                mat[2][0] = 0.0;
                mat[2][1] = 0.0;
                mat[2][2] = 1.0;
                mat[2][3] = 0.0;
            }
        }
    }

    free( screen );
    free( pos2d );
    free( pos3d );

    if( minerr >= 100.0 ) return -1;
    return 0;
}


static int calc_inp2 ( CALIB_PATT_T *patt, CALIB_COORD_T *screen, double *pos2d, double *pos3d,
                      double dist_factor[4], double x0, double y0, double f[2], double *err )
{
    double  x1, y1, x2, y2;
    double  p[LOOP_MAX], q[LOOP_MAX];
    double  para[3][3];
    double  rot[3][3], rot2[3][3];
    double  cpara[3][4], conv[3][4];
    double  ppos2d[4][2], ppos3d[4][2];
    double  d, werr, werr2;
    int     i, j, k, l;

    for( i =  0; i < patt->loop_num; i++ ) {
        get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
                   patt->h_num*patt->v_num, para );
        x1 = para[0][0] / para[2][0];
        y1 = para[1][0] / para[2][0];
        x2 = para[0][1] / para[2][1];
        y2 = para[1][1] / para[2][1];

        p[i] = (x1 - x0)*(x2 - x0);
        q[i] = (y1 - y0)*(y2 - y0);
    }
    if( get_fl(p, q, patt->loop_num, f) < 0 ) return -1;

    cpara[0][0] = f[0];
    cpara[0][1] = 0.0;
    cpara[0][2] = x0;
    cpara[0][3] = 0.0;
    cpara[1][0] = 0.0;
    cpara[1][1] = f[1];
    cpara[1][2] = y0;
    cpara[1][3] = 0.0;
    cpara[2][0] = 0.0;
    cpara[2][1] = 0.0;
    cpara[2][2] = 1.0;
    cpara[2][3] = 0.0;

    werr = 0.0;
    for( i =  0; i < patt->loop_num; i++ ) {
        get_cpara( patt->world_coord, &(screen[i*patt->h_num*patt->v_num]),
                   patt->h_num*patt->v_num, para );
        rot[0][0] = (para[0][0] - x0*para[2][0]) / f[0];
        rot[0][1] = (para[1][0] - y0*para[2][0]) / f[1];
        rot[0][2] = para[2][0];
        d = sqrt( rot[0][0]*rot[0][0] + rot[0][1]*rot[0][1] + rot[0][2]*rot[0][2] );
        rot[0][0] /= d;
        rot[0][1] /= d;
        rot[0][2] /= d;
        rot[1][0] = (para[0][1] - x0*para[2][1]) / f[0];
        rot[1][1] = (para[1][1] - y0*para[2][1]) / f[1];
        rot[1][2] = para[2][1];
        d = sqrt( rot[1][0]*rot[1][0] + rot[1][1]*rot[1][1] + rot[1][2]*rot[1][2] );
        rot[1][0] /= d;
        rot[1][1] /= d;
        rot[1][2] /= d;
        check_rotation( rot );
        rot[2][0] = rot[0][1]*rot[1][2] - rot[0][2]*rot[1][1];
        rot[2][1] = rot[0][2]*rot[1][0] - rot[0][0]*rot[1][2];
        rot[2][2] = rot[0][0]*rot[1][1] - rot[0][1]*rot[1][0];
        d = sqrt( rot[2][0]*rot[2][0] + rot[2][1]*rot[2][1] + rot[2][2]*rot[2][2] );
        rot[2][0] /= d;
        rot[2][1] /= d;
        rot[2][2] /= d;
        rot2[0][0] = rot[0][0];
        rot2[1][0] = rot[0][1];
        rot2[2][0] = rot[0][2];
        rot2[0][1] = rot[1][0];
        rot2[1][1] = rot[1][1];
        rot2[2][1] = rot[1][2];
        rot2[0][2] = rot[2][0];
        rot2[1][2] = rot[2][1];
        rot2[2][2] = rot[2][2];

        ppos2d[0][0] = pos2d[i*patt->h_num*patt->v_num*2 + 0];
        ppos2d[0][1] = pos2d[i*patt->h_num*patt->v_num*2 + 1];
        ppos2d[1][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 0];
        ppos2d[1][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num-1)*2 + 1];
        ppos2d[2][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 0];
        ppos2d[2][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*(patt->v_num-1))*2 + 1];
        ppos2d[3][0] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 0];
        ppos2d[3][1] = pos2d[i*patt->h_num*patt->v_num*2 + (patt->h_num*patt->v_num-1)*2 + 1];
        ppos3d[0][0] = pos3d[0];
        ppos3d[0][1] = pos3d[1];
        ppos3d[1][0] = pos3d[(patt->h_num-1)*2 + 0];
        ppos3d[1][1] = pos3d[(patt->h_num-1)*2 + 1];
        ppos3d[2][0] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 0];
        ppos3d[2][1] = pos3d[(patt->h_num*(patt->v_num-1))*2 + 1];
        ppos3d[3][0] = pos3d[(patt->h_num*patt->v_num-1)*2 + 0];
        ppos3d[3][1] = pos3d[(patt->h_num*patt->v_num-1)*2 + 1];

        for( j = 0; j < 5; j++ ) {
            arFittingMode = AR_FITTING_TO_IDEAL;
            werr2 = arGetTransMat3( rot2, ppos2d, ppos3d, 4, conv, dist_factor, cpara );
            for( k = 0; k < 3; k++ ) {
            for( l = 0; l < 3; l++ ) {
                rot2[k][l] = conv[k][l];
            }}
        }
        werr += werr2;

    }
    *err = sqrt( werr / patt->loop_num );

    return 0;
}

static void get_cpara( CALIB_COORD_T *world, CALIB_COORD_T *screen, int num, double para[3][3] )
{
    ARMat   *a, *b, *c;
    ARMat   *at, *aa, res;
    int     i;

    a = arMatrixAlloc( num*2, 8 );
    b = arMatrixAlloc( num*2, 1 );
    c = arMatrixAlloc( 8, num*2 );
    at = arMatrixAlloc( 8, num*2 );
    aa = arMatrixAlloc( 8, 8 );
    for( i = 0; i < num; i++ ) {
        a->m[i*16+0]  = world[i].x_coord;
        a->m[i*16+1]  = world[i].y_coord;
        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].x_coord * screen[i].x_coord;
        a->m[i*16+7]  = -world[i].y_coord * screen[i].x_coord;
        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].x_coord;
        a->m[i*16+12] = world[i].y_coord;
        a->m[i*16+13] = 1.0;
        a->m[i*16+14] = -world[i].x_coord * screen[i].y_coord;
        a->m[i*16+15] = -world[i].y_coord * screen[i].y_coord;
        b->m[i*2+0] = screen[i].x_coord;
        b->m[i*2+1] = screen[i].y_coord;
    }
    arMatrixTrans( at, a );
    arMatrixMul( aa, at, a );
    arMatrixSelfInv( aa );
    arMatrixMul( c, aa, at );
    res.row = 8;
    res.clm = 1;
    res.m = &(para[0][0]);
    arMatrixMul( &res, c, b );
    para[2][2] = 1.0;

    arMatrixFree( a );
    arMatrixFree( b );
    arMatrixFree( c );
    arMatrixFree( at );
    arMatrixFree( aa );
}

static int get_fl( double *p , double *q, int num, double f[2] )
{
    ARMat   *a, *b, *c;
    ARMat   *at, *aa, *res;
    int     i;

#if 1
    a = arMatrixAlloc( num, 2 );
    b = arMatrixAlloc( num, 1 );
    c = arMatrixAlloc( 2, num );
    at = arMatrixAlloc( 2, num );
    aa = arMatrixAlloc( 2, 2 );
    res = arMatrixAlloc( 2, 1 );
    for( i = 0; i < num; i++ ) {
        a->m[i*2+0] = *(p++);
        a->m[i*2+1] = *(q++);
        b->m[i]     = -1.0;
    }
#else
    a = arMatrixAlloc( num-1, 2 );
    b = arMatrixAlloc( num-1, 1 );
    c = arMatrixAlloc( 2, num-1 );
    at = arMatrixAlloc( 2, num-1 );
    aa = arMatrixAlloc( 2, 2 );
    res = arMatrixAlloc( 2, 1 );
    p++; q++;
    for( i = 0; i < num-1; i++ ) {
        a->m[i*2+0] = *(p++);
        a->m[i*2+1] = *(q++);
        b->m[i]     = -1.0;
    }
#endif
    arMatrixTrans( at, a );
    arMatrixMul( aa, at, a );
    arMatrixSelfInv( aa );
    arMatrixMul( c, aa, at );
    arMatrixMul( res, c, b );

    if( res->m[0] < 0 || res->m[1] < 0 ) return -1;

    f[0] = sqrt( 1.0 / res->m[0] );
    f[1] = sqrt( 1.0 / res->m[1] );

    arMatrixFree( a );
    arMatrixFree( b );
    arMatrixFree( c );
    arMatrixFree( at );
    arMatrixFree( aa );
    arMatrixFree( res );

    return 0;
}

static int check_rotation( double rot[2][3] )
{
    double  v1[3], v2[3], v3[3];
    double  ca, cb, k1, k2, k3, k4;
    double  a, b, c, d;
    double  p1, q1, r1;
    double  p2, q2, r2;
    double  p3, q3, r3;
    double  p4, q4, r4;
    double  w;
    double  e1, e2, e3, e4;
    int     f;

    v1[0] = rot[0][0];
    v1[1] = rot[0][1];
    v1[2] = rot[0][2];
    v2[0] = rot[1][0];
    v2[1] = rot[1][1];
    v2[2] = rot[1][2];
    v3[0] = v1[1]*v2[2] - v1[2]*v2[1];
    v3[1] = v1[2]*v2[0] - v1[0]*v2[2];
    v3[2] = v1[0]*v2[1] - v1[1]*v2[0];
    w = sqrt( v3[0]*v3[0]+v3[1]*v3[1]+v3[2]*v3[2] );
    if( w == 0.0 ) return -1;
    v3[0] /= w;
    v3[1] /= w;
    v3[2] /= w;

    cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
    if( cb < 0 ) cb *= -1.0;
    ca = (sqrt(cb+1.0) + sqrt(1.0-cb)) * 0.5;

    if( v3[1]*v1[0] - v1[1]*v3[0] != 0.0 ) {
        f = 0;
    }
    else {
        if( v3[2]*v1[0] - v1[2]*v3[0] != 0.0 ) {
            w = v1[1]; v1[1] = v1[2]; v1[2] = w;
            w = v3[1]; v3[1] = v3[2]; v3[2] = w;
            f = 1;
        }
        else {
            w = v1[0]; v1[0] = v1[2]; v1[2] = w;
            w = v3[0]; v3[0] = v3[2]; v3[2] = w;
            f = 2;
        }
    }
    if( v3[1]*v1[0] - v1[1]*v3[0] == 0.0 ) return -1;
    k1 = (v1[1]*v3[2] - v3[1]*v1[2]) / (v3[1]*v1[0] - v1[1]*v3[0]);
    k2 = (v3[1] * ca) / (v3[1]*v1[0] - v1[1]*v3[0]);
    k3 = (v1[0]*v3[2] - v3[0]*v1[2]) / (v3[0]*v1[1] - v1[0]*v3[1]);
    k4 = (v3[0] * ca) / (v3[0]*v1[1] - v1[0]*v3[1]);

    a = k1*k1 + k3*k3 + 1;
    b = k1*k2 + k3*k4;
    c = k2*k2 + k4*k4 - 1;

    d = b*b - a*c;
    if( d < 0 ) return -1;
    r1 = (-b + sqrt(d))/a;
    p1 = k1*r1 + k2;
    q1 = k3*r1 + k4;
    r2 = (-b - sqrt(d))/a;
    p2 = k1*r2 + k2;
    q2 = k3*r2 + k4;
    if( f == 1 ) {
        w = q1; q1 = r1; r1 = w;
        w = q2; q2 = r2; r2 = w;
        w = v1[1]; v1[1] = v1[2]; v1[2] = w;
        w = v3[1]; v3[1] = v3[2]; v3[2] = w;
        f = 0;
    }
    if( f == 2 ) {
        w = p1; p1 = r1; r1 = w;
        w = p2; p2 = r2; r2 = w;
        w = v1[0]; v1[0] = v1[2]; v1[2] = w;
        w = v3[0]; v3[0] = v3[2]; v3[2] = w;
        f = 0;
    }

    if( v3[1]*v2[0] - v2[1]*v3[0] != 0.0 ) {
        f = 0;
    }
    else {
        if( v3[2]*v2[0] - v2[2]*v3[0] != 0.0 ) {
            w = v2[1]; v2[1] = v2[2]; v2[2] = w;
            w = v3[1]; v3[1] = v3[2]; v3[2] = w;
            f = 1;
        }
        else {
            w = v2[0]; v2[0] = v2[2]; v2[2] = w;
            w = v3[0]; v3[0] = v3[2]; v3[2] = w;
            f = 2;
        }
    }
    if( v3[1]*v2[0] - v2[1]*v3[0] == 0.0 ) return -1;
    k1 = (v2[1]*v3[2] - v3[1]*v2[2]) / (v3[1]*v2[0] - v2[1]*v3[0]);
    k2 = (v3[1] * ca) / (v3[1]*v2[0] - v2[1]*v3[0]);
    k3 = (v2[0]*v3[2] - v3[0]*v2[2]) / (v3[0]*v2[1] - v2[0]*v3[1]);
    k4 = (v3[0] * ca) / (v3[0]*v2[1] - v2[0]*v3[1]);

    a = k1*k1 + k3*k3 + 1;
    b = k1*k2 + k3*k4;
    c = k2*k2 + k4*k4 - 1;

    d = b*b - a*c;
    if( d < 0 ) return -1;
    r3 = (-b + sqrt(d))/a;
    p3 = k1*r3 + k2;
    q3 = k3*r3 + k4;
    r4 = (-b - sqrt(d))/a;
    p4 = k1*r4 + k2;
    q4 = k3*r4 + k4;
    if( f == 1 ) {
        w = q3; q3 = r3; r3 = w;
        w = q4; q4 = r4; r4 = w;
        w = v2[1]; v2[1] = v2[2]; v2[2] = w;
        w = v3[1]; v3[1] = v3[2]; v3[2] = w;
        f = 0;
    }
    if( f == 2 ) {
        w = p3; p3 = r3; r3 = w;
        w = p4; p4 = r4; r4 = w;
        w = v2[0]; v2[0] = v2[2]; v2[2] = w;
        w = v3[0]; v3[0] = v3[2]; v3[2] = w;
        f = 0;
    }

    e1 = p1*p3+q1*q3+r1*r3; if( e1 < 0 ) e1 = -e1;
    e2 = p1*p4+q1*q4+r1*r4; if( e2 < 0 ) e2 = -e2;
    e3 = p2*p3+q2*q3+r2*r3; if( e3 < 0 ) e3 = -e3;
    e4 = p2*p4+q2*q4+r2*r4; if( e4 < 0 ) e4 = -e4;
    if( e1 < e2 ) {
        if( e1 < e3 ) {
            if( e1 < e4 ) {
                rot[0][0] = p1;
                rot[0][1] = q1;
                rot[0][2] = r1;
                rot[1][0] = p3;
                rot[1][1] = q3;
                rot[1][2] = r3;
            }
            else {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p4;
                rot[1][1] = q4;
                rot[1][2] = r4;
            }
        }
        else {
            if( e3 < e4 ) {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p3;
                rot[1][1] = q3;
                rot[1][2] = r3;
            }
            else {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p4;
                rot[1][1] = q4;
                rot[1][2] = r4;
            }
        }
    }
    else {
        if( e2 < e3 ) {
            if( e2 < e4 ) {
                rot[0][0] = p1;
                rot[0][1] = q1;
                rot[0][2] = r1;
                rot[1][0] = p4;
                rot[1][1] = q4;
                rot[1][2] = r4;
            }
            else {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p4;
                rot[1][1] = q4;
                rot[1][2] = r4;
            }
        }
        else {
            if( e3 < e4 ) {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p3;
                rot[1][1] = q3;
                rot[1][2] = r3;
            }
            else {
                rot[0][0] = p2;
                rot[0][1] = q2;
                rot[0][2] = r2;
                rot[1][0] = p4;
                rot[1][1] = q4;
                rot[1][2] = r4;
            }
        }
    }

    return 0;
}