Esempi in C++ (Cpp) per arMatrixAllocTrans

Esempio n. 1

File: icpCalibStereo.c Progetto: hyyh619/ARToolKit_5.3.1

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

Esempio n. 2

File: icpCore.c Progetto: 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;
}