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 );
}
ARdouble arGetTransMatSquareStereo( AR3DStereoHandle *handle,
ARMarkerInfo *marker_infoL, ARMarkerInfo *marker_infoR,
ARdouble width, ARdouble conv[3][4] )
{
ICPStereoDataT data;
ICP2DCoordT screenCoordL[4];
ICP2DCoordT screenCoordR[4];
ICP3DCoordT worldCoord[4];
ARdouble matXc2C[3][4];
ARdouble matXw2C[3][4];
ARdouble matXw2Xc[3][4];
ARdouble err;
int dir;
int i, j;
worldCoord[0].x = -width/2.0;
worldCoord[0].y = width/2.0;
worldCoord[0].z = 0.0;
worldCoord[1].x = width/2.0;
worldCoord[1].y = width/2.0;
worldCoord[1].z = 0.0;
worldCoord[2].x = width/2.0;
worldCoord[2].y = -width/2.0;
worldCoord[2].z = 0.0;
worldCoord[3].x = -width/2.0;
worldCoord[3].y = -width/2.0;
worldCoord[3].z = 0.0;
if( marker_infoL != NULL ) {
dir = marker_infoL->dir;
screenCoordL[0].x = marker_infoL->vertex[(4-dir)%4][0];
screenCoordL[0].y = marker_infoL->vertex[(4-dir)%4][1];
screenCoordL[1].x = marker_infoL->vertex[(5-dir)%4][0];
screenCoordL[1].y = marker_infoL->vertex[(5-dir)%4][1];
screenCoordL[2].x = marker_infoL->vertex[(6-dir)%4][0];
screenCoordL[2].y = marker_infoL->vertex[(6-dir)%4][1];
screenCoordL[3].x = marker_infoL->vertex[(7-dir)%4][0];
screenCoordL[3].y = marker_infoL->vertex[(7-dir)%4][1];
data.numL = 4;
data.screenCoordL = screenCoordL;
data.worldCoordL = worldCoord;
}
else {
data.numL = 0;
data.screenCoordL = NULL;
data.worldCoordL = NULL;
}
if( marker_infoR != NULL ) {
dir = marker_infoR->dir;
screenCoordR[0].x = marker_infoR->vertex[(4-dir)%4][0];
screenCoordR[0].y = marker_infoR->vertex[(4-dir)%4][1];
screenCoordR[1].x = marker_infoR->vertex[(5-dir)%4][0];
screenCoordR[1].y = marker_infoR->vertex[(5-dir)%4][1];
screenCoordR[2].x = marker_infoR->vertex[(6-dir)%4][0];
screenCoordR[2].y = marker_infoR->vertex[(6-dir)%4][1];
screenCoordR[3].x = marker_infoR->vertex[(7-dir)%4][0];
screenCoordR[3].y = marker_infoR->vertex[(7-dir)%4][1];
data.numR = 4;
data.screenCoordR = screenCoordR;
data.worldCoordR = worldCoord;
}
else {
data.numR = 0;
data.screenCoordR = NULL;
data.worldCoordR = NULL;
}
if( marker_infoL != NULL
&& icpGetInitXw2Xc_from_PlanarData(handle->icpStereoHandle->matXcl2Ul, screenCoordL, worldCoord, 4, matXw2Xc) == 0 ) {
arUtilMatInv( handle->icpStereoHandle->matC2L, matXc2C );
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
matXw2C[j][i] = matXc2C[j][0]*matXw2Xc[0][i]
+ matXc2C[j][1]*matXw2Xc[1][i]
+ matXc2C[j][2]*matXw2Xc[2][i];
}
matXw2C[j][3] += matXc2C[j][3];
}
}
else if( marker_infoR != NULL
&& icpGetInitXw2Xc_from_PlanarData(handle->icpStereoHandle->matXcr2Ur, screenCoordR, worldCoord, 4, matXw2Xc) == 0 ) {
arUtilMatInv( handle->icpStereoHandle->matC2R, matXc2C );
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
matXw2C[j][i] = matXc2C[j][0]*matXw2Xc[0][i]
+ matXc2C[j][1]*matXw2Xc[1][i]
+ matXc2C[j][2]*matXw2Xc[2][i];
}
matXw2C[j][3] += matXc2C[j][3];
}
}
else return 100000000.0;
if( icpStereoPoint(handle->icpStereoHandle, &data, matXw2C, conv, &err) < 0 ) return 100000000.0;
return err;
}
