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