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