C++ CBlobResult::GetNthBlob方法代码示例

void givedepth(IplImage *localimagergb)
{	IplImage*localimage=cvCreateImage(cvGetSize(localimagergb),IPL_DEPTH_8U,3);
	cvCvtColor(localimagergb,localimage,CV_BGR2HSV);
	IplImage *blobbedscaling=cvCreateImage(cvGetSize(localimagergb),IPL_DEPTH_8U,3);
	uchar *itemp=(uchar *)(localimage->imageData);
    IplImage *binaryscaling=cvCreateImage(cvGetSize(localimagergb),IPL_DEPTH_8U,1);
	uchar *itemp1=(uchar *)(binaryscaling ->imageData);
	for(int i=0;i<hi2->height;i++){
			for(int j=0;j<hi2->width;j++){	

					if((itemp[i*localimage->widthStep+j*localimage->nChannels] <hh)	       
					&&
					(itemp[i*localimage->widthStep+j*localimage->nChannels]>hl)
				    &&
					(itemp[i*localimage->widthStep+j*localimage->nChannels+1]<sh)
					&&
					(itemp[i*localimage->widthStep+j*localimage->nChannels+1]>sl)
					&& 
					( itemp[i*localimage->widthStep+j*localimage->nChannels+2]<vh)			
					 &&
					( itemp[i*localimage->widthStep+j*localimage->nChannels+2]>vl)		//previous 124
					)	{
						itemp1[i*binaryscaling->widthStep+j]=0;					//dark regions black rest white
						}
					else
						itemp1[i*binaryscaling->widthStep+j]=255;
			}}
    cvErode( binaryscaling, binaryscaling, NULL, 4);
	cvDilate(binaryscaling, binaryscaling, NULL, 4);
	CBlobResult  blob;				
	CBlob *currentBlob=NULL;
	blob=CBlobResult(binaryscaling,NULL,255);
	blob.Filter(blob,B_EXCLUDE,CBlobGetArea(),B_LESS,500);
	cvMerge(binaryscaling,binaryscaling,binaryscaling,NULL,blobbedscaling);
	CBlob hand1,hand2;																//two blobs,one for each hand
	blob.GetNthBlob( CBlobGetArea(), 0, (hand2));	
	blob.GetNthBlob( CBlobGetArea(), 1, (hand1 ));
	hand1.FillBlob(blobbedscaling,CV_RGB(0,0,255));											//fill the color of blob of hand one with blue
	hand2.FillBlob(blobbedscaling,CV_RGB(0,255,0));											//fill the color of blob of hand two with green 
	coordinates (blobbedscaling,0);
}

/* Fetch a frame (if available) and process it, calling appropriate 
 callbacks when data becomes available. */
void MarkerCapture::tick(){
    IplImage *thresh_frame = NULL;
    CBlobResult blobs;
    
    // Acquire the lock, update the current frame.
    pthread_mutex_lock(&frame_mutex);
    current_frame = cvCloneImage(cvQueryFrame(camera));
    if(color_acquired && current_frame){
        thresh_frame = apply_threshold(current_frame, target_color);
    }else{
        // create a suplicant.
        thresh_frame = cvCreateImage(cvGetSize(current_frame),IPL_DEPTH_8U,1);
    }
    pthread_mutex_unlock(&frame_mutex);
    // Lock released. Done messing with buffers.
    
    if(frame_update_callback){
        (*frame_update_callback)(this, current_frame, thresh_frame);
    }
    if(color_acquired){
        blobs = detect_blobs(thresh_frame, CV_BLOB_SIZE_MIN);
        if(blobs.GetNumBlobs() >= 2){ // need 2 or more blobs for positional fix.
            MarkerPositionEstimate position;
            // fetch the two largest blobs, by area.
            CBlob blob0, blob1;
            blobs.GetNthBlob(CBlobGetArea(), 0, blob0);
            blobs.GetNthBlob(CBlobGetArea(), 1, blob1);
            // perform positional calculations
            position.distance = distance(blob0, blob1);
            position.angle = angle(blob0, blob1);
            position.blob0_center = blob_center(blob0);
            position.blob1_center = blob_center(blob1);
            // call the update handler.
            if(position_update_callback){
                (*position_update_callback)(this, position);
            }
        }
        blobs.ClearBlobs();
    }
    
    pthread_mutex_lock(&frame_mutex);
    cvReleaseImage(&current_frame);
    cvReleaseImage(&thresh_frame);
    pthread_mutex_unlock(&frame_mutex);
    
    int curr_time = clock();
    fps = CLOCKS_PER_SEC/(double)(curr_time - time);
    time = curr_time;
}

    void TabletopSegmentor::pcCallback(sensor_msgs::PointCloud2::ConstPtr pc_msg) {
        if(!pc_lock.try_lock())
            return;

        pcl::PointCloud<pcl::PointXYZRGB> pc_full, pc_full_frame;
        pcl::fromROSMsg(*pc_msg, pc_full);
        string base_frame("/base_link");
        ros::Time now = ros::Time::now();
        tf_listener.waitForTransform(pc_msg->header.frame_id, base_frame, now, ros::Duration(3.0));
        pcl_ros::transformPointCloud(base_frame, pc_full, pc_full_frame, tf_listener);
        // pc_full_frame is in torso lift frame

        cv::Mat cur_height_img = cv::Mat::zeros(imgx, imgy, CV_8U);
        
        BOOST_FOREACH(const pcl::PointXYZRGB& pt, pc_full_frame.points) {
            if(pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
                continue;
            int32_t x, y, z;
            x = (pt.x - minx)/(maxx-minx) * imgx;
            y = (pt.y - miny)/(maxy-miny) * imgy;
            z = (pt.z - minz)/(maxz-minz) * 256;
            if(x < 0 || y < 0) continue; 
            if(x >= imgx || y >= imgy) continue;
            if(z < 0 || z >= 256) continue;
            if(cur_height_img.at<uint8_t>(x, y) == 0 || cur_height_img.at<uint8_t>(x, y) < (uint8_t) z)
                cur_height_img.at<uint8_t>(x, y) = (uint8_t) z;
        }
        cv::max(height_img_max, cur_height_img, height_img_max);
        cv::Mat cur_height_img_flt;
        cur_height_img.convertTo(cur_height_img_flt, CV_32F);
        height_img_sum += cur_height_img_flt;
        cv::Mat cur_count(imgx, imgy, CV_8U);
        cur_count = (cur_height_img > 0) / 255;
        cv::Mat cur_count_flt(imgx, imgy, CV_32F);
        cur_count.convertTo(cur_count_flt, CV_32F);
        height_img_count += cur_count_flt;
        cv::Mat height_img_avg_flt = height_img_sum / height_img_count;
        cv::Mat height_img_avg(imgx, imgy, CV_8U);
        height_img_avg_flt.convertTo(height_img_avg, CV_8U);
        height_img_avg = height_img_max;

        cv::Mat height_hist(256, 1, CV_32F, cv::Scalar(0));
        for(uint32_t x=0;x<imgx;x++)
            for(uint32_t y=0;y<imgy;y++) {
                if(height_img_avg.at<uint8_t>(x,y) == 255)
                    height_img_avg.at<uint8_t>(x,y) = 0;
                if(height_img_avg.at<uint8_t>(x,y) != 0) {
                    height_hist.at<float>(height_img_avg.at<uint8_t>(x,y), 0)++;
                }
            }
        ////////////////////// Finding best table height /////////////////////////
        uint32_t gfiltlen = 25;
        float stddev = 256/(maxz-minz) * 0.015;
        cv::Mat gauss_filt(gfiltlen, 1, CV_32F, cv::Scalar(0));
        for(uint32_t i=0;i<gfiltlen;i++)
            gauss_filt.at<float>(i,0) = 0.39894 / stddev * std::exp(-(i-((float)gfiltlen)/2)*(i-((float)gfiltlen)/2)/(2*stddev*stddev));
        //cout << gauss_filt;
        uint32_t maxval = 0, maxidx = 0;
        for(uint32_t i=0;i<256-gfiltlen;i++) {
            uint32_t sum = 0;
            for(uint32_t j=0;j<gfiltlen;j++) 
                sum += height_hist.at<float>(i+j,0) * gauss_filt.at<float>(j,0);
            if(sum > maxval && i != 0) {
                maxval = sum;
                maxidx = i+gfiltlen/2;
            }
        }
        int32_t table_height = ((int32_t)maxidx);
        //printf("%d %d, ", maxval, maxidx);
        /////////////////////////// Getting table binary /////////////////////
        cv::Mat height_img_thresh(imgx, imgy, CV_8U);
        height_img_thresh = height_img_avg.clone();
        for(uint32_t x=0;x<imgx;x++)
            for(uint32_t y=0;y<imgy;y++) {
                if(std::fabs(table_height - ((int32_t)height_img_thresh.at<uint8_t>(x,y))) < stddev*2)
                    height_img_thresh.at<uint8_t>(x,y) = 255;
                else
                    height_img_thresh.at<uint8_t>(x,y) = 0;
            }
        //////////////////////////////////////////////////////////////////
        IplImage height_img_thresh_ipl = height_img_thresh;
        IplConvKernel* element = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT);
        cvMorphologyEx(&height_img_thresh_ipl, &height_img_thresh_ipl, NULL, element, CV_MOP_CLOSE, num_closes);
        //cvMorphologyEx(&height_img_thresh, &height_img_thresh, NULL, element, CV_MOP_OPEN, 2);

        cv::Mat height_img_thresh_blob = height_img_thresh.clone();
        IplImage blob_img = height_img_thresh_blob;
        CBlobResult blobs = CBlobResult(&blob_img, NULL, 0);
        //blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 10);
        CBlob biggestblob;
        blobs.GetNthBlob(CBlobGetArea(), 0, biggestblob);
        cv::Mat table_blob(imgx, imgy, CV_8U, cv::Scalar(0)); IplImage table_blob_img = table_blob;
        biggestblob.FillBlob(&table_blob_img, cv::Scalar(150));
        //drawCvBox2D(blob_img, table_roi, cv::Scalar(50), 1);
        CvBox2D table_roi = biggestblob.GetEllipse(); table_roi.angle *= CV_PI/180;
        cv::Mat table_hull(imgx, imgy, CV_8U, cv::Scalar(0));
        IplImage hull_img = table_hull;
        fillCvBox2D(hull_img, table_roi, cv::Scalar(255));
        //printf("Cvbox: %f, %f, %f, %f, %f\n", table_roi.center.x, table_roi.center.y, table_roi.size.width, table_roi.size.height, table_roi.angle);

//.........这里部分代码省略.........

void
Auvsi_Recognize::extractLetter( void )
{
	typedef cv::Vec<unsigned char, 1> VT_binary;
	#ifdef TWO_CHANNEL
		typedef cv::Vec<T, 2> VT;
	#else
		typedef cv::Vec<T, 3> VT;
	#endif
	typedef cv::Vec<int, 1> IT;
	

	
	
	
	
	// Erode input slightly
	cv::Mat input;
	cv::erode( _shape, input, cv::Mat() );

	// Remove any small white blobs left over
	CBlobResult blobs;
	CBlob * currentBlob;
	CBlob biggestBlob;
	IplImage binaryIpl = input;

	blobs = CBlobResult( &binaryIpl, NULL, 0 );
	blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );

	blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );

	for (int i = 0; i < blobs.GetNumBlobs(); i++ )
	{
    	currentBlob = blobs.GetBlob(i);
		currentBlob->FillBlob( &binaryIpl, cvScalar(0));
	}

	// Perform k-means on this region only
	int areaLetter = (int)biggestBlob.Area();
	cv::Mat kMeansInput = cv::Mat( areaLetter, 1, _image.type() );

	// Discard if we couldn't extract a letter
	if( areaLetter <= 0 )
	{
		_letter = cv::Mat( _shape );
		_letter = cv::Scalar(0);
		return;
	}

	cv::MatIterator_<VT_binary> binaryIterator = input.begin<VT_binary>();
	cv::MatIterator_<VT_binary> binaryEnd = input.end<VT_binary>();
	cv::MatIterator_<VT> kMeansIterator = kMeansInput.begin<VT>();

	for( ; binaryIterator != binaryEnd; ++binaryIterator )
	{
		if( (*binaryIterator)[0] > 0 )
		{
			(*kMeansIterator) = _image.at<VT>( binaryIterator.pos() );
			++kMeansIterator;
		}
	}

	// Get k-means labels
	cv::Mat labels = doClustering<T>( kMeansInput, 2, false );	
	int numZeros = areaLetter - cv::countNonZero( labels );
	bool useZeros = numZeros < cv::countNonZero( labels );

	// Reshape into original form
	_letter = cv::Mat( _shape.size(), _shape.type() );
	_letter = cv::Scalar(0);

	binaryIterator = input.begin<VT_binary>();
	binaryEnd = input.end<VT_binary>();
	cv::MatIterator_<IT> labelsIterator = labels.begin<IT>();

	for( int index = 0; binaryIterator != binaryEnd; ++binaryIterator )
	{
		if( (*binaryIterator)[0] > 0 )
		{
			// Whichever label was the minority, we make that value white and all other values black
			unsigned char value = (*labelsIterator)[0];

			if( useZeros )
				if( value )
					value = 0;
				else
					value = 255;
			else
				if( value )
					value = 255;
				else
					value = 0;

			_letter.at<VT_binary>( binaryIterator.pos() ) = VT_binary( value );
			++labelsIterator;
		}
	}
}

void
Auvsi_Recognize::extractShape( void )
{
	typedef cv::Vec<T, 1> VT;

	// Reduce input to two colors
	cv::Mat reducedColors = doClustering<T>( _image, 2 );	
	cv::Mat grayScaled, binary;

	// Make output grayscale
	grayScaled = convertToGray( reducedColors );
	//cv::cvtColor( reducedColors, grayScaled, CV_RGB2GRAY );

	// Make binary
	double min, max;
	cv::minMaxLoc( grayScaled, &min, &max );
	cv::threshold( grayScaled, binary, min, 1.0, cv::THRESH_BINARY );	

	// ensure that background is black, image white
	if( binary.at<VT>(0, 0)[0] > 0.0f )
		cv::threshold( grayScaled, binary, min, 1.0, cv::THRESH_BINARY_INV );

	binary.convertTo( binary, CV_8U, 255.0f );

	// Fill in all black regions smaller than largest black region with white
	CBlobResult blobs;
	CBlob * currentBlob;
	IplImage binaryIpl = binary;
	blobs = CBlobResult( &binaryIpl, NULL, 255 );
	
	// Get area of biggest blob
	CBlob biggestBlob;
	blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );

	// Remove all blobs of smaller area
	blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );

	for (int i = 0; i < blobs.GetNumBlobs(); i++ )
	{
    	currentBlob = blobs.GetBlob(i);
		currentBlob->FillBlob( &binaryIpl, cvScalar(255));
	}
	


	// Fill in all small white regions black 
	blobs = CBlobResult( &binaryIpl, NULL, 0 );
	blobs.GetNthBlob( CBlobGetArea(), 0, biggestBlob );

	blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, biggestBlob.Area() );

	for (int i = 0; i < blobs.GetNumBlobs(); i++ )
	{
    	currentBlob = blobs.GetBlob(i);
		currentBlob->FillBlob( &binaryIpl, cvScalar(0));
	}

	binary = cv::Scalar(0);
	biggestBlob.FillBlob( &binaryIpl, cvScalar(255));

	_shape = binary;
} 

//.........这里部分代码省略.........
				lut[i] = (uchar)v;
			}
		}
		else {
			double delta = -128.*contrast/100;
			double a = (256.-delta*2)/255.;
			double b = a*brightness + delta;
			for(int i = 0; i < 256; i++ ) {
				int v = cvRound(a*i + b);
				if( v < 0 )
					v = 0;
				if( v > 255 )
					v = 255;
				lut[i] = (uchar)v;
			}
		}

		cvLUT( inputImage, bright_cont_image, lut_mat );
		cvShowImage( "Brightness and Contrast Window", bright_cont_image);







		// ---------------
		// Blob Manipulation Code begins here:

		// Extract the blobs using a threshold of 100 in the image
		blobs = CBlobResult( bright_cont_image, NULL, blob_extraction_threshold, true );

		// discard the blobs with less area than 5000 pixels
		// ( the criteria to filter can be any class derived from COperadorBlob ) 
		blobs.Filter( blobs, B_INCLUDE, CBlobGetArea(), B_GREATER_OR_EQUAL, min_blob_size);
		blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER, max_blob_size);

		// build an output image equal to the input but with 3 channels (to draw the coloured blobs)
		cvMerge( bright_cont_image, bright_cont_image, bright_cont_image, NULL, outputImage );

		// plot the selected blobs in a output image
		for (int i=0; i < blobs.GetNumBlobs(); i++) {
			blobs.GetNthBlob( CBlobGetArea(), i, my_enumerated_blob );
			// Color 5/6 of the color wheel (300 degrees)
			my_enumerated_blob.FillBlob( outputImage, cv_hsv2rgb((float)i/blobs.GetNumBlobs() * 300, 1, 1));
		}
		// END Blob Manipulation Code
		// ---------------


		sprintf(str, "Count: %d", blobs.GetNumBlobs());
		cvPutText(outputImage, str, cvPoint(50, 25), &font, cvScalar(255,0,255));
		
		cvShowImage("Blob Output Window", outputImage);






/*
		// Rainbow manipulation:
		for (int i=0; i < CV_CAP_PROP_FRAME_WIDTH; i++) {
			for (int j=0; j < CV_CAP_PROP_FRAME_HEIGHT; j++) {
// This line is not figure out yet...
//				pixel_color_set = ((uchar*)(rainbowImage->imageData + rainbowImage->widthStep * j))[i * 3]

				((uchar*)(rainbowImage->imageData + rainbowImage->widthStep * j))[i * 3] = 30;
				((uchar*)(rainbowImage->imageData + rainbowImage->widthStep * j))[i * 3 + 1] = 30;
				((uchar*)(rainbowImage->imageData + rainbowImage->widthStep * j))[i * 3 + 2] = 30;
			}
		}
		cvShowImage("Rainbow Window", rainbowImage);
*/







		//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
		//remove higher bits using AND operator
		if( (cvWaitKey(10) & 255) == 27 ) break;

	}

	cvReleaseImage(&inputImage);
	cvReleaseImage(&histAdjustedImage);
	cvReleaseImage(&hist_image);
	cvReleaseImage(&bright_cont_image);
	cvReleaseImage(&outputImage);
	cvReleaseImage(&rainbowImage);

	// Release the capture device housekeeping
	cvReleaseCapture( &capture );
	cvDestroyAllWindows();

	return 0;
}

    void TabletopDetector::pcCallback(sensor_msgs::PointCloud2::ConstPtr pc_msg) {
        if(!pc_lock.try_lock())
            return;

        pcl::PointCloud<pcl::PointXYZRGB> pc_full, pc_full_frame;
        pcl::fromROSMsg(*pc_msg, pc_full);
        string base_frame("/base_link");
        ros::Time now = ros::Time::now();
        tf_listener.waitForTransform(pc_msg->header.frame_id, base_frame, now, ros::Duration(3.0));
        pcl_ros::transformPointCloud(base_frame, pc_full, pc_full_frame, tf_listener);
        // pc_full_frame is in torso lift frame

        cv::Mat cur_height_img = cv::Mat::zeros(imgx, imgy, CV_8U);
        
        BOOST_FOREACH(const pcl::PointXYZRGB& pt, pc_full_frame.points) {
            if(pt.x != pt.x || pt.y != pt.y || pt.z != pt.z)
                continue;
            int32_t x, y, z;
            x = (pt.x - minx)/(maxx-minx) * imgx;
            y = (pt.y - miny)/(maxy-miny) * imgy;
            z = (pt.z - minz)/(maxz-minz) * 256;
            if(x < 0 || y < 0) continue; 
            if(x >= imgx || y >= imgy) continue;
            if(z < 0 || z >= 256) continue;
            if(cur_height_img.at<uint8_t>(x, y) == 0 || cur_height_img.at<uint8_t>(x, y) < (uint8_t) z)
                cur_height_img.at<uint8_t>(x, y) = (uint8_t) z;
        }
        cv::max(height_img_max, cur_height_img, height_img_max);
        cv::Mat cur_height_img_flt;
        cur_height_img.convertTo(cur_height_img_flt, CV_32F);
        height_img_sum += cur_height_img_flt;
        cv::Mat cur_count(imgx, imgy, CV_8U);
        cur_count = (cur_height_img > 0) / 255;
        cv::Mat cur_count_flt(imgx, imgy, CV_32F);
        cur_count.convertTo(cur_count_flt, CV_32F);
        height_img_count += cur_count_flt;
        cv::Mat height_img_avg_flt = height_img_sum / height_img_count;
        cv::Mat height_img_avg(imgx, imgy, CV_8U);
        height_img_avg_flt.convertTo(height_img_avg, CV_8U);
        height_img_avg = height_img_max;

        cv::Mat height_hist(256, 1, CV_32F, cv::Scalar(0));
        for(uint32_t x=0;x<imgx;x++)
            for(uint32_t y=0;y<imgy;y++) {
                if(height_img_avg.at<uint8_t>(x,y) == 255)
                    height_img_avg.at<uint8_t>(x,y) = 0;
                if(height_img_avg.at<uint8_t>(x,y) != 0) {
                    height_hist.at<float>(height_img_avg.at<uint8_t>(x,y), 0)++;
                }
            }
        ////////////////////// Finding best table height /////////////////////////
        uint32_t gfiltlen = 25;
        float stddev = 256/(maxz-minz) * 0.015;
        cv::Mat gauss_filt(gfiltlen, 1, CV_32F, cv::Scalar(0));
        for(uint32_t i=0;i<gfiltlen;i++)
            gauss_filt.at<float>(i,0) = 0.39894 / stddev * std::exp(-(i-((float)gfiltlen)/2)*(i-((float)gfiltlen)/2)/(2*stddev*stddev));
        //cout << gauss_filt;
        uint32_t maxval = 0, maxidx = 0;
        for(uint32_t i=0;i<256-gfiltlen;i++) {
            uint32_t sum = 0;
            for(uint32_t j=0;j<gfiltlen;j++) 
                sum += height_hist.at<float>(i+j,0) * gauss_filt.at<float>(j,0);
            if(sum > maxval && i != 0) {
                maxval = sum;
                maxidx = i+gfiltlen/2;
            }
        }
        int32_t table_height = ((int32_t)maxidx);
        //printf("%d %d, ", maxval, maxidx);
        /////////////////////////// Getting table binary /////////////////////
        cv::Mat height_img_thresh(imgx, imgy, CV_8U);
        height_img_thresh = height_img_avg.clone();
        for(uint32_t x=0;x<imgx;x++)
            for(uint32_t y=0;y<imgy;y++) {
                if(std::fabs(table_height - ((int32_t)height_img_thresh.at<uint8_t>(x,y))) < stddev*2)
                    height_img_thresh.at<uint8_t>(x,y) = 255;
                else
                    height_img_thresh.at<uint8_t>(x,y) = 0;
            }
        //////////////////////////////////////////////////////////////////
        IplImage height_img_thresh_ipl = height_img_thresh;
        IplConvKernel* element = cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT);
        cvMorphologyEx(&height_img_thresh_ipl, &height_img_thresh_ipl, NULL, element, CV_MOP_CLOSE, num_closes);
        //cvMorphologyEx(&height_img_thresh, &height_img_thresh, NULL, element, CV_MOP_OPEN, 2);

        cv::Mat height_img_thresh_blob = height_img_thresh.clone();
        IplImage blob_img = height_img_thresh_blob;
        CBlobResult blobs = CBlobResult(&blob_img, NULL, 0);
        //blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 10);
        CBlob biggestblob;
        blobs.GetNthBlob(CBlobGetArea(), 0, biggestblob);
        cv::Mat table_blob(imgx, imgy, CV_8U, cv::Scalar(0)); IplImage table_blob_img = table_blob;
        biggestblob.FillBlob(&table_blob_img, cv::Scalar(150));
        //drawCvBox2D(blob_img, table_roi, cv::Scalar(50), 1);
        CvBox2D table_roi = biggestblob.GetEllipse(); table_roi.angle *= CV_PI/180;
        cv::Mat table_hull(imgx, imgy, CV_8U, cv::Scalar(0));
        IplImage hull_img = table_hull;
        fillCvBox2D(hull_img, table_roi, cv::Scalar(255));
        //printf("Cvbox: %f, %f, %f, %f, %f\n", table_roi.center.x, table_roi.center.y, table_roi.size.width, table_roi.size.height, table_roi.angle);

//.........这里部分代码省略.........

 int main() {
  CvPoint pt1,pt2;
  CvRect regt;
  CvPoint cir_center;
  CvPoint frame_center;
  CvPoint A,B,C,D;
  CvPoint temp;
  double angle,spinsize;
  int cir_radius=1; 
  int frame_width=160, frame_height=120;
   CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
   if ( !capture ) {
     fprintf(stderr, "ERROR: capture is NULL \n" );
     getchar();
     return -1;
   }
  cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_WIDTH,frame_width);// 120x160 
  cvSetCaptureProperty(capture,CV_CAP_PROP_FRAME_HEIGHT,frame_height);
  //cvSetCaptureProperty(capture, CV_CAP_PROP_FPS,10);
//  cvSetCaptureProperty(capture,CV_CAP_PROP_POS_FRAMES,5);  
 // Create a window in which the captured images will be presented
   cvNamedWindow( "mywindow", CV_WINDOW_AUTOSIZE );
   // Show the image captured from the camera in the window and repeat
   while ( 1 ) {
     // Get one frame
     IplImage* frame = cvQueryFrame( capture );
     if ( !frame ) {
       fprintf( stderr, "ERROR: frame is null...\n" );
       getchar();
       break;
     }
     int modfheight, modfwidth;

     modfheight = frame->height;
     modfwidth = frame->width;
     // create modified frame with 1/4th the original size
     IplImage* modframe = cvCreateImage(cvSize((int)(modfwidth/4),(int)(modfheight/4)),frame->depth,frame->nChannels); //cvCreateImage(size of frame, depth, noofchannels)
     cvResize(frame, modframe,CV_INTER_LINEAR);
     // create HSV(Hue, Saturation, Value) frame
     IplImage* hsvframe = cvCreateImage(cvGetSize(modframe),8, 3);
     cvCvtColor(modframe, hsvframe, CV_BGR2HSV); //cvCvtColor(input frame,outputframe,method)
     // create a frame within threshold.
     IplImage* threshframe = cvCreateImage(cvGetSize(hsvframe),8,1);
     cvInRangeS(hsvframe,cvScalar(15, 100, 100),cvScalar(60, 220, 220),threshframe); //cvInRangeS(input frame, cvScalar(min range),cvScalar(max range),output frame)
     // created dilated image
     IplImage* dilframe = cvCreateImage(cvGetSize(threshframe),8,1);
     cvDilate(threshframe,dilframe,NULL,2); //cvDilate(input frame, output frame, mask, number of times to dilate)

     CBlobResult blobs;
     blobs = CBlobResult(dilframe,NULL,0); // CBlobresult(inputframe, mask, threshold) Will filter all white parts of image
     blobs.Filter(blobs,B_EXCLUDE,CBlobGetArea(),B_LESS,50);//blobs.Filter(input, cond, criteria, cond, const) Filter all images whose area is less than 50 pixels
     CBlob biggestblob;
     blobs.GetNthBlob(CBlobGetArea(),0,biggestblob); //GetNthBlob(criteria, number, output) Get only the largest blob based on CblobGetArea()
     // get 4 points to define the rectangle
     pt1.x = biggestblob.MinX()*4;
     pt1.y = biggestblob.MinY()*4;
     pt2.x = biggestblob.MaxX()*4;
     pt2.y = biggestblob.MaxY()*4;
     cir_center.x=(pt1.x+pt2.x)/2;
     cir_center.y=(pt1.y+pt2.y)/2;
     frame_center.x=frame_width/2;
     frame_center.y=frame_height/2;
     A.x=frame_center.x-4;
     A.y=frame_center.y;
     B.x=frame_center.x+4;
     B.y=frame_center.y;
     C.y=frame_center.y-4;
     C.x=frame_center.x;
     D.y=frame_center.y+4;
     D.x=frame_center.x;
     cvRectangle(frame,pt1,pt2,cvScalar(255,0,0),1,8,0); // draw rectangle around the biggest blob
     cvCircle( frame, cir_center, cir_radius, cvScalar(0,255,255), 1, 8, 0 ); // center point of the rectangle
     cvLine(frame, A, B,cvScalar(255,0,255),2,8,0);
     cvLine(frame, C, D,cvScalar(255,0,255),2,8,0);
     if (cir_center.x!=0&&cir_center.y!=0){
     spinsize=sqrt((cir_center.x-frame_center.x)*(cir_center.x-frame_center.x) +(cir_center.y-frame_center.y)*(cir_center.y-frame_center.y));
     angle = atan2((double)cir_center.y-frame_center.y,(double)cir_center.x-frame_center.x);
     temp.x=(int)(frame_center.x+spinsize/5*cos(angle+3.1416/4));
     temp.y=(int)(frame_center.y+spinsize/5*sin(angle+3.1415/4));
     cvLine(frame, temp, frame_center,cvScalar(0,255,0),1,8,0);	

     temp.x=(int)(frame_center.x+spinsize/5*cos(angle-3.1416/4));
     temp.y=(int)(frame_center.y+spinsize/5*sin(angle-3.1415/4));
     cvLine(frame, temp, frame_center,cvScalar(0,255,0),1,8,0);	
	
     cvLine(frame, cir_center, frame_center,cvScalar(0,255,0),1,8,0);

     //cvCircle( frame, frame_center, cir_radius, cvScalar(0,255,255), 2, 8, 0 );
}
     cvShowImage( "mywindow", frame); // show output image
     // Do not release the frame!
     //If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
     //remove higher bits using AND operator
     if ( (cvWaitKey(10) & 255) == 27 ) break;
   }
   // Release the capture device housekeeping
   cvReleaseCapture( &capture );
   cvDestroyWindow( "mywindow" );
   return 0;
 }

/*
 * thread for displaying the opencv content
 */
void *cv_threadfunc (void *ptr) {
	IplImage* timg = cvCloneImage(rgbimg); // Image we do our processing on
	IplImage* dimg = cvCloneImage(rgbimg); // Image we draw on
	CvSize sz = cvSize( timg->width & -2, timg->height & -2);
	IplImage* outimg = cvCreateImage(sz, 8, 3);

	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* squares; // Sequence for squares - sets of 4 points
	CvSeq* contours; // Raw contours list
	CvSeq* result; // Single contour being processed

	CBlobResult blobs;
	CBlob *currentBlob;

	IplImage *pyr = cvCreateImage(cvSize(sz.width/2, sz.height/2), 8, 1);

	// Set region of interest
	cvSetImageROI(timg, cvRect(0, 0, sz.width, sz.height));
	cvSetImageROI(dimg, cvRect(0, 0, sz.width, sz.height));

	// Processing and contours
	while (1) {
		squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvPoint), storage);

		pthread_mutex_lock( &mutex_rgb );
		cvCopy(rgbimg, dimg, 0);
		cvCopy(rgbimg, timg, 0);
		pthread_mutex_unlock( &mutex_rgb );

		// BLUR TEST
		// cvPyrDown(dimg, pyr, 7);
		// cvPyrUp(pyr, timg, 7);

		// DILATE TEST
		IplConvKernel* element = cvCreateStructuringElementEx(5, 5, 2, 2, 0);
		IplConvKernel* element2 = cvCreateStructuringElementEx(3, 3, 1, 1, 0);
		cvDilate(timg, timg, element, 2);
		cvErode(timg, timg, element2, 3);

		// THRESHOLD TEST 
		cvThreshold(timg, timg, 200, 255, CV_THRESH_BINARY);

		// Output processed or raw image.
		cvCvtColor(timg, outimg, CV_GRAY2BGR);

		// BLOB TEST
		blobs = CBlobResult( timg, (IplImage*)NULL, 0, true );
		// blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 50 );
		
		printf("Blobs: %d\n", blobs.GetNumBlobs());

		CBlob biggestBlob;
		blobs.GetNthBlob( CBlobGetArea(), 1, biggestBlob );
		biggestBlob.FillBlob( outimg, CV_RGB(255, 0, 0) );
		CvSeq* dest;
		biggestBlob.GetConvexHull(dest);
		
		// for (int i = 0; i < blobs.GetNumBlobs(); i++ )
		// {
		// 	currentBlob = blobs.GetBlob(i);
		// 	currentBlob->FillBlob( outimg, CV_RGB(255,0,0) );
		// }
		

//		// CONTOUR FINDING
//		cvFindContours(timg, storage, &contours, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
//
//		while (contours)
//		{
//			// Approximate contour, accuracy proportional to perimeter of contour; may want to tune accuracy.
//			result = cvApproxPoly(contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(contours) * 0.02, 0);
//			// Filter small contours and contours w/o 4 vertices (filters noise, finds rectangles)
//			if (result->total == 4 && 
//				fabs(cvContourArea(result, CV_WHOLE_SEQ)) > 600 && 
//				cvCheckContourConvexity(result))
//			{
//				// Skipped checking whether angles were close to 90 degrees here; may want to implement.
//				// Probably also want to check if it's square enough to filter out ex. long windows.
//
//				for (int i = 0; i < 4; i++)
//				{
//					// Write vertices to output sequence
//					cvSeqPush(squares, (CvPoint*)cvGetSeqElem(result, i));
//				}
//			}
//
//			// Take next contour
//			contours = contours->h_next;
//		}
//
//
//		// DRAW RECTANGLES
//		CvSeqReader reader;
//		cvStartReadSeq(squares, &reader, 0);
//
//		// Read 4 points at a time
//		CvPoint pt[4];
//.........这里部分代码省略.........

void blobbing( IplImage *hi, char * win1, char * win2, int check )	{
 	cvCvtColor(hi,hi2,CV_BGR2HSV);
	uchar *itemp=(uchar *)(hi2->imageData);
	uchar *itemp1=(uchar *)(hitemp->imageData);
	
	//	 binary conversion 
	for(int i=0;i<hi2->height;i++){
			for(int j=0;j<hi2->width;j++){

					if((itemp[i*hi2->widthStep+j*hi2->nChannels] <hh)	       
					&&
					(itemp[i*hi2->widthStep+j*hi2->nChannels]>hl)
				    &&
					(itemp[i*hi2->widthStep+j*hi2->nChannels+1]<sh)
					&&
					(itemp[i*hi2->widthStep+j*hi2->nChannels+1]>sl)
					
					&& 
					( itemp[i*hi2->widthStep+j*hi2->nChannels+2]<vh)			
					 &&
					( itemp[i*hi2->widthStep+j*hi2->nChannels+2]>vl)		//previous 124
					)
					
					
					{
						itemp1[i*hitemp->widthStep+j]=0;					//dark regions black rest white
						}
					else
						itemp1[i*hitemp->widthStep+j]=255;

		  
			}}
	 
	cvErode( hitemp, hitemp1, NULL, 3);
	cvDilate(hitemp1, hitemp1, NULL, 3);

	hitemp=hitemp1;
	CBlobResult  blob;				
	CBlob *currentBlob=NULL;
	blob=CBlobResult(hitemp1,NULL,255);
	blob.Filter(blob,B_EXCLUDE,CBlobGetArea(),B_LESS,500);
	cvMerge(hitemp1,hitemp1,hitemp1,NULL,out);
 
	CBlob hand1,hand2;																//two blobs,one for each hand
	blob.GetNthBlob( CBlobGetArea(), 0, (hand2));	
	blob.GetNthBlob( CBlobGetArea(), 1, (hand1 ));
	hand1.FillBlob(out,CV_RGB(0,0,255));											//fill the color of blob of hand one with blue
	hand2.FillBlob(out,CV_RGB(0,255,0));											//fill the color of blob of hand two with green 
	coordinates (out,check);														//to find the coordinates of the hands we pass the image onto the function coordinates
	int greater1,greater2,lesser1,lesser2;
	if(x>X)		{
		greater1=x,greater2=y;
		lesser1=X,lesser2=Y;
	}
	else		{
		greater1=X,greater2=Y;
	    lesser1=x,lesser2=y;
	}
 	
	/*cvCircle (  hi, cvPoint(greater1,greater2), 10,
					cvScalar(0,0,255), -1, 8		   );
	cvCircle (  hi, cvPoint(lesser1,lesser2), 10,
					cvScalar(0,255,255), -1, 8		);
					*/
	cvResizeWindow(win2,280,280);
	cvMoveWindow(win2,0,0);
	cvShowImage(win2,out);
	return ; 
}