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