static void getResultQuat( ARMarkerInfo *marker_info ) { double target_trans[3][4]; double cam_trans[3][4]; double quat[4], pos[3]; char string1[256]; char string2[256]; if( arGetTransMat(marker_info, target_center, target_width, target_trans) < 0 ) return; if( arUtilMatInv(target_trans, cam_trans) < 0 ) return; if( arUtilMat2QuatPos(cam_trans, quat, pos) < 0 ) return; sprintf(string1," QUAT: Pos x: %3.1f y: %3.1f z: %3.1f\n", pos[0], pos[1], pos[2]); sprintf(string2, " Quat qx: %3.2f qy: %3.2f qz: %3.2f qw: %3.2f ", quat[0], quat[1], quat[2], quat[3]); strcat( string1, string2 ); if( disp_mode ) { draw( "target", target_trans, 0, 0 ); draw_exview( a, b, r, target_trans, 1, 1 ); } else { draw( "target", target_trans, 1, 1 ); draw_exview( a, b, r, target_trans, 0, 0 ); } print_string( string1 ); return; }
int arsInitCparam( ARSParam *sparam ) { arImXsize = sparam->xsize; arImYsize = sparam->ysize; arsParam = *sparam; arUtilMatInv( arsParam.matL2R, arsMatR2L ); return(0); }
//==================== // カメラの位置の取得 //==================== void cARTK::getCameraPos( Vector3 *pvec3CamPos ) { double dInvPattTransMat[3][4]; arUtilMatInv( m_dPattTransMat, dInvPattTransMat ); pvec3CamPos->x = (float)dInvPattTransMat[0][3]; pvec3CamPos->y = (float)dInvPattTransMat[1][3]; pvec3CamPos->z = (float)dInvPattTransMat[2][3]; Vector3Transform( pvec3CamPos, pvec3CamPos, m_matRotX ); pvec3CamPos->x *= (float)m_dViewScaleFactor; pvec3CamPos->y *= (float)m_dViewScaleFactor; pvec3CamPos->z *= (float)m_dViewScaleFactor; }
//============================================== // Baseを原点とした場合のTargetの相対位置を取得 //============================================== void cARTK::getMarkerPos( Vector3 *pvec3MarkerPos, int iPattIndexBase, int iPattIndexTarget ) { double dInvBaseTransMat[3][4]; double dTargetTransMat[3][4]; arUtilMatInv( m_sMarkerInfo[iPattIndexBase].dTransMat, dInvBaseTransMat ); arUtilMatMul( dInvBaseTransMat, m_sMarkerInfo[iPattIndexTarget].dTransMat, dTargetTransMat ); pvec3MarkerPos->x = (float)dTargetTransMat[0][3]; pvec3MarkerPos->y = (float)dTargetTransMat[1][3]; pvec3MarkerPos->z = (float)dTargetTransMat[2][3]; Vector3Transform( pvec3MarkerPos, pvec3MarkerPos, m_matRotX ); pvec3MarkerPos->x *= (float)m_dViewScaleFactor; pvec3MarkerPos->y *= (float)m_dViewScaleFactor; pvec3MarkerPos->z *= (float)m_dViewScaleFactor; }
osg::MatrixTransform* Worker::mt2mtCoordinates(osg::MatrixTransform* mtA, osg::MatrixTransform* mtB) { double matrix1[3][4]; double matrix2[3][4]; // Auxiliar double matrix to calculate distances between OSG objects(matrixTransforms). double matrixA[3][4]; double matrixB[3][4]; // mtA and mtB in double matrix format. osg::MatrixTransform* mtAfromB; mtAfromB = new osg::MatrixTransform(); matrixTransform2matrixDouble(mtA,matrixA); matrixTransform2matrixDouble(mtB,matrixB); // if (osgArt->getMarker1()->valid() || osgArt->getMarker2()->valid()) // { arUtilMatInv(matrixA, matrix1);// Now matrix1 is the inverse of matrixA arUtilMatMul(matrix1, matrixB, matrix2); matrixDouble2matrixTransform(matrix2, mtAfromB); return mtAfromB; // } // else // { // std::cout<<"The ARToolKit marks are out of camera vision. Please show the 2 marks to the camera."<<std::endl; // distance2mole1=9999; // } }
static void getResultRaw( ARMarkerInfo *marker_info ) { double target_trans[3][4]; double cam_trans[3][4]; char string[256]; if( arGetTransMat(marker_info, target_center, target_width, target_trans) < 0 ) return; if( arUtilMatInv(target_trans, cam_trans) < 0 ) return; sprintf(string," RAW: Cam Pos x: %3.1f y: %3.1f z: %3.1f", cam_trans[0][3], cam_trans[1][3], cam_trans[2][3]); if( disp_mode ) { draw( "target", target_trans, 0, 0 ); draw_exview( a, b, r, target_trans, 1, 1 ); } else { draw( "target", target_trans, 1, 1 ); draw_exview( a, b, r, target_trans, 0, 0 ); } print_string( string ); return; }
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 ); }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int j, k; int i; /* grab a vide frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; argDrawMode2D(); argDispImage( dataPtr, 0,0 ); /* detect the markers in the video frame */ if( arDetectMarker(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } arVideoCapNext(); /* check for object visibility */ for( i = 0; i < PTT_NUM; i++){ k = -1; for( j = 0; j < marker_num; j++ ) { if( object[i].patt_id == marker_info[j].id ) { if( k == -1 ) k = j; else if( marker_info[k].cf < marker_info[j].cf ) k = j; } } if( k == -1 ) { /* not find marker */ object[i].visible = 0; isFirst[i] = 1; } else{ /* get the transformation between the marker and the real camera */ if( isFirst[i]){ arGetTransMat(&marker_info[k], object[i].patt_center, object[i].patt_width, object[i].patt_trans); }else{ arGetTransMatCont(&marker_info[k], object[i].patt_trans, object[i].patt_center, object[i].patt_width, object[i].patt_trans); } object[i].visible = 1; isFirst[i] = 0; /* 追加 */ if(i == PTT2_MARK_ID){ arUtilMatInv( object[PTT2_MARK_ID].patt_trans, itrans2); // 逆行列の計算 } } } //Initialize(); // fix me draw(); argSwapBuffers(); }
ARMultiMarkerInfoT *arMultiReadConfigFile( const char *filename, ARPattHandle *pattHandle ) { FILE *fp; ARMultiEachMarkerInfoT *marker; ARMultiMarkerInfoT *marker_info; ARdouble wpos3d[4][2]; char buf[256], pattPath[2048], dummy; int num; int patt_type = 0; int i, j; if ((fp = fopen(filename, "r")) == NULL) { ARLOGe("Error: unable to open multimarker config file '%s'.\n", filename); ARLOGperror(NULL); return NULL; } get_buff(buf, 256, fp); if( sscanf(buf, "%d", &num) != 1 ) { ARLOGe("Error processing multimarker config file '%s': First line must be number of marker configs to read.\n", filename); fclose(fp); return NULL; } ARLOGd("Reading %d markers from multimarker file '%s'\n", num, filename); arMalloc(marker, ARMultiEachMarkerInfoT, num); for( i = 0; i < num; i++ ) { get_buff(buf, 256, fp); if (sscanf(buf, #if defined(__LP64__) && !defined(__APPLE__) "%lu%c", #else "%llu%c", #endif &(marker[i].globalID), &dummy) != 1) { // Try first as matrix code. if (!pattHandle) { ARLOGe("Error processing multimarker config file '%s': pattern '%s' specified in multimarker configuration while in barcode-only mode.\n", filename, buf); goto bail; } if (!arUtilGetDirectoryNameFromPath(pattPath, filename, sizeof(pattPath), 1)) { // Get directory prefix. ARLOGe("Error processing multimarker config file '%s': Unable to determine directory name.\n", filename); goto bail; } strncat(pattPath, buf, sizeof(pattPath) - strlen(pattPath) - 1); // Add name of file to open. if ((marker[i].patt_id = arPattLoad(pattHandle, pattPath)) < 0) { ARLOGe("Error processing multimarker config file '%s': Unable to load pattern '%s'.\n", filename, pattPath); goto bail; } marker[i].patt_type = AR_MULTI_PATTERN_TYPE_TEMPLATE; patt_type |= 0x01; } else { if ((marker[i].globalID & 0xffff8000ULL) == 0ULL) marker[i].patt_id = (int)(marker[i].globalID & 0x00007fffULL); // If upper 33 bits are zero, use lower 31 bits as regular matrix code. else marker[i].patt_id = 0; ARLOGd("Marker %3d is matrix code %llu.\n", i + 1, marker[i].globalID); marker[i].patt_type = AR_MULTI_PATTERN_TYPE_MATRIX; patt_type |= 0x02; } get_buff(buf, 256, fp); if( sscanf(buf, #ifdef ARDOUBLE_IS_FLOAT "%f", #else "%lf", #endif &marker[i].width) != 1 ) { ARLOGe("Error processing multimarker config file '%s', marker definition %3d: First line must be pattern width.\n", filename, i + 1); goto bail; } j = 0; get_buff(buf, 256, fp); if( sscanf(buf, #ifdef ARDOUBLE_IS_FLOAT "%f %f %f %f", #else "%lf %lf %lf %lf", #endif &marker[i].trans[j][0], &marker[i].trans[j][1], &marker[i].trans[j][2], &marker[i].trans[j][3]) != 4 ) { // Perhaps this is an old ARToolKit v2.x multimarker file? // If so, then the next line is two values (center) and should be skipped. float t1, t2; if( sscanf(buf, "%f %f", &t1, &t2) != 2 ) { ARLOGe("Error processing multimarker config file '%s', marker definition %3d: Lines 2 - 4 must be marker transform.\n", filename, i + 1); goto bail; } } else j++; do { get_buff(buf, 256, fp); if( sscanf(buf, #ifdef ARDOUBLE_IS_FLOAT "%f %f %f %f", #else "%lf %lf %lf %lf", #endif &marker[i].trans[j][0], &marker[i].trans[j][1], &marker[i].trans[j][2], &marker[i].trans[j][3]) != 4 ) { ARLOGe("Error processing multimarker config file '%s', marker definition %3d: Lines 2 - 4 must be marker transform.\n", filename, i + 1); goto bail; } j++; } while (j < 3); arUtilMatInv( (const ARdouble (*)[4])marker[i].trans, marker[i].itrans ); wpos3d[0][0] = -marker[i].width/2.0; wpos3d[0][1] = marker[i].width/2.0; wpos3d[1][0] = marker[i].width/2.0; wpos3d[1][1] = marker[i].width/2.0; wpos3d[2][0] = marker[i].width/2.0; wpos3d[2][1] = -marker[i].width/2.0; wpos3d[3][0] = -marker[i].width/2.0; wpos3d[3][1] = -marker[i].width/2.0; for( j = 0; j < 4; j++ ) { marker[i].pos3d[j][0] = marker[i].trans[0][0] * wpos3d[j][0] + marker[i].trans[0][1] * wpos3d[j][1] + marker[i].trans[0][3]; marker[i].pos3d[j][1] = marker[i].trans[1][0] * wpos3d[j][0] + marker[i].trans[1][1] * wpos3d[j][1] + marker[i].trans[1][3]; marker[i].pos3d[j][2] = marker[i].trans[2][0] * wpos3d[j][0] + marker[i].trans[2][1] * wpos3d[j][1] + marker[i].trans[2][3]; } } fclose(fp); arMalloc(marker_info, ARMultiMarkerInfoT, 1); marker_info->marker = marker; marker_info->marker_num = num; marker_info->prevF = 0; if( (patt_type & 0x03) == 0x03 ) marker_info->patt_type = AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE_AND_MATRIX; else if( patt_type & 0x01 ) marker_info->patt_type = AR_MULTI_PATTERN_DETECTION_MODE_TEMPLATE; else marker_info->patt_type = AR_MULTI_PATTERN_DETECTION_MODE_MATRIX; marker_info->cfPattCutoff = AR_MULTI_CONFIDENCE_PATTERN_CUTOFF_DEFAULT; marker_info->cfMatrixCutoff = AR_MULTI_CONFIDENCE_MATRIX_CUTOFF_DEFAULT; return marker_info; bail: fclose(fp); free(marker); return NULL; }
AR_DLL_API ARMultiMarkerInfoT *arMultiReadConfigFile( const char *filename ) { FILE *fp=fopen(filename,"r"); if(fp == NULL){ return NULL; } char buf[256]; get_buff(buf, 256, fp); int num; if( sscanf(buf, "%d", &num) != 1 ) { fclose(fp); return NULL; } ARMultiEachMarkerInfoT *marker; ARMultiMarkerInfoT *marker_info; arMalloc(marker,ARMultiEachMarkerInfoT,num); std::string dir=getdir(filename); for(int i = 0; i < num; i++ ) { get_buff(buf, 256, fp); char buf1[256]; if( sscanf(buf, "%s", buf1) != 1 ) { fclose(fp); free(marker); return NULL; } if( (marker[i].patt_id = arLoadPatt((dir+"/"+buf1).c_str())) < 0 ) { fclose(fp); free(marker); return NULL; } get_buff(buf, 256, fp); if( sscanf(buf, "%lf", &marker[i].width) != 1 ) { fclose(fp); free(marker); return NULL; } get_buff(buf, 256, fp); if( sscanf(buf, "%lf %lf", &marker[i].center[0], &marker[i].center[1]) != 2 ) { fclose(fp); free(marker); return NULL; } for(int j = 0; j < 3; j++ ) { get_buff(buf, 256, fp); if( sscanf(buf, "%lf %lf %lf %lf", &marker[i].trans[j][0], &marker[i].trans[j][1], &marker[i].trans[j][2], &marker[i].trans[j][3]) != 4 ) { fclose(fp); free(marker); return NULL; } } arUtilMatInv( marker[i].trans, marker[i].itrans ); double wpos3d[4][2]; wpos3d[0][0] = marker[i].center[0] - marker[i].width/2.0; wpos3d[0][1] = marker[i].center[1] + marker[i].width/2.0; wpos3d[1][0] = marker[i].center[0] + marker[i].width/2.0; wpos3d[1][1] = marker[i].center[1] + marker[i].width/2.0; wpos3d[2][0] = marker[i].center[0] + marker[i].width/2.0; wpos3d[2][1] = marker[i].center[1] - marker[i].width/2.0; wpos3d[3][0] = marker[i].center[0] - marker[i].width/2.0; wpos3d[3][1] = marker[i].center[1] - marker[i].width/2.0; for(int j = 0; j < 4; j++ ) { marker[i].pos3d[j][0] = marker[i].trans[0][0] * wpos3d[j][0] + marker[i].trans[0][1] * wpos3d[j][1] + marker[i].trans[0][3]; marker[i].pos3d[j][1] = marker[i].trans[1][0] * wpos3d[j][0] + marker[i].trans[1][1] * wpos3d[j][1] + marker[i].trans[1][3]; marker[i].pos3d[j][2] = marker[i].trans[2][0] * wpos3d[j][0] + marker[i].trans[2][1] * wpos3d[j][1] + marker[i].trans[2][3]; } } fclose(fp); marker_info = (ARMultiMarkerInfoT *)malloc( sizeof(ARMultiMarkerInfoT) ); if( marker_info == NULL ) {free(marker); return NULL;} marker_info->marker = marker; marker_info->marker_num = num; marker_info->prevF = 0; return marker_info; }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int i, j, k; /* grab a vide frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; argDrawMode2D(); argDispImage( dataPtr, 0,0 ); /* detect the markers in the video frame */ if( arDetectMarker(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } arVideoCapNext(); argDrawMode3D(); argDraw3dCamera( 0, 0 ); glClearDepth( 1.0 ); glClear(GL_DEPTH_BUFFER_BIT); /* check for object visibility */ for( i = 0; i < 2; i++ ) { k = -1; for( j = 0; j < marker_num; j++ ) { if( object[i].patt_id == marker_info[j].id ) { if( k == -1 ) k = j; else if( marker_info[k].cf < marker_info[j].cf ) k = j; } } object[i].visible = k; if( k >= 0 ) { arGetTransMat(&marker_info[k], object[i].center, object[i].width, object[i].trans); draw( object[i].model_id, object[i].trans ); } } argSwapBuffers(); if( object[0].visible >= 0 && object[1].visible >= 0 ) { double wmat1[3][4], wmat2[3][4]; arUtilMatInv(object[0].trans, wmat1); arUtilMatMul(wmat1, object[1].trans, wmat2); for( j = 0; j < 3; j++ ) { for( i = 0; i < 4; i++ ) printf("%8.4f ", wmat2[j][i]); printf("\n"); } printf("\n\n"); } }
return marker_num; } /* * Class: net_towerdefender_image_ARToolkit * Method: arUtilMatInv * Signature: ([D[D)I */ JNIEXPORT jint JNICALL Java_net_towerdefender_image_ARToolkit_arUtilMatInv( JNIEnv *env, jclass this, jdoubleArray mat1, jdoubleArray mat2) { double *mat1Ptr; double *mat2Ptr; int retval; mat1Ptr = (*env)->GetDoubleArrayElements(env, mat1, JNI_FALSE); mat2Ptr = (*env)->GetDoubleArrayElements(env, mat2, JNI_FALSE); retval = arUtilMatInv(mat1Ptr, mat2Ptr); (*env)->ReleaseDoubleArrayElements(env, mat1, mat1Ptr, 0); (*env)->ReleaseDoubleArrayElements(env, mat2, mat2Ptr, 0); return retval; } /* * Class: net_towerdefender_image_ARToolkit * Method: arUtilMatMul * Signature: ([D[D[D)I */ JNIEXPORT jint JNICALL Java_net_towerdefender_image_ARToolkit_arUtilMatMul( JNIEnv *env, jclass this, jdoubleArray mat1, jdoubleArray mat2, jdoubleArray result) { double *mat1Ptr; double *mat2Ptr;
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; }
ARMultiMarkerInfoT* Tracker::arMultiReadConfigFile(const char *filename) { FILE *fp; ARMultiEachMarkerInfoT *marker; ARMultiMarkerInfoT *marker_info; ARFloat wpos3d[4][2]; char buf[256], buf1[256]; int num; int i, j; setlocale(LC_NUMERIC, "C"); if ((fp = fopen(filename, "r")) == NULL) return NULL; get_buff(buf, 256, fp); if (sscanf(buf, "%d", &num) != 1) { fclose(fp); return NULL; } arMalloc(marker, ARMultiEachMarkerInfoT, num); for (i = 0; i < num; i++) { get_buff(buf, 256, fp); if (sscanf(buf, "%s", buf1) != 1) { fclose(fp); free(marker); return NULL; } // Added by Daniel: if the markername is an integer number // we directly interprete this as the marker id (used for // id-based markers) if (isNumber(buf1)) marker[i].patt_id = atoi(buf1); else if ((marker[i].patt_id = arLoadPatt(buf1)) < 0) { fclose(fp); free(marker); return NULL; } get_buff(buf, 256, fp); #ifdef _USE_DOUBLE_ if( sscanf(buf, "%lf", &marker[i].width) != 1 ) { #else if (sscanf(buf, "%f", &marker[i].width) != 1) { #endif fclose(fp); free(marker); return NULL; } get_buff(buf, 256, fp); #ifdef _USE_DOUBLE_ if( sscanf(buf, "%lf %lf", &marker[i].center[0], &marker[i].center[1]) != 2 ) { #else if (sscanf(buf, "%f %f", &marker[i].center[0], &marker[i].center[1]) != 2) { #endif fclose(fp); free(marker); return NULL; } for (j = 0; j < 3; j++) { get_buff(buf, 256, fp); if (sscanf(buf, #ifdef _USE_DOUBLE_ "%lf %lf %lf %lf", #else "%f %f %f %f", #endif &marker[i].trans[j][0], &marker[i].trans[j][1], &marker[i].trans[j][2], &marker[i].trans[j][3]) != 4) { fclose(fp); free(marker); return NULL; } } arUtilMatInv(marker[i].trans, marker[i].itrans); wpos3d[0][0] = marker[i].center[0] - marker[i].width * 0.5f; wpos3d[0][1] = marker[i].center[1] + marker[i].width * 0.5f; wpos3d[1][0] = marker[i].center[0] + marker[i].width * 0.5f; wpos3d[1][1] = marker[i].center[1] + marker[i].width * 0.5f; wpos3d[2][0] = marker[i].center[0] + marker[i].width * 0.5f; wpos3d[2][1] = marker[i].center[1] - marker[i].width * 0.5f; wpos3d[3][0] = marker[i].center[0] - marker[i].width * 0.5f; wpos3d[3][1] = marker[i].center[1] - marker[i].width * 0.5f; for (j = 0; j < 4; j++) { marker[i].pos3d[j][0] = marker[i].trans[0][0] * wpos3d[j][0] + marker[i].trans[0][1] * wpos3d[j][1] + marker[i].trans[0][3]; marker[i].pos3d[j][1] = marker[i].trans[1][0] * wpos3d[j][0] + marker[i].trans[1][1] * wpos3d[j][1] + marker[i].trans[1][3]; marker[i].pos3d[j][2] = marker[i].trans[2][0] * wpos3d[j][0] + marker[i].trans[2][1] * wpos3d[j][1] + marker[i].trans[2][3]; } } fclose(fp); setlocale(LC_NUMERIC, "C"); marker_info = (ARMultiMarkerInfoT *) malloc(sizeof(ARMultiMarkerInfoT)); if (marker_info == NULL) { free(marker); return NULL; } marker_info->marker = marker; marker_info->marker_num = num; marker_info->prevF = 0; return marker_info; } } // namespace ARToolKitPlus