Beispiel #1
0
static void calib(void)
{
    ICPHandleT *icpHandleL;
    ICPHandleT *icpHandleR;
    ICPDataT    icpData;
    ARdouble    initMatXw2Xc[3][4];
    ARdouble    initTransL2R[3][4], matL[3][4], matR[3][4], invMatL[3][4];
    ARdouble    transL2R[3][4];
    ARdouble    err;
    int         i;

    if( (icpHandleL = icpCreateHandle(paramL.mat)) == NULL ) {
        ARLOG("Error!! icpCreateHandle\n");
        return;
    }
    icpSetBreakLoopErrorThresh( icpHandleL, 0.00001 );
    if( (icpHandleR = icpCreateHandle(paramR.mat)) == NULL ) {
        ARLOG("Error!! icpCreateHandle\n");
        return;
    }
    icpSetBreakLoopErrorThresh( icpHandleR, 0.00001 );

    for( i = 0; i < calibImageNum; i++ ) {
        if( icpGetInitXw2Xc_from_PlanarData(paramL.mat, calibData[i].screenCoordL, calibData[i].worldCoordL, calibData[i].numL, calibData[i].initMatXw2Xcl) < 0 ) {
            ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
            return;
        }
        icpData.screenCoord = calibData[i].screenCoordL;
        icpData.worldCoord  = calibData[i].worldCoordL;
        icpData.num = calibData[i].numL;
    }

    if( icpGetInitXw2Xc_from_PlanarData(paramL.mat, calibData[0].screenCoordL, calibData[0].worldCoordL, calibData[0].numL, initMatXw2Xc) < 0 ) {
        ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
        return;
    }
    icpData.screenCoord = calibData[0].screenCoordL;
    icpData.worldCoord  = calibData[0].worldCoordL;
    icpData.num = calibData[0].numL;
    if( icpPoint(icpHandleL, &icpData, initMatXw2Xc, matL, &err) < 0 ) {
        ARLOG("Error!! icpPoint\n");
        return;
    }
    if( icpGetInitXw2Xc_from_PlanarData(paramR.mat, calibData[0].screenCoordR, calibData[0].worldCoordR, calibData[0].numR, matR) < 0 ) {
        ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
        return;
    }
    icpData.screenCoord = calibData[0].screenCoordR;
    icpData.worldCoord  = calibData[0].worldCoordR;
    icpData.num = calibData[0].numR;
    if( icpPoint(icpHandleR, &icpData, initMatXw2Xc, matR, &err) < 0 ) {
        ARLOG("Error!! icpPoint\n");
        return;
    }
    arUtilMatInv( matL, invMatL );
    arUtilMatMul( matR, invMatL, initTransL2R );

    if( icpCalibStereo(calibData, calibImageNum, paramL.mat, paramR.mat, initTransL2R, transL2R, &err) < 0 ) {
        ARLOG("Calibration error!!\n");
        return;
    }
    ARLOG("Estimated transformation matrix from Left to Right.\n");
    arParamDispExt( transL2R );

    saveParam( transL2R );
}
Beispiel #2
0
static float  ar2GetTransMat( ICPHandleT *icpHandle, float  initConv[3][4], float  pos2d[][2], float  pos3d[][3], int num,
                              float  conv[3][4], int robustMode )
{   
    ICPDataT       data;
    float          dx, dy, dz;
    ARdouble       initMat[3][4], mat[3][4];
    ARdouble       err;
    int            i, j;
    
    arMalloc( data.screenCoord, ICP2DCoordT, num );
    arMalloc( data.worldCoord,  ICP3DCoordT, num );
    
    dx = dy = dz = 0.0;
    for( i = 0; i < num; i++ ) {
        dx += pos3d[i][0];
        dy += pos3d[i][1];
        dz += pos3d[i][2];
    }
    dx /= num;
    dy /= num;
    dz /= num;
    
    for( i = 0; i < num; i++ ) {
        data.screenCoord[i].x = pos2d[i][0];
        data.screenCoord[i].y = pos2d[i][1];
        data.worldCoord[i].x  = pos3d[i][0] - dx;
        data.worldCoord[i].y  = pos3d[i][1] - dy;
        data.worldCoord[i].z  = pos3d[i][2] - dz;
    }
    data.num = num;

    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 3; i++ ) initMat[j][i] = (ARdouble)(initConv[j][i]);
    }
    initMat[0][3] = (ARdouble)(initConv[0][0] * dx + initConv[0][1] * dy + initConv[0][2] * dz + initConv[0][3]);
    initMat[1][3] = (ARdouble)(initConv[1][0] * dx + initConv[1][1] * dy + initConv[1][2] * dz + initConv[1][3]);
    initMat[2][3] = (ARdouble)(initConv[2][0] * dx + initConv[2][1] * dy + initConv[2][2] * dz + initConv[2][3]);
    
    if( robustMode == 0 ) {
        if( icpPoint( icpHandle, &data, initMat, mat, &err ) < 0 ) {
            err = 100000000.0F;
        }
    }
    else {
        if( icpPointRobust( icpHandle, &data, initMat, mat, &err ) < 0 ) {
            err = 100000000.0F;
        }
    }

    free( data.screenCoord );
    free( data.worldCoord );

    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 3; i++ ) conv[j][i] = (float)mat[j][i];
    }
    conv[0][3] = (float)(mat[0][3] - mat[0][0] * dx - mat[0][1] * dy - mat[0][2] * dz);
    conv[1][3] = (float)(mat[1][3] - mat[1][0] * dx - mat[1][1] * dy - mat[1][2] * dz);
    conv[2][3] = (float)(mat[2][3] - mat[2][0] * dx - mat[2][1] * dy - mat[2][2] * dz);

    return (float)err;
}