AR3DHandle *ar3DCreateHandle2(ARdouble cpara[3][4]) { AR3DHandle *handle; arMalloc( handle, AR3DHandle, 1 ); handle->icpHandle = icpCreateHandle( cpara ); if( handle->icpHandle == NULL ) { free( handle ); return NULL; } return handle; }
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 ); }