コード例 #1
0
static int icp2GetTS(ARdouble TS[], ARdouble dU[], ARdouble J[], int dataNum, int paramNum)
{
    ARMat matTS, matU, matJ;
    ARMat *matJt, *matJtJ, *matJtU;

    matTS.row = paramNum;
    matTS.clm = 1;
    matTS.m   = TS;

    matU.row = dataNum;
    matU.clm = 1;
    matU.m   = dU;

    matJ.row = dataNum;
    matJ.clm = paramNum;
    matJ.m   = J;

    matJt = arMatrixAllocTrans(&matJ);
    if (matJt == NULL)
        return -1;

    matJtJ = arMatrixAllocMul(matJt, &matJ);
    if (matJtJ == NULL)
    {
        arMatrixFree(matJt);
        return -1;
    }

    matJtU = arMatrixAllocMul(matJt, &matU);
    if (matJtJ == NULL)
    {
        arMatrixFree(matJt);
        arMatrixFree(matJtJ);
        return -1;
    }

    if (arMatrixSelfInv(matJtJ) < 0)
    {
        arMatrixFree(matJt);
        arMatrixFree(matJtJ);
        arMatrixFree(matJtU);
        return -1;
    }

    arMatrixMul(&matTS, matJtJ, matJtU);
    arMatrixFree(matJt);
    arMatrixFree(matJtJ);
    arMatrixFree(matJtU);

    return 0;
}
コード例 #2
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 );
	
}
コード例 #3
0
ファイル: arGetCode.c プロジェクト: AVRGroup/EducAR-Quadrics
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 );
}
コード例 #4
0
ファイル: icpCore.c プロジェクト: AadityaDev/artoolkit5
int icpGetDeltaS( ARdouble S[6], ARdouble dU[], ARdouble J_U_S[][6], int n )
{
    ARMat   matS, matU, matJ;
    ARMat  *matJt, *matJtJ, *matJtU;

    matS.row = 6;
    matS.clm = 1;
    matS.m   = S;

    matU.row = n;
    matU.clm = 1;
    matU.m   = dU;

    matJ.row = n;
    matJ.clm = 6;
    matJ.m   = &J_U_S[0][0];

#if ICP_DEBUG
    ARLOG("cc1 - matU\n");
    arMatrixDisp( &matU );
    ARLOG("cc1 - matJ\n");
    arMatrixDisp( &matJ );
#endif
    matJt = arMatrixAllocTrans( &matJ );
    if( matJt == NULL ) return -1;
#if ICP_DEBUG
    ARLOG("cc1 - matJt\n");
    arMatrixDisp( matJt );
#endif
    matJtJ = arMatrixAllocMul( matJt, &matJ );
    if( matJtJ == NULL ) {
        arMatrixFree( matJt );
        return -1;
    }
#if ICP_DEBUG
    ARLOG("cc2 - matJtJ\n");
    arMatrixDisp( matJtJ );
#endif
    matJtU = arMatrixAllocMul( matJt, &matU );
    if( matJtU == NULL ) {
        arMatrixFree( matJt );
        arMatrixFree( matJtJ );
        return -1;
    }
#if ICP_DEBUG
    ARLOG("cc3 -- matJtU\n");
    arMatrixDisp( matJtU );
#endif
    if( arMatrixSelfInv(matJtJ) < 0 ) {
        arMatrixFree( matJt );
        arMatrixFree( matJtJ );
        arMatrixFree( matJtU );
        return -1;
    }

#if ICP_DEBUG
    ARLOG("cc4 -- matJtJ_Inv\n");
    arMatrixDisp( matJtJ );
#endif
    arMatrixMul( &matS, matJtJ, matJtU );
    arMatrixFree( matJt );
    arMatrixFree( matJtJ );
    arMatrixFree( matJtU );
#if ICP_DEBUG
    ARLOG("cc5 -- matS\n");
    arMatrixDisp( &matS );
#endif

    return 0;
}
コード例 #5
0
ファイル: ar3DUtil.c プロジェクト: Avatarchik/nft_generator
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;
}
コード例 #6
0
ファイル: ar3DUtil.c プロジェクト: Avatarchik/nft_generator
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;
}
コード例 #7
0
ファイル: paramGet.c プロジェクト: AVRGroup/EducAR-Quadrics
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;
}
コード例 #8
0
ファイル: arGetTransMat.c プロジェクト: KenyStev/LuxxorAR
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;
}