static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *markerInfo; int markerNum; ARdouble patt_trans[3][4]; ARdouble err; int imageProcMode; int debugMode; int j, k; /* grab a video frame */ if ((dataPtr = (ARUint8*)arVideoGetImage()) == NULL) { arUtilSleep(2); return; } /* detect the markers in the video frame */ if (arDetectMarker(arHandle, dataPtr) < 0) { cleanup(); exit(0); } argSetWindow(w1); argDrawMode2D(vp1); arGetDebugMode(arHandle, &debugMode); if (debugMode == 0) { argDrawImage(dataPtr); } else { arGetImageProcMode(arHandle, &imageProcMode); if (imageProcMode == AR_IMAGE_PROC_FRAME_IMAGE) { argDrawImage(arHandle->labelInfo.bwImage); } else { argDrawImageHalf(arHandle->labelInfo.bwImage); } } argSetWindow(w2); argDrawMode2D(vp2); argDrawImage(dataPtr); argSetWindow(w1); if (count % 10 == 0) { sprintf(fps, "%f[fps]", 10.0 / arUtilTimer()); arUtilTimerReset(); } count++; glColor3f(0.0f, 1.0f, 0.0f); argDrawStringsByIdealPos(fps, 10, ysize - 30); markerNum = arGetMarkerNum(arHandle); if (markerNum == 0) { argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); return; } /* check for object visibility */ markerInfo = arGetMarker(arHandle); k = -1; for (j = 0; j < markerNum; j++) { // ARLOG("ID=%d, CF = %f\n", markerInfo[j].id, markerInfo[j].cf); if (patt_id == markerInfo[j].id) { if (k == -1) { if (markerInfo[j].cf > 0.7) k = j; } else if (markerInfo[j].cf > markerInfo[k].cf) k = j; } } if (k == -1) { argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); return; } err = arGetTransMatSquare(ar3DHandle, &(markerInfo[k]), patt_width, patt_trans); sprintf(errValue, "err = %f", err); glColor3f(0.0f, 1.0f, 0.0f); argDrawStringsByIdealPos(fps, 10, ysize - 30); argDrawStringsByIdealPos(errValue, 10, ysize - 60); // ARLOG("err = %f\n", err); draw(patt_trans); argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); }
static void mainLoop( void ) { AR2VideoBufferT *videoBuffL; AR2VideoBufferT *videoBuffR; ARUint8 *dataPtrL; ARUint8 *dataPtrR; int cornerFlagL; int cornerFlagR; int cornerCountL; int cornerCountR; char buf[256]; int i; if ((videoBuffL = ar2VideoGetImage(vidL))) { gVideoBuffL = videoBuffL; } if ((videoBuffR = ar2VideoGetImage(vidR))) { gVideoBuffR = videoBuffR; } if (gVideoBuffL && gVideoBuffR) { // Warn about significant time differences. i = ((int)gVideoBuffR->time_sec - (int)gVideoBuffL->time_sec) * 1000 + ((int)gVideoBuffR->time_usec - (int)gVideoBuffL->time_usec) / 1000; if( i > 20 ) { ARLOG("Time diff = %d[msec]\n", i); } else if( i < -20 ) { ARLOG("Time diff = %d[msec]\n", i); } dataPtrL = gVideoBuffL->buff; dataPtrR = gVideoBuffR->buff; glClear(GL_COLOR_BUFFER_BIT); argDrawMode2D( vpL ); argDrawImage( dataPtrL ); argDrawMode2D( vpR ); argDrawImage( dataPtrR ); copyImage( dataPtrL, (ARUint8 *)calibImageL->imageData, xsizeL*ysizeL, pixFormatL ); cornerFlagL = cvFindChessboardCorners(calibImageL, cvSize(chessboardCornerNumY,chessboardCornerNumX), cornersL, &cornerCountL, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS ); copyImage( dataPtrR, (ARUint8 *)calibImageR->imageData, xsizeR*ysizeR, pixFormatR ); cornerFlagR = cvFindChessboardCorners(calibImageR, cvSize(chessboardCornerNumY,chessboardCornerNumX), cornersR, &cornerCountR, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS ); argDrawMode2D( vpL ); if(cornerFlagL) glColor3f(1.0f, 0.0f, 0.0f); else glColor3f(0.0f, 1.0f, 0.0f); glLineWidth(2.0f); //ARLOG("Detected corners = %d\n", cornerCount); for( i = 0; i < cornerCountL; i++ ) { argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y-5, cornersL[i].x+5, cornersL[i].y+5); argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y+5, cornersL[i].x+5, cornersL[i].y-5); //ARLOG(" %f, %f\n", cornersL[i].x, cornersL[i].y); sprintf(buf, "%d\n", i); argDrawStringsByObservedPos(buf, cornersL[i].x, cornersL[i].y+20); } argDrawMode2D( vpR ); if(cornerFlagR) glColor3f(1.0f, 0.0f, 0.0f); else glColor3f(0.0f, 1.0f, 0.0f); glLineWidth(2.0f); //ARLOG("Detected corners = %d\n", cornerCount); for( i = 0; i < cornerCountR; i++ ) { argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y-5, cornersR[i].x+5, cornersR[i].y+5); argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y+5, cornersR[i].x+5, cornersR[i].y-5); //ARLOG(" %f, %f\n", cornersR[i].x, cornersR[i].y); sprintf(buf, "%d\n", i); argDrawStringsByObservedPos(buf, cornersR[i].x, cornersR[i].y+20); } if( cornerFlagL && cornerFlagR ) { cornerFlag = 1; glColor3f(1.0f, 0.0f, 0.0f); } else { cornerFlag = 0; glColor3f(0.0f, 1.0f, 0.0f); } argDrawMode2D( vpL ); sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum); argDrawStringsByIdealPos(buf, 10, 30); argSwapBuffers(); gVideoBuffL = gVideoBuffR = NULL; } else arUtilSleep(2); }
static void mainLoop(void) { AR2VideoBufferT *buff; ARMarkerInfo *markerInfo; int markerNum; ARdouble patt_trans[3][4]; ARdouble err; int debugMode; int j, k; /* grab a video frame */ buff = arVideoGetImage(); if (!buff || !buff->fillFlag) { arUtilSleep(2); return; } /* detect the markers in the video frame */ if( arDetectMarker(arHandle, buff) < 0 ) { cleanup(); exit(0); } argSetWindow(w1); arGetDebugMode(arHandle, &debugMode); if (debugMode == AR_DEBUG_ENABLE) { int imageProcMode; argViewportSetPixFormat(vp1, AR_PIXEL_FORMAT_MONO); // Drawing the debug image. argDrawMode2D(vp1); arGetImageProcMode(arHandle, &imageProcMode); if (imageProcMode == AR_IMAGE_PROC_FRAME_IMAGE) argDrawImage(arHandle->labelInfo.bwImage); else argDrawImageHalf(arHandle->labelInfo.bwImage); } else { AR_PIXEL_FORMAT pixFormat; arGetPixelFormat(arHandle, &pixFormat); argViewportSetPixFormat(vp1, pixFormat); // Drawing the input image. argDrawMode2D(vp1); argDrawImage(buff->buff); } argSetWindow(w2); argDrawMode2D(vp2); argDrawImage(buff->buff); argSetWindow(w1); if( count % 10 == 0 ) { sprintf(fps, "%f[fps]", 10.0/arUtilTimer()); arUtilTimerReset(); } count++; glColor3f(0.0f, 1.0f, 0.0f); argDrawStringsByIdealPos(fps, 10, ysize-30); markerNum = arGetMarkerNum( arHandle ); if( markerNum == 0 ) { argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); return; } /* check for object visibility */ markerInfo = arGetMarker( arHandle ); k = -1; for( j = 0; j < markerNum; j++ ) { //ARLOG("ID=%d, CF = %f\n", markerInfo[j].id, markerInfo[j].cf); if( patt_id == markerInfo[j].id ) { if( k == -1 ) { if (markerInfo[j].cf > 0.7) k = j; } else if (markerInfo[j].cf > markerInfo[k].cf) k = j; } } if( k == -1 ) { argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); return; } err = arGetTransMatSquare(ar3DHandle, &(markerInfo[k]), patt_width, patt_trans); sprintf(errValue, "err = %f", err); glColor3f(0.0f, 1.0f, 0.0f); argDrawStringsByIdealPos(fps, 10, ysize-30); argDrawStringsByIdealPos(errValue, 10, ysize-60); //ARLOG("err = %f\n", err); draw(patt_trans); argSetWindow(w1); argSwapBuffers(); argSetWindow(w2); argSwapBuffers(); }