capimagestereo.cpp

1394 接口视觉工具箱 （英文工具箱

		mexErrMsgTxt("Error while checking for connected cameras.\n");

	} else {

		numCameras++;

	}


	/* this is a stereo driver  --  abort if there are not at least 2 cameras... */
	if(numCameras < 2) {

		mexErrMsgTxt("Couldn't find at least two cameras on the bus.\n");

	} else {

		/* ensure that there are at least 2 cameras on the bus */
		mexPrintf("Detected %d cameras...\n", numCameras);
		
	}


    /* only initialize the camera if no CAMERA_STOP request has been issued */
    if(myCameraModes(mode) != CAMERA_STOP) {
        
        /* stop all cameras - if necessary */
		if(initCamera) {

			//mexPrintf("Stopping all cameras\n");
			
			for(int cam=0; cam<numCameras; cam++) {

				/* select current camera */
				Camera[cam].SelectCamera(cam);

				/* stop selected camera */
				Camera[cam].StopImageCapture();

			} /* for */

			//mexPrintf("All cameras stopped\n");

			/* all cameras stopped */
			initCamera = 0;

			/* indicate mode change */
			currentMode = mode;

		} /* if */
        
		
		/* (re-)initialize all cameras */
		for(int cam=0; cam<numCameras; cam++) {

			/* select current camera */
			Camera[cam].SelectCamera(cam);

			if(Camera[cam].InitCamera() != CAM_SUCCESS)
				mexErrMsgTxt("Error during initialization of the camera.\n");
			////mexPrintf("Camera successfully initialized\n");
        

			/* set image buffer, define pixel format */
			// 0:		{160 ,120 ,COLOR_CODE_YUV444},
			// 1:		{320 ,240 ,COLOR_CODE_YUV422},
			// 2:		{640 ,480 ,COLOR_CODE_YUV411},
			// 3:		{640 ,480 ,COLOR_CODE_YUV422},
			// 4:		{640 ,480 ,COLOR_CODE_RGB8},
			// 5:		{640 ,480 ,COLOR_CODE_Y8},
			// 6:		{640 ,480 ,COLOR_CODE_Y16}

			/* only set up camera, if the requested mode is supported by the connected camera */
			if(Camera[cam].HasVideoMode(0L, mode)) {

				/* camera supports the requested mode */
				switch(mode) {
            
					case CAMERA_YUV444_160x120:
                
						/* 160x120 YUV444, 30 fps */
						//mexPrintf("Mode %d -> Selecting YUV444 sink (30 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(0);			/* 0: YUV(4:4:4) */
						Camera[cam].SetVideoFrameRate(4);		/* 4: 30 fps */
                
						break;
                
					case CAMERA_YUV422_320x240:
                
						/* 320x240 YUV422, 30 fps */
						//mexPrintf("Mode %d -> Selecting YUV422 sink (30 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(1);			/* 1: YUV(4:2:2) */
						Camera[cam].SetVideoFrameRate(4);		/* 4: 30 fps */
                
						break;
                
					case CAMERA_YUV411_640x480:
					default:

						/* 640x480 YUV411, 30 fps */
						mexPrintf("Mode: %d -> Selecting YUV411 sink (30 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(2);			/* 2: YUV(4:1:1) */
						Camera[cam].SetVideoFrameRate(4);		/* 4: 30 fps */

						break;
                    
					case CAMERA_YUV422_640x480:
                
						/* 640x480 YUV422, 15 fps */
						//mexPrintf("Mode %d -> Selecting YUV422 sink (15 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(3);			/* 3: YUV(4:2:2) */
						Camera[cam].SetVideoFrameRate(3);		/* 3: 15 fps */
                
						break;
                
					case CAMERA_RGB8_640x480:
                
						/* 640x480 RGB, 15 fps */
						mexPrintf("Mode: %d -> Selecting RGB sink (15 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(4);			/* 4: RGB8 */
						Camera[cam].SetVideoFrameRate(3);		/* 3: 15 fps */
                
						break;
                
					case CAMERA_Y8_640x480:
                
						/* 640x480 B&W, 8 bit per pixel, 30 fps */
						mexPrintf("Mode: %d -> Selecting B&W sink (30 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(5);			/* 5: Mono, 8-bit */
						Camera[cam].SetVideoFrameRate(4);		/* 4: 30 fps */
                
						break;
                
					case CAMERA_Y16_640x480:
                
						/* 640x480 B&W 16 bit per pixel, 15 fps */
						mexPrintf("Mode: %d -> Selecting B&W sink (15 fps)\n", mode);
						Camera[cam].SetVideoFormat(0);
						Camera[cam].SetVideoMode(6);			/* 6: Mono, 16-bit */
						Camera[cam].SetVideoFrameRate(3);		/* 3: 15 fps */
                
						break;
                
				} /* switch */
        

				// set dimension of the output variable
				Camera[cam].GetVideoFrameDimensions(&frameWidth, &frameHeight);
	
				/* fix dimensions */
				dims[0] = (unsigned int)frameHeight;
				dims[1] = (unsigned int)frameWidth;
				dims[2] = 3;

				/* total number of bytes kept in the buffer */
				nElements = dims[0] * dims[1] * dims[2];

				
				if(Camera[cam].StartImageCapture() != CAM_SUCCESS)
					mexErrMsgTxt("Error at attempted start of image capture.\n");

				/* load camera settings or use default values (can be found using '1394CameraDemo.exe') */
				myConfigCamera(cam);

  
				/* fetch one image to waste some time (camera settles) ... */
				for(int ii=0; ii<2; ii++) {
					if(Camera[cam].CaptureImage() != CAM_SUCCESS)
						mexErrMsgTxt("Error during image capture.\n");
				}


		//		/* doesn't work... */
		//		Sleep( 250 );

				/* camera initialized -> set 'initFlag' */
				mexPrintf("Setting flag initCamera\n");
				initCamera = 1;

				/* indicate mode change */
				currentMode = mode;


			} else {

				/* camera does not support the requested mode */
				mexErrMsgTxt("Requested mode is not supported by connected camera.\n");

			}
        
		}  /* for all cameras */


	} else {
        
		/* CAMERA_STOP request has been issued */
        mexPrintf("Stopping camera...\n");
        
        /* stop capture - if currently ongoing */
		if(initCamera) {

			mexPrintf("Stopping all cameras\n");
			
			for(int cam=0; cam<numCameras; cam++) {

				/* select current camera */
				Camera[cam].SelectCamera(cam);

				/* stop selected camera */
				Camera[cam].StopImageCapture();

			} /* for */

		}

		//mexPrintf("All cameras stopped\n");

        /* indicate that we can free the grabber (in mexFunction) */
        initCamera = 0;
        
		/* indicate mode change */
		currentMode = mode;

    }
    
} /* mySetupCamera ============================================================ */



/* mexFunction ============================================================= */
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
    
mxArray		    *pArray;                    /* output data */
unsigned char	*MATbuf;                    /* pointer to MATLAB memory */
    
myCameraModes	mode;

    
    /************************************************************************/
    /* Check arguments and read input (number of pixels)			        */
    /************************************************************************/
    
	/* should be possible to stop the camera using 'capImageStereo(-1);' */
	if(nlhs==0 && nrhs ==1) {

		mode = myCameraModes((int)(mxGetScalar(prhs[0])));
		if(mode == CAMERA_STOP) {

			/* stop acquisition - if currently ongoing */
			if(initCamera) {

				/* stop camera */
				mySetupCamera(CAMERA_STOP);

			}

			return;
			
		}

	}

	
    if(!(nlhs==2 && (nrhs==1 || nrhs==2 || nrhs==4 || nrhs==6))) {
        
        mexErrMsgTxt("Usage:   [rgb1, rgb2] = capImageStereo(mode [, verbose][, sizeX, sizeY][, originX, originY])\n    " \
					 "mode:    -1 STOP\n" \
					 "              0 YUV444 (160x120, 30 fps)\n" \
					 "              1 YUV422 (320x240, 30 fps)\n" \
					 "              2 YUV411 (640x480, 30 fps)\n" \
					 "              3 YUV422 (640x480, 15 fps)\n" \
					 "              4 RGB8   (640x480, 15 fps)\n" \
					 "              5 Y8     (640x480, 30 fps)\n" \
					 "              6 Y16    (640x480, 15 fps)\n" \
					 "    verbose:  0 silent\n" \
					 "              1 verbose\n");

    }
    else {
        
        /* get 'mode' parameter */
        mode = myCameraModes((int)(mxGetScalar(prhs[0])));
        //mexPrintf("mode = %d\n", mode);
        //mexPrintf("initCamera = %d\n", initCamera);

        // defaults
		verboseFlag     = 0;
		requestedWidth  = 0;
		requestedHeight = 0;
		requestedOrigX  = 0;
		requestedOrigY  = 0;


		/* optional: verbosity flag */
		if(nrhs >= 2) {
	        verboseFlag     = (unsigned int)(mxGetScalar(prhs[1]));
		}
        
		/* optional: size */
		if(nrhs >= 4) {
			requestedWidth  = (unsigned int)(mxGetScalar(prhs[2]));
			requestedHeight = (unsigned int)(mxGetScalar(prhs[3]));
		}
        
		/* optional: origin */
		if(nrhs == 6) {
			requestedOrigX  = (unsigned int)(mxGetScalar(prhs[4]));
			requestedOrigY  = (unsigned int)(mxGetScalar(prhs[5]));
		}
        
    }


    /* initialize camera - this is done everytime there's a mode change... */
    if(currentMode != mode) {
        
		/* (re)initialize camera */
		//mexPrintf("Setting up camera(s)...\n");
        mySetupCamera(mode);

    }
    
    
    /* resize... if necessary */
    if(	currWidth  != requestedWidth  || \
		currHeight != requestedHeight || \
		currOrigX  != requestedOrigX  || \
		currOrigY  != requestedOrigY     ) {
        
		/* set requested ORIGIN (X)  of the image to be returned to MATLAB */
		if(requestedOrigX >= 0 && requestedOrigX < frameWidth) {
			
			/* change of origin (X) */
			origX = requestedOrigX;
			
		} else {
			
			/* default origin (X) */
			origX = 0;
			//mexPrintf("Requested origin-x (%d) outside valid limits (0 ... %d). Using default origin (0).\n", requestedOrigX, frameWidth-1);
			
		}

		/* remember that the origin has been set */
		currOrigX = requestedOrigX;

		
		/* set requested WIDTH of the image to be returned to MATLAB */
		if(requestedWidth > 0 && requestedWidth <= frameWidth-origX) {
			
			/* requested width is valid -> reduce width of output array */
			dims[1] = requestedWidth;
			
		} else {

			/* requested width exceeds framewidth -> allocate as much as available */
			dims[1] = frameWidth - origX;

		}
		
		/* remember that the width has been set */
		currWidth = requestedWidth;
			

		/* set requested ORIGIN (Y)  of the image to be returned to MATLAB */
		if(requestedOrigY >= 0 && requestedOrigY < frameHeight) {
			
			/* change of origin (Y) */
			origY = requestedOrigY;
			
		} else {
			
			/* default origin (X) */
			origY = 0;
			//mexPrintf("Requested origin-y (%d) outside valid limits (0 ... %d). Using default origin (0).\n", requestedOrigY, frameHeight-1);
			
		}
		
		/* remember that the origin has been set */
		currOrigY = requestedOrigY;


		/* set requested HEIGHT of the image to be returned to MATLAB */
		if(requestedHeight > 0 && requestedHeight <= frameHeight-origY) {
			
			/* requested height is valid -> reduce height of output array */
			dims[0] = requestedHeight;
			
		} else {

			/* requested height exceeds frameheight -> allocate as much as available */
			dims[0] = frameHeight - origY;

		}

		/* remember that the height has been set */
		currHeight = requestedHeight;
			
		
		#ifdef ERASE
		// debug only
		mexPrintf("requestedWidth  = %d\n", requestedWidth);
		mexPrintf("currWidth       = %d\n", currWidth);
		mexPrintf("dims[1]         = %d\n", dims[1]);
		mexPrintf("requestedHeight = %d\n", requestedHeight);
		mexPrintf("currHeight      = %d\n", currHeight);
		mexPrintf("dims[0]         = %d\n", dims[0]);
		mexPrintf("requestedOrigX  = %d\n", requestedOrigX);
		mexPrintf("currOrigX       = %d\n", currOrigY);
		mexPrintf("origX           = %d\n", origX);
		mexPrintf("requestedOrigY  = %d\n", requestedOrigY);
		mexPrintf("currOrigY       = %d\n", currOrigY);
		mexPrintf("origY           = %d\n", origY);
		#endif

    }


	/* acquire image -- make sure camera is actually running... */
    if(initCamera == 1) {
        
		/* loop through both cameras... */
		for(int cam=0; cam<2; cam++) {
		
			/* create a MATLAB array with 'm_width' columns, 'm_height' rows, 'real numbers'	*/
			pArray = mxCreateNumericArray(3, (int const *)&dims[0], mxUINT8_CLASS, mxREAL);
			//mexPrintf("pArray created\n");
    
			//mexPrintf("Capturing image\n");
			if(Camera[cam].CaptureImage() != CAM_SUCCESS)
				mexErrMsgTxt("Error during image capture.\n");
        
       		/* convert camera data to RGB and copy it to 'bufRGB' (input format automatically honoured) */
			if(Camera[cam].getRGB(bufRGB, nElements) != CAM_SUCCESS)
				mexErrMsgTxt("Error during conversion to RGB.\n");
        
			/* transfer data from DMA buffer to the MATLAB buffer...		 */
			MATbuf = (unsigned char *)mxGetPr(pArray);

			/* reformat data for MATLAB */
			//mexPrintf("before copy (always in BGR format...)\n");
			for(unsigned int row=0; row<dims[0]; row++) /* height */ {
				////mexPrintf("row = %d  --  ", row);
				for(unsigned int col=0; col<dims[1]; col++) /* width */ {
					////mexPrintf("%d ", col);
					for(unsigned int rgb=0; rgb<3; rgb++)
						MATbuf[row+col*dims[0]+rgb*dims[0]*dims[1]] = 
						(unsigned char)(*(bufRGB+(origY+row)*(frameWidth*3)+(origX+col)*3+(rgb)));	// B,G,R
				}
			}
			//mexPrintf("after copy\n");
        
		    /* set return pointer */
			//mexPrintf("Setting return pointer %d (%0x)\n", cam+1, pArray);
		    plhs[cam] = pArray;

		}  /* for all cameras */

    } /* initCamera == 1 */
	else {

		mexPrintf("Camera not initialized...\n");

	}
   
} /* mexFunction ============================================================ */