自学教程：C++ ARLOG函数代码示例

static void   keyEvent( unsigned char key, int x, int y){    int   value;    /* quit if the ESC key is pressed */    if( key == 0x1b ) {        cleanup();        exit(0);    }    if( key == '1' ) {        arGetLabelingThresh( arHandle, &value );        value -= 5;        if( value < 0 ) value = 0;        arSetLabelingThresh( arHandle, value );        ARLOG("thresh = %d/n", value);    }    if( key == '2' ) {        arGetLabelingThresh( arHandle, &value );        value += 5;        if( value > 255 ) value = 255;        arSetLabelingThresh( arHandle, value );        ARLOG("thresh = %d/n", value);    }    if( key == 'd' ) {        arGetDebugMode( arHandle, &value );        value = 1 - value;        arSetDebugMode( arHandle, value );    }}

static void  keyEvent( unsigned char key, int x, int y){    CvPoint2D32f   *p1, *p2;    int             i;    if( key == 0x1b || key == 'q' || key == 'Q' ) {        cleanup();    }    if( cornerFlag && key==' ' ) {        cvFindCornerSubPix( calibImage, corners, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),                            cvSize(-1,-1), cvTermCriteria (CV_TERMCRIT_ITER, 100, 0.1)  );        p1 = &corners[0];        p2 = &cornerSet[capturedImageNum*chessboardCornerNumX*chessboardCornerNumY];        for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {            *(p2++) = *(p1++);        }        capturedImageNum++;        ARLOG("---------- %2d/%2d -----------/n", capturedImageNum, calibImageNum);        for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {            ARLOG("  %f, %f/n", corners[i].x, corners[i].y);        }        ARLOG("---------- %2d/%2d -----------/n", capturedImageNum, calibImageNum);        if( capturedImageNum == calibImageNum ) {            calib();            cleanup();        }    }}

int main(int argc, char *argv[]){    ARParam cparam;    // ARParamLT          *cparamLT;    float trans[3][4];    float pos[2];    float dpi[2];    // char                name[1024], ext[1024];    int   i, j;    float z;    init(argc, argv);    if (!cpara)        cpara = cparaDefault;    // ar2UtilDivideExt( cpara, name, ext );    // Load the camera parameters, resize for the window and init.    // if( arParamLoad(name, ext, 1, &cparam) < 0 )    if (arParamLoad(cpara, 1, &cparam) < 0)    {        ARLOGe("setupCamera(): Error loading parameter file %s for camera./n", cpara);        exit(-1);    }    if (xsize != -1 && ysize != -1 && (cparam.xsize != xsize || cparam.ysize != ysize))    {        ARLOG("*** Camera Parameter resized from %d, %d. ***/n", cparam.xsize, cparam.ysize);        arParamChangeSize(&cparam, xsize, ysize, &cparam);    }    ARLOG("*** Camera Parameter ***/n");    arParamDisp(&cparam);    // if ((cparamLT = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == NULL) {    //    ARLOGe("setupCamera(): Error: arParamLTCreate./n");    //    exit(-1);    // }    pos[0] = 0.0;    pos[1] = 0.0;    for (j = 0; j < 3; j++)        for (i = 0; i < 4; i++)            trans[j][i] = ((i == j) ? 1.0 : 0.0);    for (i = 10; i <= 1000; i *= 10)    {        for (j = 1; j < 10; j++)        {            z           = j * i;            trans[2][3] = z;            ar2GetResolution2(&cparam, trans, pos, dpi);            ARLOG("Distance: %f [mm] --> Resolution = %10.5f, %10.5f [DPI]/n", z, dpi[0], dpi[1]);        }    }    return (0);}

static void  keyEvent( unsigned char key, int x, int y){    int             i;    if( key == 0x1b || key == 'q' || key == 'Q' ) {        cleanup();    }    if( cornerFlag && key==' ' ) {        cvFindCornerSubPix( calibImageL, cornersL, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),                            cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1)  );        for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {            arParamObserv2Ideal(paramL.dist_factor, (double)cornersL[i].x, (double)cornersL[i].y,                                &calibData[capturedImageNum].screenCoordL[i].x, &calibData[capturedImageNum].screenCoordL[i].y, paramL.dist_function_version);        }        cvFindCornerSubPix( calibImageR, cornersR, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),                            cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1)  );        for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {            arParamObserv2Ideal(paramR.dist_factor, (double)cornersR[i].x, (double)cornersR[i].y,                                &calibData[capturedImageNum].screenCoordR[i].x, &calibData[capturedImageNum].screenCoordR[i].y, paramR.dist_function_version);        }        ARLOG("---------- %2d/%2d -----------/n", capturedImageNum+1, calibImageNum);        for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {            ARLOG("  %f, %f  ----   %f, %f/n", calibData[capturedImageNum].screenCoordL[i].x, calibData[capturedImageNum].screenCoordL[i].y,                  calibData[capturedImageNum].screenCoordR[i].x, calibData[capturedImageNum].screenCoordR[i].y);        }        ARLOG("---------- %2d/%2d -----------/n", capturedImageNum+1, calibImageNum);        capturedImageNum++;        if( capturedImageNum == calibImageNum ) {            calib();            cleanup();        }    }}

static void usage( char *com ){    ARLOG("%s <filename>/n", com);    ARLOG("    -fset     Show fset features./n");    ARLOG("    -fset2    Show fset2 features./n");    ARLOG("%s <filename>/n", com);    exit(0);}

static int init( int argc, char *argv[] ){    ARGViewport		viewport;    char		*filename = NULL;    int			xmax, ymax;    float		xzoom, yzoom;    float		zoom;    int			i;    for( i = 1; i < argc; i++ ) {        if( filename == NULL ) filename = argv[i];        else usage(argv[0] );    }    if (!filename || !filename[0]) usage(argv[0]);    ARLOG("Read ImageSet./n");    ar2UtilRemoveExt( filename );    imageSet = ar2ReadImageSet( filename );    if( imageSet == NULL ) {        ARLOGe("file open error: %s.iset/n", filename );        exit(0);    }    ARLOG("  end./n");    arMalloc(vp, ARGViewportHandle *, imageSet->num);    xmax = ymax = 0;    for( i = 0; i < imageSet->num; i++ ) {        if( imageSet->scale[i]->xsize > xmax ) xmax = imageSet->scale[i]->xsize;        if( imageSet->scale[i]->ysize > ymax ) ymax = imageSet->scale[i]->ysize;    }    xzoom = yzoom = 1.0;    while( xmax > winXsize*xzoom ) xzoom += 1.0;    while( ymax > winYsize*yzoom ) yzoom += 1.0;    if( xzoom > yzoom ) zoom = 1.0/xzoom;    else                zoom = 1.0/yzoom;    winXsize = xmax * zoom;    winYsize = ymax * zoom;    ARLOG("Size = (%d,%d) Zoom = %f/n", xmax, ymax, zoom);    argCreateWindow( winXsize, winYsize );    for( i = 0; i < imageSet->num; i++ ) {        viewport.sx = viewport.sy = 0;        viewport.xsize = imageSet->scale[i]->xsize * zoom;        viewport.ysize = imageSet->scale[i]->ysize * zoom;        vp[i] = argCreateViewport( &viewport );        argViewportSetImageSize( vp[i], imageSet->scale[i]->xsize, imageSet->scale[i]->ysize );        argViewportSetDispMethod( vp[i], AR_GL_DISP_METHOD_TEXTURE_MAPPING_FRAME );        //argViewportSetDispMethod( vp[i], AR_GL_DISP_METHOD_GL_DRAW_PIXELS );        argViewportSetDispMode( vp[i], AR_GL_DISP_MODE_FIT_TO_VIEWPORT );        argViewportSetDistortionMode( vp[i], AR_GL_DISTORTION_COMPENSATE_DISABLE );    }    reportCurrentDPI();        return 0;}

static void saveParam(ARParam *param){    char   *name = NULL, *cwd = NULL;    size_t len;    int    nameOK;    arMalloc(name, char, MAXPATHLEN);    arMalloc(cwd, char, MAXPATHLEN);    if (!getcwd(cwd, MAXPATHLEN))        ARLOGe("Unable to read current working directory./n");    nameOK = 0;    ARLOG("Filename[%s]: ", SAVE_FILENAME);    if (fgets(name, MAXPATHLEN, stdin) != NULL)    {        // Trim whitespace from end of name.        len = strlen(name);        while (len > 0 && (name[len - 1] == '/r' || name[len - 1] == '/n' || name[len - 1] == '/t' || name[len - 1] == ' '))        {            len--;            name[len] = '/0';        }        if (len > 0)        {            nameOK = 1;            if (arParamSave(name, 1, param) < 0)            {                ARLOG("Parameter write error!!/n");            }            else            {                ARLOG("Saved parameter file '%s/%s'./n", cwd, name);            }        }    }    // Try and save with a default name.    if (!nameOK)    {        if (arParamSave(SAVE_FILENAME, 1, param) < 0)        {            ARLOG("Parameter write error!!/n");        }        else        {            ARLOG("Saved parameter file '%s/%s'./n", cwd, SAVE_FILENAME);        }    }    free(name);    free(cwd);}

static void reportCurrentDPI(void){#if AR2_CAPABLE_ADAPTIVE_TEMPLATE    ARLOG("%f[dpi] image. Size = (%d,%d)/n", imageSet->scale[page / (AR2_BLUR_IMAGE_MAX)]->dpi,          imageSet->scale[page / (AR2_BLUR_IMAGE_MAX)]->xsize,          imageSet->scale[page / (AR2_BLUR_IMAGE_MAX)]->ysize);#else    ARLOG("%f[dpi] image. Size = (%d,%d)/n", imageSet->scale[page]->dpi,          imageSet->scale[page]->xsize,          imageSet->scale[page]->ysize);#endif}

static void dispFunc( void ){    int     x, y;    int     i, j;    char    str[32];    glClear( GL_COLOR_BUFFER_BIT );#if AR2_CAPABLE_ADAPTIVE_TEMPLATE    argViewportSetPixFormat( vp[page/AR2_BLUR_IMAGE_MAX], AR_PIXEL_FORMAT_MONO );    argDrawMode2D( vp[page/AR2_BLUR_IMAGE_MAX] );    argDrawImage( imageSet->scale[page/AR2_BLUR_IMAGE_MAX]->imgBWBlur[page%AR2_BLUR_IMAGE_MAX] );#else    argViewportSetPixFormat( vp[page], AR_PIXEL_FORMAT_MONO );    argDrawMode2D( vp[page] );    argDrawImage( imageSet->scale[page]->imgBW );#endif    if (display_fset) {        for( i = 0; i < featureSet->list[page].num; i++ ) {            x = featureSet->list[page].coord[i].x;            y = featureSet->list[page].coord[i].y;            drawFeatureRect( x, y, AR2_DEFAULT_TS1, AR2_DEFAULT_TS2 );            sprintf(str, "%d", i);            glColor3f( 0.0f, 0.0f, 1.0f );            argDrawStringsByObservedPos(str, x, y);        }        ARLOG("fset:  Num of feature points: %d/n", featureSet->list[page].num);    }    if (display_fset2) {        for( i = j = 0; i < refDataSet->num; i++ ) {            if( refDataSet->refPoint[i].refImageNo != page ) continue;            x = refDataSet->refPoint[i].coord2D.x;            y = refDataSet->refPoint[i].coord2D.y;            glColor3f( 0.0f, 1.0f, 0.0f );            argDrawLineByObservedPos(x-5, y-5, x+5, y+5);            argDrawLineByObservedPos(x+5, y-5, x-5, y+5);            j++;        }        ARLOG("fset2: Num of feature points: %d/n", j);#if 0        for (i = 0; i < refDataSet->pageNum; i++) {            for (j = 0; j < refDataSet->pageInfo[i].imageNum; j++) {                if (refDataSet->pageInfo[i].imageInfo[j].imageNo == page) {                    ARLOG("fset2: Image size: %dx%d/n", refDataSet->pageInfo[i].imageInfo[j].width, refDataSet->pageInfo[i].imageInfo[j].height);                }            }        }#endif    }    argSwapBuffers();}

void icpDispMat( char *title, ARdouble *mat, int row, int clm ){    int    i, j;    ARLOG("====== %s ========/n", title);    for( j = 0; j < row; j++ ) {        for( i = 0; i < clm; i++ ) {            ARLOG("%7.5f ", mat[j*clm+i]);        }        ARLOG("/n");    }    ARLOG("-------------------------/n");    return;}

void arUtilPrintMtx16(const ARdouble mtx16[16]){    int i;    for (i = 0; i < 4; i++) {        ARLOG("[% .3f % .3f % .3f] [% 6.1f]/n", mtx16[i], mtx16[i + 4], mtx16[i + 8], mtx16[i + 12]);    }}

void arUtilPrintTransMat(const ARdouble trans[3][4]){    int i;    for (i = 0; i < 3; i++) {        ARLOG("[% .3f % .3f % .3f] [% 6.1f]/n", trans[i][0], trans[i][1], trans[i][2], trans[i][3]);    }}

static void keyEvent(unsigned char key, int x, int y){    int threshChange = 0;	switch (key) {		case 'T':		case 't':			printf("Enter new threshold value (now = %d): ", thresh);			scanf("%d",&thresh); while( getchar()!='/n' );				printf("/n");			break;		case '1':		case '-':			threshChange = -5;			break;		case '2':		case '=':		case '+':			threshChange = 5;			break;		default:			break;	}	if (threshChange) {		thresh += threshChange;		if (thresh < 0) thresh = 0;		if (thresh > 255) thresh = 255;		ARLOG("Threshhold changed to %d./n", thresh);	}}

int ar2VideoDispOptionAndroid( void ){    ARLOG(" -device=Android/n");    ARLOG(" -width=N/n");    ARLOG("    specifies desired width of image./n");    ARLOG(" -height=N/n");    ARLOG("    specifies desired height of image./n");    ARLOG(" -cachedir=/path/to/cache/n");    ARLOG("    Specifies the path in which to look for/store camera parameter cache files./n");    ARLOG("    Default is working directory./n");    ARLOG("/n");        return 0;}

static void usage(char *com){    ARLOG("Usage: %s [options]/n", com);    ARLOG("Options:/n");    ARLOG("  --vconf <video parameter for the camera>/n");    ARLOG("  -cornerx=n: specify the number of corners on chessboard in X direction./n");    ARLOG("  -cornery=n: specify the number of corners on chessboard in Y direction./n");    ARLOG("  -imagenum=n: specify the number of images captured for calibration./n");    ARLOG("  -pattwidth=n: specify the square width in the chessbaord./n");    ARLOG("  -h -help --help: show this message/n");    exit(0);}

static void Keyboard(unsigned char key, int x, int y){	switch (key) {		case 0x1B:						// Quit.		case 'Q':		case 'q':			cleanup();			exit(0);			break;		case '?':		case '/':			ARLOG("Keys:/n");			ARLOG(" q or [esc]    Quit demo./n");			ARLOG(" ? or /        Show this help./n");			ARLOG("/nAdditionally, the ARVideo library supplied the following help text:/n");			arVideoDispOption();			break;		default:			break;	}}

static int setupCamera(const char *cparam_name, char *vconf, ARParamLT **cparamLT_p){	    ARParam			cparam;	int				xsize, ysize;    AR_PIXEL_FORMAT pixFormat;    // Open the video path.    if (arVideoOpen(vconf) < 0) {    	ARLOGe("setupCamera(): Unable to open connection to camera./n");    	return (FALSE);	}	    // Find the size of the window.    if (arVideoGetSize(&xsize, &ysize) < 0) {        ARLOGe("setupCamera(): Unable to determine camera frame size./n");        arVideoClose();        return (FALSE);    }    ARLOGi("Camera image size (x,y) = (%d,%d)/n", xsize, ysize);		// Get the format in which the camera is returning pixels.	pixFormat = arVideoGetPixelFormat();	if (pixFormat == AR_PIXEL_FORMAT_INVALID) {    	ARLOGe("setupCamera(): Camera is using unsupported pixel format./n");        arVideoClose();		return (FALSE);	}		// Load the camera parameters, resize for the window and init.    if (arParamLoad(cparam_name, 1, &cparam) < 0) {		ARLOGe("setupCamera(): Error loading parameter file %s for camera./n", cparam_name);        arVideoClose();        return (FALSE);    }    if (cparam.xsize != xsize || cparam.ysize != ysize) {        ARLOGw("*** Camera Parameter resized from %d, %d. ***/n", cparam.xsize, cparam.ysize);        arParamChangeSize(&cparam, xsize, ysize, &cparam);    }#ifdef DEBUG    ARLOG("*** Camera Parameter ***/n");    arParamDisp(&cparam);#endif    if ((*cparamLT_p = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == NULL) {        ARLOGe("setupCamera(): Error: arParamLTCreate./n");        arVideoClose();        return (FALSE);    }		return (TRUE);}

static int setupMarkersObjects(char *objectDataFilename, ObjectData_T **objectDataRef, int *objectDataCountRef, ARHandle *arhandle){    // Load in the object data - trained markers and associated bitmap files.    if ((*objectDataRef = read_VRMLdata(objectDataFilename, objectDataCountRef)) == NULL) {        ARLOGe("setupMarkersObjects(): read_VRMLdata returned error !!/n");        return (FALSE);    }    ARLOG("Object count = %d/n", *objectDataCountRef);    arPattAttach(arhandle, (*objectDataRef)->pattHandle);    arSetLabelingMode(arhandle, (prefMarkersHaveWhiteBorders ? AR_LABELING_WHITE_REGION : AR_LABELING_BLACK_REGION));    return (TRUE);}

static void init(int argc, char *argv[]){    int                i;    int                gotTwoPartOption;        i = 1; // argv[0] is name of app, so start at 1.    while (i < argc) {        gotTwoPartOption = FALSE;        // Look for two-part options first.        if ((i + 1) < argc) {            if (strcmp(argv[i], "--cpara") == 0) {                i++;                cpara = argv[i];                gotTwoPartOption = TRUE;            }        }        if (!gotTwoPartOption) {            // Look for single-part options.            if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "-h") == 0) {                usage(argv[0]);            } else if (strcmp(argv[i], "--version") == 0 || strcmp(argv[i], "-version") == 0 || strcmp(argv[i], "-v") == 0) {                ARLOG("%s version %s/n", argv[0], AR_HEADER_VERSION_STRING);                exit(0);            } else if( strncmp(argv[i], "-loglevel=", 10) == 0 ) {                if (strcmp(&(argv[i][10]), "DEBUG") == 0) arLogLevel = AR_LOG_LEVEL_DEBUG;                else if (strcmp(&(argv[i][10]), "INFO") == 0) arLogLevel = AR_LOG_LEVEL_INFO;                else if (strcmp(&(argv[i][10]), "WARN") == 0) arLogLevel = AR_LOG_LEVEL_WARN;                else if (strcmp(&(argv[i][10]), "ERROR") == 0) arLogLevel = AR_LOG_LEVEL_ERROR;                else usage(argv[0]);            } else if( strncmp(argv[i], "-width=", 7) == 0 ) {                if( sscanf(&(argv[i][7]), "%d", &xsize) != 1 ) usage(argv[0]);                if( xsize <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-height=", 8) == 0 ) {                if( sscanf(&(argv[i][8]), "%d", &ysize) != 1 ) usage(argv[0]);                if( ysize <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-cpara=", 7) == 0 ) {                cpara = &(argv[i][7]);            } else {                if (!cpara) cpara = argv[i];                else {                    ARLOGe("Error: invalid command line argument '%s'./n", argv[i]);                    usage(argv[0]);                }            }        }        i++;    }}

static void usage(char *com){    ARLOG("Usage: %s [options] <camera parameter>/n", com);    ARLOG("  -width=w: scale the camera parameter to width w./n");    ARLOG("  -height=h: scale the camera parameter to height h./n");    ARLOG("  --cpara <camera parameter file>/n");    ARLOG("  -cpara=<camera parameter file>/n");    ARLOG("  -h -help --help: show this message/n");    exit(0);}

static int setupOptical(const char *optical_param_name, ARdouble *fovy_p, ARdouble *aspect_p, ARdouble m[16], const ARdouble scale){	// Load the optical parameters.    if (arParamLoadOptical(optical_param_name, fovy_p, aspect_p, m) < 0) {		ARLOGe("setupOptical(): Error loading optical parameters from file %s./n", optical_param_name);        return (FALSE);    }    ARLOG("*** Optical parameters ***/n");    arParamDispOptical(*fovy_p, *aspect_p, m);		if (scale != 0.0) {		m[12] *= scale;		m[13] *= scale;		m[14] *= scale;	}		return (TRUE);}

static void usage(char *com){    ARLOG("Usage: %s [options]/n", com);    ARLOG("Options:/n");    ARLOG("  --vconf <video parameter for the camera>/n");    ARLOG("  --cpara <camera parameter file for the camera>/n");    ARLOG("  -cpara=<camera parameter file for the camera>/n");	ARLOG("  --width w     Use display/window width of w pixels./n");	ARLOG("  --height h    Use display/window height of h pixels./n");	ARLOG("  --refresh f   Use display refresh rate of f Hz./n");	ARLOG("  --windowed    Display in window, rather than fullscreen./n");	ARLOG("  --fullscreen  Display fullscreen, rather than in window./n");    ARLOG("  -h -help --help: show this message/n");    exit(0);}

int main(int argc, char** argv){	char    glutGamemode[32] = "";    char   *vconf = NULL;    char    cparaDefault[] = "../share/artoolkit-examples/Data/camera_para.dat";    char   *cpara = NULL;    int     i;    int     gotTwoPartOption;    const char markerConfigDataFilename[] = "../share/artoolkit-examples/Data/markers.dat";	const char objectDataFilename[] = "../share/artoolkit-examples/Data/objects.dat";    //	// Process command-line options.	//    	glutInit(&argc, argv);        i = 1; // argv[0] is name of app, so start at 1.    while (i < argc) {        gotTwoPartOption = FALSE;        // Look for two-part options first.        if ((i + 1) < argc) {            if (strcmp(argv[i], "--vconf") == 0) {                i++;                vconf = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i], "--cpara") == 0) {                i++;                cpara = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--width") == 0) {                i++;                // Get width from second field.                if (sscanf(argv[i], "%d", &prefWidth) != 1) {                    ARLOGe("Error: --width option must be followed by desired width./n");                }                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--height") == 0) {                i++;                // Get height from second field.                if (sscanf(argv[i], "%d", &prefHeight) != 1) {                    ARLOGe("Error: --height option must be followed by desired height./n");                }                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--refresh") == 0) {                i++;                // Get refresh rate from second field.                if (sscanf(argv[i], "%d", &prefRefresh) != 1) {                    ARLOGe("Error: --refresh option must be followed by desired refresh rate./n");                }                gotTwoPartOption = TRUE;            }        }        if (!gotTwoPartOption) {            // Look for single-part options.            if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "-h") == 0) {                usage(argv[0]);            } else if (strncmp(argv[i], "-cpara=", 7) == 0) {                cpara = &(argv[i][7]);            } else if (strcmp(argv[i], "--version") == 0 || strcmp(argv[i], "-version") == 0 || strcmp(argv[i], "-v") == 0) {                ARLOG("%s version %s/n", argv[0], AR_HEADER_VERSION_STRING);                exit(0);            } else if (strcmp(argv[i],"--windowed") == 0) {                prefWindowed = TRUE;            } else if (strcmp(argv[i],"--fullscreen") == 0) {                prefWindowed = FALSE;            } else {                ARLOGe("Error: invalid command line argument '%s'./n", argv[i]);                usage(argv[0]);            }        }        i++;    }    	//	// Video setup.	//    	if (!setupCamera((cpara ? cpara : cparaDefault), vconf, &gCparamLT)) {		ARLOGe("main(): Unable to set up AR camera./n");		exit(-1);	}    //    // AR init.    //        // Init AR.    gARPattHandle = arPattCreateHandle();	if (!gARPattHandle) {		ARLOGe("Error creating pattern handle./n");		exit(-1);	}        gARHandle = arCreateHandle(gCparamLT);    if (!gARHandle) {        ARLOGe("Error creating AR handle./n");		exit(-1);    }    arPattAttach(gARHandle, gARPattHandle);//.........这里部分代码省略.........

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

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 init(int argc, char *argv[]){    char              *vconfL = NULL;    char              *vconfR = NULL;    char              *cparaL = NULL;    char              *cparaR = NULL;    char               cparaLDefault[] = "Data/cparaL.dat";    char               cparaRDefault[] = "Data/cparaR.dat";    ARParam            wparam;    ARGViewport        viewport;    int                i, j;    int                gotTwoPartOption;    int                screenWidth, screenHeight, screenMargin;    double             wscalef, hscalef, scalef;    chessboardCornerNumX = 0;    chessboardCornerNumY = 0;    calibImageNum        = 0;    patternWidth         = 0.0f;    i = 1; // argv[0] is name of app, so start at 1.    while (i < argc) {        gotTwoPartOption = FALSE;        // Look for two-part options first.        if ((i + 1) < argc) {            if (strcmp(argv[i], "--vconfL") == 0) {                i++;                vconfL = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i], "--vconfR") == 0) {                i++;                vconfR = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i], "--cparaL") == 0) {                i++;                cparaL = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i], "--cparaR") == 0) {                i++;                cparaR = argv[i];                gotTwoPartOption = TRUE;            }        }        if (!gotTwoPartOption) {            // Look for single-part options.            if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "-h") == 0) {                usage(argv[0]);            } else if (strcmp(argv[i], "--version") == 0 || strcmp(argv[i], "-version") == 0 || strcmp(argv[i], "-v") == 0) {                ARLOG("%s version %s/n", argv[0], AR_HEADER_VERSION_STRING);                exit(0);            } else if( strncmp(argv[i], "-cornerx=", 9) == 0 ) {                if( sscanf(&(argv[i][9]), "%d", &chessboardCornerNumX) != 1 ) usage(argv[0]);                if( chessboardCornerNumX <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-cornery=", 9) == 0 ) {                if( sscanf(&(argv[i][9]), "%d", &chessboardCornerNumY) != 1 ) usage(argv[0]);                if( chessboardCornerNumY <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-imagenum=", 10) == 0 ) {                if( sscanf(&(argv[i][10]), "%d", &calibImageNum) != 1 ) usage(argv[0]);                if( calibImageNum <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-pattwidth=", 11) == 0 ) {                if( sscanf(&(argv[i][11]), "%f", &patternWidth) != 1 ) usage(argv[0]);                if( patternWidth <= 0 ) usage(argv[0]);            } else if( strncmp(argv[i], "-cparaL=", 8) == 0 ) {                cparaL = &(argv[i][8]);            } else if( strncmp(argv[i], "-cparaR=", 8) == 0 ) {                cparaR = &(argv[i][8]);            } else {                ARLOGe("Error: invalid command line argument '%s'./n", argv[i]);                usage(argv[0]);            }        }        i++;    }    if( chessboardCornerNumX == 0 ) chessboardCornerNumX = CHESSBOARD_CORNER_NUM_X;    if( chessboardCornerNumY == 0 ) chessboardCornerNumY = CHESSBOARD_CORNER_NUM_Y;    if( calibImageNum == 0 )        calibImageNum = CALIB_IMAGE_NUM;    if( patternWidth == 0.0f )      patternWidth = (float)CHESSBOARD_PATTERN_WIDTH;    if (!cparaL) cparaL = cparaLDefault;    if (!cparaR) cparaR = cparaRDefault;    ARLOG("CHESSBOARD_CORNER_NUM_X = %d/n", chessboardCornerNumX);    ARLOG("CHESSBOARD_CORNER_NUM_Y = %d/n", chessboardCornerNumY);    ARLOG("CHESSBOARD_PATTERN_WIDTH = %f/n", patternWidth);    ARLOG("CALIB_IMAGE_NUM = %d/n", calibImageNum);    ARLOG("Video parameter Left : %s/n", vconfL);    ARLOG("Video parameter Right: %s/n", vconfR);    ARLOG("Camera parameter Left : %s/n", cparaL);    ARLOG("Camera parameter Right: %s/n", cparaR);    if( (vidL=ar2VideoOpen(vconfL)) == NULL ) {        ARLOGe("Cannot found the first camera./n");        exit(0);    }    if( (vidR=ar2VideoOpen(vconfR)) == NULL ) {        ARLOGe("Cannot found the second camera./n");        exit(0);    }    if( ar2VideoGetSize(vidL, &xsizeL, &ysizeL) < 0 ) exit(0);    if( ar2VideoGetSize(vidR, &xsizeR, &ysizeR) < 0 ) exit(0);//.........这里部分代码省略.........

static void   init(int argc, char *argv[]){    ARParam         cparam;    ARGViewport     viewport;    ARPattHandle    *arPattHandle;    char            vconf[512];    AR_PIXEL_FORMAT pixFormat;    ARUint32        id0, id1;    int             i;    if (argc == 1)        vconf[0] = '/0';    else    {        strcpy(vconf, argv[1]);        for (i = 2; i < argc; i++)        {            strcat(vconf, " "); strcat(vconf, argv[i]);        }    }    /* open the video path */    if (arVideoOpen(vconf) < 0)        exit(0);    if (arVideoGetSize(&xsize, &ysize) < 0)        exit(0);    ARLOG("Image size (x,y) = (%d,%d)/n", xsize, ysize);    if ((pixFormat = arVideoGetPixelFormat()) < 0)        exit(0);    if (arVideoGetId(&id0, &id1) == 0)    {        ARLOG("Camera ID = (%08x, %08x)/n", id1, id0);        sprintf(vconf, VPARA_NAME, id1, id0);        if (arVideoLoadParam(vconf) < 0)        {            ARLOGe("No camera setting data!!/n");        }    }    /* set the initial camera parameters */    if (arParamLoad(CPARA_NAME, 1, &cparam) < 0)    {        ARLOGe("Camera parameter load error !!/n");        exit(0);    }    arParamChangeSize(&cparam, xsize, ysize, &cparam);    ARLOG("*** Camera Parameter ***/n");    arParamDisp(&cparam);    if ((gCparamLT = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) == NULL)    {        ARLOGe("Error: arParamLTCreate./n");        exit(-1);    }    if ((arHandle = arCreateHandle(gCparamLT)) == NULL)    {        ARLOGe("Error: arCreateHandle./n");        exit(0);    }    if (arSetPixelFormat(arHandle, pixFormat) < 0)    {        ARLOGe("Error: arSetPixelFormat./n");        exit(0);    }    if ((ar3DHandle = ar3DCreateHandle(&cparam)) == NULL)    {        ARLOGe("Error: ar3DCreateHandle./n");        exit(0);    }    if ((arPattHandle = arPattCreateHandle()) == NULL)    {        ARLOGe("Error: arPattCreateHandle./n");        exit(0);    }    if ((patt_id = arPattLoad(arPattHandle, PATT_NAME)) < 0)    {        ARLOGe("pattern load error !!/n");        exit(0);    }    arPattAttach(arHandle, arPattHandle);    /* open the graphics window */    w1             = argCreateWindow(xsize, ysize);    viewport.sx    = 0;    viewport.sy    = 0;    viewport.xsize = xsize;    viewport.ysize = ysize;    if ((vp1 = argCreateViewport(&viewport)) == NULL)        exit(0);//.........这里部分代码省略.........

int icpGetDeltaS( ARdouble S[6], ARdouble dU[], ARdouble J_U_S[][6], int n ){    ARMat   matS, matU, matJ;    ARMat  *matJt, *matJtJ, *matJtU;    matS.row = 6;    matS.clm = 1;    matS.m   = S;    matU.row = n;    matU.clm = 1;    matU.m   = dU;    matJ.row = n;    matJ.clm = 6;    matJ.m   = &J_U_S[0][0];#if ICP_DEBUG    ARLOG("cc1 - matU/n");    arMatrixDisp( &matU );    ARLOG("cc1 - matJ/n");    arMatrixDisp( &matJ );#endif    matJt = arMatrixAllocTrans( &matJ );    if( matJt == NULL ) return -1;#if ICP_DEBUG    ARLOG("cc1 - matJt/n");    arMatrixDisp( matJt );#endif    matJtJ = arMatrixAllocMul( matJt, &matJ );    if( matJtJ == NULL ) {        arMatrixFree( matJt );        return -1;    }#if ICP_DEBUG    ARLOG("cc2 - matJtJ/n");    arMatrixDisp( matJtJ );#endif    matJtU = arMatrixAllocMul( matJt, &matU );    if( matJtU == NULL ) {        arMatrixFree( matJt );        arMatrixFree( matJtJ );        return -1;    }#if ICP_DEBUG    ARLOG("cc3 -- matJtU/n");    arMatrixDisp( matJtU );#endif    if( arMatrixSelfInv(matJtJ) < 0 ) {        arMatrixFree( matJt );        arMatrixFree( matJtJ );        arMatrixFree( matJtU );        return -1;    }#if ICP_DEBUG    ARLOG("cc4 -- matJtJ_Inv/n");    arMatrixDisp( matJtJ );#endif    arMatrixMul( &matS, matJtJ, matJtU );    arMatrixFree( matJt );    arMatrixFree( matJtJ );    arMatrixFree( matJtU );#if ICP_DEBUG    ARLOG("cc5 -- matS/n");    arMatrixDisp( &matS );#endif    return 0;}

int ar2VideoDispOptionWinMC(void){    ARLOG(" -device=WinMC/n");	ARLOG(" -devNum=N/n");    ARLOG("    Use device number N (default N=1)./n");    ARLOG("    e.g. for a second camera input, N=2./n");	ARLOG(" -format=X/n");    ARLOG("    Return images with pixels in format X, where X is one of: /n");    ARLOG("    BGRA, BGR, NV12/420f, 2vuy/UYVY, yuvs/YUY2, RGB_565, RGBA_5551./n");    ARLOG(" -width=N/n");    ARLOG("    Request video format of width N pixels./n");    ARLOG(" -height=N/n");    ARLOG("    Request video format of height N pixels./n");	ARLOG(" -position=(rear|back|front|left|right|top|bottom|default)/n");    ARLOG("    choose between rear/back and front-mounted camera (where available)./n");    ARLOG("    other options include cameras mounted on left, right, top or bottom edges of the device./n");	ARLOG(" -flipV/n");    ARLOG("    Flip the image vertically./n");	ARLOG(" -noFlipV/n");    ARLOG("    Do not flip the image vertically./n");	//ARLOG(" -flipH/n");    //ARLOG("    Flip the image horizontally./n");	//ARLOG(" -noFlipH/n");    //ARLOG("    Do not flip the image horizontally./n");    ARLOG("/n");    return 0;}

int main(int argc, char** argv){	char    glutGamemode[32] = "";    char   *vconf = NULL;    char    cparaDefault[] = "Data2/camera_para.dat";    char   *cpara = NULL;    int     i;    int     gotTwoPartOption;    const char markerConfigDataFilename[] = "Data2/markers.dat";	const char objectDataFilename[] = "Data2/objects.dat";	#ifdef DEBUG    arLogLevel = AR_LOG_LEVEL_DEBUG;#endif        //	// Process command-line options.	//    	glutInit(&argc, argv);        i = 1; // argv[0] is name of app, so start at 1.    while (i < argc) {        gotTwoPartOption = FALSE;        // Look for two-part options first.        if ((i + 1) < argc) {            if (strcmp(argv[i], "--vconf") == 0) {                i++;                vconf = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i], "--cpara") == 0) {                i++;                cpara = argv[i];                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--width") == 0) {                i++;                // Get width from second field.                if (sscanf(argv[i], "%d", &prefWidth) != 1) {                    ARLOGe("Error: --width option must be followed by desired width./n");                }                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--height") == 0) {                i++;                // Get height from second field.                if (sscanf(argv[i], "%d", &prefHeight) != 1) {                    ARLOGe("Error: --height option must be followed by desired height./n");                }                gotTwoPartOption = TRUE;            } else if (strcmp(argv[i],"--refresh") == 0) {                i++;                // Get refresh rate from second field.                if (sscanf(argv[i], "%d", &prefRefresh) != 1) {                    ARLOGe("Error: --refresh option must be followed by desired refresh rate./n");                }                gotTwoPartOption = TRUE;            }        }        if (!gotTwoPartOption) {            // Look for single-part options.            if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 || strcmp(argv[i], "-h") == 0) {                usage(argv[0]);            } else if (strncmp(argv[i], "-cpara=", 7) == 0) {                cpara = &(argv[i][7]);            } else if (strcmp(argv[i], "--version") == 0 || strcmp(argv[i], "-version") == 0 || strcmp(argv[i], "-v") == 0) {                ARLOG("%s version %s/n", argv[0], AR_HEADER_VERSION_STRING);                exit(0);            } else if (strcmp(argv[i],"--windowed") == 0) {                prefWindowed = TRUE;            } else if (strcmp(argv[i],"--fullscreen") == 0) {                prefWindowed = FALSE;            } else {                ARLOGe("Error: invalid command line argument '%s'./n", argv[i]);                usage(argv[0]);            }        }        i++;    }    	//	// Video setup.	//    	if (!setupCamera((cpara ? cpara : cparaDefault), vconf, &gCparamLT)) {		ARLOGe("main(): Unable to set up AR camera./n");		exit(-1);	}    //    // AR init.    //        if (!initNFT(gCparamLT, arVideoGetPixelFormat())) {		ARLOGe("main(): Unable to init NFT./n");		exit(-1);    }    //    // Markers setup.    ////.........这里部分代码省略.........