C++ CBlob::FillBlob方法代碼示例

void ScheinrieseApp::findBlobs() {
    CBlobResult blobs;
    int i;
    CBlob *currentBlob;
    IplImage *original, *originalThr;

    // load an image and threshold it
    original = cvLoadImage("pic1.png", 0);
    cvThreshold( original, originalThr, 100, 0, 255, CV_THRESH_BINARY );

    // find non-white blobs in thresholded image
    blobs = CBlobResult( originalThr, NULL, 255 );
    // exclude the ones smaller than param2 value
    blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, param2 );

    // get mean gray color of biggest blob
    CBlob biggestBlob;
    CBlobGetMean getMeanColor( original );
    double meanGray;

    blobs.GetNth( CBlobGetArea(), 0, biggestBlob );
    meanGray = getMeanColor( biggestBlob );

    // display filtered blobs
    cvMerge( originalThr, originalThr, originalThr, NULL, displayedImage );

    for (i = 0; i < blobs.GetNumBlobs(); i++ )
    {
        currentBlob = blobs.GetBlob(i);
        currentBlob->FillBlob( displayedImage, CV_RGB(255,0,0));
    }
}

// threshold trackbar callback
void on_trackbar( int dummy )
{
	if(!originalThr)
	{
		originalThr = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U,1);
	}

	if(!displayedImage)
	{
		displayedImage = cvCreateImage(cvGetSize(original), IPL_DEPTH_8U,3);
	}
	
	// threshold input image
	cvThreshold( original, originalThr, param1, 255, CV_THRESH_BINARY );

	// get blobs and filter them using its area
	CBlobResult blobs;
	int i;
	CBlob *currentBlob;

	// find blobs in image
	blobs = CBlobResult( originalThr, NULL, 255 );
	blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, param2 );

	// display filtered blobs
	cvMerge( originalThr, originalThr, originalThr, NULL, displayedImage );

	for (i = 0; i < blobs.GetNumBlobs(); i++ )
	{
		currentBlob = blobs.GetBlob(i);
		currentBlob->FillBlob( displayedImage, CV_RGB(255,0,0));
	}
	 
    cvShowImage( wndname, displayedImage );
	
}

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

    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(int argc, char *argv[])
{
    CvCapture* capture = cvCreateFileCapture( "recording_01.avi");



    handOrientation rightOrientationLast = NONE, leftOrientationLast = NONE;
    handOrientation rightOrientationCur = NONE, leftOrientationCur = NONE;


	//cvNamedWindow("Input Image", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow("Skin Pixels", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("Skin Blobs", CV_WINDOW_AUTOSIZE);

    while(1){
        Mat imageBGR = cvQueryFrame(capture);
        if(imageBGR.empty())break;
        //imshow("Input Image", imageBGR);

        // Convert the image to HSV colors.
        Mat imageHSV = Mat(imageBGR.size(), CV_8UC3);	// Full HSV color image.
        cvtColor(imageBGR, imageHSV, CV_BGR2HSV);				// Convert from a BGR to an HSV image.

        std::vector<Mat> channels(3);
        split(imageHSV, channels);

        Mat planeH = channels[0];
        Mat planeS = channels[1];
        Mat planeV = channels[2];


        // Detect which pixels in each of the H, S and V channels are probably skin pixels.
        threshold(channels[0], channels[0], 150, UCHAR_MAX, CV_THRESH_BINARY_INV);//18
        threshold(channels[1], channels[1], 60, UCHAR_MAX, CV_THRESH_BINARY);//50
        threshold(channels[2], channels[2], 170, UCHAR_MAX, CV_THRESH_BINARY);//80


        // Combine all 3 thresholded color components, so that an output pixel will only
        // be white if the H, S and V pixels were also white.
        Mat imageSkinPixels = Mat( imageBGR.size(), CV_8UC3);	// Greyscale output image.
        bitwise_and(channels[0], channels[1], imageSkinPixels);				// imageSkin = H {BITWISE_AND} S.
        bitwise_and(imageSkinPixels, channels[2], imageSkinPixels);	// imageSkin = H {BITWISE_AND} S {BITWISE_AND} V.

        // Show the output image on the screen.

        //imshow("Skin Pixels", imageSkinPixels);


        IplImage ipl_imageSkinPixels = imageSkinPixels;

        // Find blobs in the image.
        CBlobResult blobs;

        blobs = CBlobResult(&ipl_imageSkinPixels, NULL, 0);	// Use a black background color.

        // Ignore the blobs whose area is less than minArea.

        blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, minBlobArea);

        srand (time(NULL));

        // Show the large blobs.
        IplImage* imageSkinBlobs = cvCreateImage(imageBGR.size(), 8, 3);	//Colored Output//,1); Greyscale output image.
        for (int i = 0; i < blobs.GetNumBlobs(); i++) {
            CBlob *currentBlob = blobs.GetBlob(i);
            currentBlob->FillBlob(imageSkinBlobs, CV_RGB(rand()%255,rand()%255,rand()%255));	// Draw the large blobs as white.

             cvDrawRect(imageSkinBlobs,
                  cvPoint(currentBlob->GetBoundingBox().x,currentBlob->GetBoundingBox().y),
                  cvPoint(currentBlob->GetBoundingBox().x + currentBlob->GetBoundingBox().width,currentBlob->GetBoundingBox().y + currentBlob->GetBoundingBox().height),
                  cvScalar(0,0,255),
                  2);//Draw Bounding Boxes

        }

        cvShowImage("Skin Blobs", imageSkinBlobs);

        //Gestures

        //std::cout << "Number of Blobs: "<< blobs.GetNumBlobs() <<endl;

        if(blobs.GetNumBlobs() == 0){
            //picture empty
        }else if(blobs.GetNumBlobs() == 1) {
            //head detected
        }else if(blobs.GetNumBlobs() == 2 || blobs.GetNumBlobs() == 3){
            //head + one hand || head + two hands
            CvRect rect[3];
            int indexHead = -1, indexHandLeft = -1, indexHandRight = -1;


            //Get Bounding Boxes
            for(int i = 0; i< blobs.GetNumBlobs(); i++){
                rect[i] = blobs.GetBlob(i)->GetBoundingBox();
            }

            //Detect Head and Hand indexes
            if(blobs.GetNumBlobs() == 2){
                int indexHand = -1;
                if(getCenterPoint(rect[0]).y < getCenterPoint(rect[1]).y){
//.........這裏部分代碼省略.........

void MyWindow::on_updateImages()
{
    if(video_being_processed)
    {
        cam>>camImage;
        cv::resize(camImage,colorImage,cv::Size(320,240));
        cv::cvtColor(colorImage,thImage,CV_RGB2HSV_FULL);

        using namespace cv;

        inRange(thImage,start,end,thImage);

        blur(thImage, thImage, Size(blurVal,blurVal));
        medianBlur(thImage,thImage,mblurVal);
        GaussianBlur(thImage, thImage, Size(gblurVal,gblurVal), 0);

        using namespace cv;

        Mat dilatekernel;

        if(kernelShape == 0)

            dilatekernel = getStructuringElement(MORPH_ELLIPSE, Size(kernelSizeVal, kernelSizeVal));

        if(kernelShape == 1)

            dilatekernel = getStructuringElement(MORPH_RECT, Size(kernelSizeVal, kernelSizeVal));

        if(kernelShape == 2)

            dilatekernel = getStructuringElement(MORPH_CROSS, Size(kernelSizeVal, kernelSizeVal));

        // First and second both argument selectable

        if(dilate_b)

            dilate(thImage, thImage, dilatekernel);

        if(erode_b)

            erode(thImage, thImage, dilatekernel);

        /////////////////////////////////////////////////////////////


        if(blob_enabled){

            IplImage Iipl = thImage;

            CBlobResult blobs, blobsclutter;
            CBlob * currentBlob;

            blobs = CBlobResult(&Iipl, NULL, bg_r);

            // The last parameter indicates the background. That is the color
            // on which we are searching for the clutter.

            // The last parameter in fillblob is the color with which we want to fill
            // the clutter that we found.

            blobs.Filter(blobs, filter_arg2, CBlobGetArea(), filter_arg4, filter_arg5_area);
            for (int i = 0; i < blobs.GetNumBlobs(); i++ )
            {
                currentBlob = blobs.GetBlob(i);
                currentBlob->FillBlob(&Iipl,Scalar(fg_r));
            }

            //        blobsclutter = CBlobResult(&Iipl, NULL, 0);
            //        blobsclutter.Filter(blobsclutter, B_INCLUDE, CBlobGetArea(), B_LESS, 500);
            //        for (int i = 0; i < blobsclutter.GetNumBlobs(); i++ )
            //        {
            //            currentBlob = blobsclutter.GetBlob(i);
            //            currentBlob->FillBlob(&Iipl,Scalar(0));
            //        }

        }

        binaryImage = thImage;

        Mat drawing;

        if(draw_contours){
            // Drawing contours

            int thresh = 100;
            RNG rng(12345);

            Mat threshold_output;
            vector<vector<Point> > contours;
            vector<Vec4i> hierarchy;

            threshold( thImage, threshold_output, thresh, 255, THRESH_BINARY );
            /// Find contours
            findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );

            /// Approximate contours to polygons + get bounding rects and circles
            vector<vector<Point> > contours_poly( contours.size() );
            vector<Rect> boundRect( contours.size() );
            vector<Point2f>center( contours.size() );
            vector<float>radius( contours.size() );
//.........這裏部分代碼省略.........

//==============================================================================
void PanTiltCameraClass::blobTracking(IplImage* hsv_mask,
                                      IplImage* pFour,
                                      IplImage* pImg)
{
   //--- Get blobs and filter them using the blob area
   CBlobResult blobs;
   CBlob *currentBlob;
   
   //--- Create a thresholded image and display image --------------------
   //--- Creates binary image
   IplImage* originalThr = cvCreateImage(cvGetSize(hsv_mask), IPL_DEPTH_8U,1);
   
   //--- Create 3-channel image
   IplImage* display = cvCreateImage(cvGetSize(hsv_mask),IPL_DEPTH_8U,3);
   
   //--- Copies the original
   cvMerge( hsv_mask, hsv_mask, hsv_mask, NULL, display );
   
   //--- Makes a copy for processing
   cvCopy(hsv_mask,originalThr);
   
   //--- Find blobs in image ---------------------------------------------
   int blobThreshold = 0;
   bool blobFindMoments = true;
   blobs = CBlobResult( originalThr, originalThr, blobThreshold, blobFindMoments);
   
   //--- filters blobs according to size and radius constraints
   blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, this->minBlobSize );
   
   //--- display filtered blobs ------------------------------------------
   
   //--- copies the original in (for background)
   cvMerge( originalThr, originalThr, originalThr, NULL, display );
   
   CvPoint pts[this->NUMBER_OF_CIRCLES];
   
   //--- This sequence marks all the blobs
   for (int i = 0; i < blobs.GetNumBlobs(); i++ )
   {
      currentBlob = blobs.GetBlob(i);
      currentBlob->FillBlob( display, CV_RGB(0,0,255));				
      
      //--- Get blobs centerpoint
      CvPoint bcg;
      bcg.x = (int)(currentBlob->MinX()+((currentBlob->MaxX()-currentBlob->MinX())/2));
      bcg.y = (int)(currentBlob->MinY()+((currentBlob->MaxY()-currentBlob->MinY())/2));
      
      //--- Print the CG on the picture
      char blobtext[40];
      for(int k=0;k<this->NUMBER_OF_CIRCLES;k++)
      {
         sprintf(blobtext,"%d",k+1);
         TargetReticle(display,&pts[k],blobtext,6,CV_RGB(255,0,0));
      }//for
   }//for each blob
   
   //--- Set the ROI in the pFour image
   cvSetImageROI(pFour,cvRect(pImg->width,pImg->height+80,pImg->width,pImg->height));
   cvCopy(display,pFour);
   
   //Reset region of interest
   cvResetImageROI(display);						
   
   //Clean up
   cvReleaseImage( &originalThr );
   cvReleaseImage( &display);
}

//.........這裏部分代碼省略.........
					max_1 = i;
				} else if(area > maxArea_2){
					maxArea_2 = area;
					max_2 = i;
				}
			}
			int i_1 = max_1;
			int i_2 = max_2;
			double area_left, area_right;
			Rect rect_1;
			Rect rect_2;

			//determines which hand is left/right
			blob_1 = blobs.GetBlob(i_1);
			blob_2 = blobs.GetBlob(i_2);
			center_1 = blob_1->getCenter();
			center_2 = blob_2->getCenter();
			bool left_is_1 = (center_1.x < center_2.x)? true : false;
			leftBlob = (left_is_1)? blob_1 : blob_2;
			rightBlob = (left_is_1)? blob_2 : blob_1;
			center_left = leftBlob->getCenter();
			center_right = rightBlob->getCenter();

			//determine the number of valid hands
			//validity is decided by whether or not the hand followed a logical movement,
			//and if the area of the blob is large enough to be accepted
			int valids = 0;
			rect_1 = leftBlob->GetBoundingBox();
			rectangle(copy, rect_1.tl(), rect_1.br(), leftColor_2, 5);
			error_left = norm(statePt_left - center_left);
			area_left = leftBlob->Area();
			left_valid = error_left < sensitivity && area_left > area;
			if(left_valid){
				leftBlob->FillBlob(copy,leftColor, true);
				valids ++;
			}
			circle(copy, center_left, 5, leftColor_2, -1);


			rect_2 = rightBlob->GetBoundingBox();
			rectangle(copy, rect_2.tl(), rect_2.br(), rightColor_2, 5);
			error_right = norm(statePt_right - center_right);
			area_right = rightBlob->Area();
			right_valid = error_right < sensitivity && area_right > area;
			if(right_valid){
				rightBlob->FillBlob(copy,rightColor, true);
				valids ++;
			}
			circle(copy, center_right, 5, rightColor_2, -1);


			//finds the blob representing the robot
			//we could add a restriction to only choose a blob between the two hands
			//in terms of x-coordinate
			//a Kalman check can easily be done for the robot
			Point robot_center;
			maxArea_1 = 0;
			max_1 = 0;
			numBlobs = robot_blobs.GetNumBlobs();
			if(0 < numBlobs){
				for(int i=0;i<numBlobs;i++){
					curblob = robot_blobs.GetBlob(i);
					robot_center = curblob->getCenter();
					double dist_1 = norm(center_1 - robot_center);
					double dist_2 = norm(center_2 - robot_center);
					if(dist_1 < len || dist_2 < len){

IplImage* blobDetection2(IplImage* imgThreshRed, IplImage* imgThreshGreen) {
    // get blobs and filter them using its area
    int i, j;
    //  int areaBlob = 100;
    float distMark = 10;
    CBlobResult blobsRed, blobsGreen, whiteRedBlobs, whiteGreenBlobs;
    CBlob *currentBlob;
    double px, py;

    // Create Image
    IplImage* displayedImage = cvCreateImage(cvGetSize(imgThreshRed), IPL_DEPTH_8U, 3);

    // find all the RED related blobs in the image
    blobsRed = CBlobResult(imgThreshRed, NULL, 0);
    // find all the GREEN related blobs in the image
    blobsGreen = CBlobResult(imgThreshGreen, NULL, 0);

    // select the ones with mean gray-level equal to 255 (white) and put
    // them in the whiteBlobs variable
    blobsRed.Filter(whiteRedBlobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 1.0);
    blobsGreen.Filter(whiteGreenBlobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 1.0);

#ifdef DEBUG_PRINT    
    printf("White Blobs: %d\n", whiteBlobs.GetNumBlobs());
#endif

    // display filtered blobs
    cvMerge(imgThreshRed, imgThreshRed, imgThreshRed, NULL, displayedImage);

    // RED
    CvPoint2D32f redCenter[whiteRedBlobs.GetNumBlobs()];

    for (i = 0; i < whiteRedBlobs.GetNumBlobs(); i++) {
        currentBlob = whiteRedBlobs.GetBlob(i);
        px = (currentBlob->MaxX() + currentBlob->MinX()) / 2.0;
        py = (currentBlob->MaxY() + currentBlob->MinY()) / 2.0;
        redCenter[i] = cvPoint2D32f(px, py);

#ifdef DEBUG_PRINT    
        printf("%2.2f\t%2.2f\n", px, py);
#endif

        if (currentBlob->Area() > areaBlob) {
            // Add Cross to the image
            currentBlob->FillBlob(displayedImage, CV_RGB(255, 0, 0));
            cvCircle(displayedImage, cvPointFrom32f(redCenter[i]), 2, cvScalar(255, 0, 0), 10, 8, 0);
        }
    }

    // GREEN
    CvPoint2D32f greenCenter[whiteGreenBlobs.GetNumBlobs()];

    for (i = 0; i < whiteGreenBlobs.GetNumBlobs(); i++) {
        currentBlob = whiteGreenBlobs.GetBlob(i);
        px = (currentBlob->MaxX() + currentBlob->MinX()) / 2.0;
        py = (currentBlob->MaxY() + currentBlob->MinY()) / 2.0;
        greenCenter[i] = cvPoint2D32f(px, py);

#ifdef DEBUG_PRINT    
        printf("%2.2f\t%2.2f\n", px, py);
#endif

        if (currentBlob->Area() > areaBlob) {
            // Add Cross to the image
            currentBlob->FillBlob(displayedImage, CV_RGB(255, 0, 0));
            cvCircle(displayedImage, cvPointFrom32f(greenCenter[i]), 2, cvScalar(0, 255, 0), 10, 8, 0);
        }
    }

    // Populating the list of potential robots
    
    potRobList.robNum = 0;

    for (i = 0; i < robMax; i++)
        potRobList.robList[i].active = 0;

    int redUsage[whiteRedBlobs.GetNumBlobs()];
    int greenUsage[whiteGreenBlobs.GetNumBlobs()];

    for (i = 0; i < whiteRedBlobs.GetNumBlobs(); i++)
        redUsage[i] = 0;

    for (j = 0; j < whiteGreenBlobs.GetNumBlobs(); j++)
        greenUsage[j] = 0;



    // Detect Robots
    float distCenter[whiteRedBlobs.GetNumBlobs()][whiteGreenBlobs.GetNumBlobs()];
    for (i = 0; i < min(whiteRedBlobs.GetNumBlobs(), robMax); i++) {
        currentBlob = whiteRedBlobs.GetBlob(i);
        if (currentBlob->Area() > areaBlob) {
            for (j = 0; j < min(whiteGreenBlobs.GetNumBlobs(), robMax); j++) {
                currentBlob = whiteGreenBlobs.GetBlob(j);
                if (currentBlob->Area() > areaBlob) {
                    distCenter[i][j] = computeDist(redCenter[i], greenCenter[j]);
                    //printf("[%d] - [%d]: %2.2f\n", i, j, distCenter[i][j]);
                    //printf("[%d] - [%d]: %2.2f\n", i, j, distCenter[i][j]);
                    // Print a connection line if this could be a robot
                    if (redUsage[i] == 0 && greenUsage[j] == 0 && checkDistMarker(distCenter[i][j], distMark)) {
//.........這裏部分代碼省略.........

SHModel* ShapeModel( CvCapture* g_capture,StaticBGModel* BGModel , BGModelParams* BGParams){

	int num_frames = 0;
	int total_blobs=0;
	float Sumatorio = 0;
	float SumatorioDes = 0;
	IplImage* frame = NULL;

	STFrame* frameData = NULL;
	SHModel* Shape = NULL;

	CBlobResult blobs;
	CBlob *currentBlob;

	IplImage* ImGris = cvCreateImage(cvGetSize( BGModel->Imed ), 8, 1 );
	IplImage* Imblob = cvCreateImage(cvGetSize( BGModel->Imed ), 8, 3 );
	IplImage* lastBG = cvCreateImage( cvGetSize( BGModel->Imed ),8, 1 );
	IplImage* lastIdes = cvCreateImage( cvGetSize( BGModel->Imed ), IPL_DEPTH_32F, 1);
	cvZero(Imblob);
	// Iniciar estructura para modelo de forma

	Shape = ( SHModel *) malloc( sizeof( SHModel));
	if ( !Shape ) {error(4);return 0;}
	Shape->FlyAreaDes = 0;
	Shape->FlyAreaMedia=0;
	//Pone a 0 los valores del vector areas


	//EXTRACCION DE LOS BLOBS Y CALCULO DE MEDIANA/MEDIA Y DESVIACION TIPICA PARA TODOS LOS FRAMES
	cvSetCaptureProperty( g_capture,1,BGParams->initDelay ); // establecemos la posición
	while( num_frames < ShParams->FramesTraining ){
		frame = cvQueryFrame( g_capture );
		if ( !frame ) {
			error(2);
			break;
		}
		if ( (cvWaitKey(10) & 255) == 27 ) break;

		ImPreProcess( frame, ImGris, BGModel->ImFMask, 0, BGModel->DataFROI);

		// Cargamos datos del fondo
		if(!frameData ) { //en la primera iteración iniciamos el modelo dinamico al estático
			// Iniciar estructura para datos del nuevo frame
			frameData = InitNewFrameData( frame );
			cvCopy(  BGModel->Imed,frameData->BGModel);
			cvSet(frameData->IDesvf, cvScalar(1));
			cvCopy(  BGModel->Imed,lastBG);
		}
		else{	// cargamos los últimos parámetros del fondo.
			cvCopy( lastBG, frameData->BGModel);
			cvCopy( lastIdes,frameData->IDesvf );
		}
	//	obtener la mascara del FG y la lista con los datos de sus blobs.
		//// BACKGROUND UPDATE
		// Actualización del fondo
		// establecer parametros

		UpdateBGModel( ImGris,frameData->BGModel,frameData->IDesvf, BGParams, BGModel->DataFROI, BGModel->ImFMask );
		/////// BACKGROUND DIFERENCE. Obtención de la máscara del foreground
		BackgroundDifference( ImGris, frameData->BGModel,frameData->IDesvf, frameData->FG ,BGParams, BGModel->DataFROI);

		// guardamos las imagenes para iniciar el siguiente frame
		cvCopy( frameData->BGModel, lastBG);
		cvCopy(  frameData->IDesvf,lastIdes);

		//Obtener los Blobs y excluir aquellos que no interesan por su tamaño
//		cvSetImageROI(  frameData->FG , BGModel->DataFROI);

		blobs = CBlobResult( frameData->FG, NULL, 100, true );
		blobs.Filter( blobs, B_EXCLUDE, CBlobGetArea(),B_GREATER,100);
		blobs.Filter( blobs, B_EXCLUDE, CBlobGetPerimeter(),B_GREATER,1000);

		int j = blobs.GetNumBlobs();//numero de blos encontrados en el frame

		total_blobs=total_blobs+j; // Contabiliza los blobs encontrados para todos los frames

		//Recorrer Blob a blob y obtener las caracteristicas del AREA de cada uno de ellos

		for (int i = 0; i < blobs.GetNumBlobs(); i++ ){ //for 1

			currentBlob = blobs.GetBlob(i);

			CBlobGetArea();
			if(ShParams->SHOW_DATA_AREAS) {
				//printf("Area blob %d = %f ",i,currentBlob->area);
			}
			//Estimar la media de las Areas

			Sumatorio = Sumatorio + currentBlob->area;
			SumatorioDes = SumatorioDes + currentBlob->area*currentBlob->area;

			muestrearAreas( currentBlob->area);
			currentBlob->FillBlob( Imblob, CV_RGB(255,0,0));

		}//Fin del For 1

		Shape->FlyAreaMedia = Sumatorio / total_blobs;
		Shape->FlyAreaDes = (SumatorioDes / total_blobs) - Shape->FlyAreaMedia*Shape->FlyAreaMedia;

		num_frames += 1;
//.........這裏部分代碼省略.........

void iptask::markerDetect(void)
{
     IplImage * frame,*img_hsv,*img_proc,* new1;
     CvMemStorage * storage = cvCreateMemStorage(0);
     ros::NodeHandle n;
     ros::Publisher marker = n.advertise<ikat_ip_data::ip_marker_data>("marker_data",3);
     ros::Rate looprate(5);
     int count = 0;
     CvSeq * contours,*final_contour;
     int total_con;
     double maxarea;
     marker_data * Data =(marker_data *)malloc(sizeof(marker_data));
     CBlobResult blobs;
     CBlob * currentblob;
     CvPoint2D32f vertices[4];
     //CvCapture * img_video=cvCaptureFromAVI("downward-pipe-15_56_17.avi");
     frame=cvQueryFrame(img);
     cvNamedWindow("Image Actual");
     cvNamedWindow("final Image");
     img_hsv=cvCreateImage(cvGetSize(frame),8,3);
     img_proc=cvCreateImage(cvGetSize(frame),8,1);
     new1=cvCreateImage(cvGetSize(frame),8,1);
     while(ros::ok())
     {
         ikat_ip_data::ip_marker_data msg;
         IplImage * img_con=cvCreateImage(cvGetSize(frame),8,1);
         frame=cvQueryFrame(img);
         if(!frame)
                 break;
         cvShowImage("Image Actual",frame);
         cvCvtColor(frame,img_hsv,CV_RGB2HSV);
         cvInRangeS(img_hsv,cvScalar(100,100,100),cvScalar(120,170,255),img_proc);
         cvSmooth(img_proc,img_proc,CV_GAUSSIAN,11,11);
         cvErode(img_proc,img_proc);
         blobs=CBlobResult(img_proc,NULL,0);
         blobs.Filter(blobs,B_EXCLUDE,CBlobGetArea(),B_LESS,75);
         for (int i = 0; i < blobs.GetNumBlobs(); i++ )
         {
                 currentblob = blobs.GetBlob(i);
                 currentblob->FillBlob(img_proc,cvScalar(255));
         }
         cvCanny(img_proc,img_proc,10,200);
         total_con=cvFindContours(img_proc,storage,&contours,sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
         if(contours->total==0)
             continue;
         final_contour=cvApproxPoly(contours,sizeof(CvContour),storage,CV_POLY_APPROX_DP,1,1);
         maxarea=0;
         cvZero(img_con);
         CvBox2D rect;
         while(final_contour)
         {
              rect=cvMinAreaRect2(final_contour, storage);
              if(rect.size.height*rect.size.width>maxarea)
              {
                  Data->center.x=rect.center.x;
                  Data->center.y=rect.center.y;
                  Data->size.x=rect.size.width;
                  Data->size.y=rect.size.height;
                  Data->angle=rect.angle;
                  maxarea=rect.size.height*rect.size.width;
                  msg.Marker_data[0]=Data->center.x;
                  msg.Marker_data[1]=Data->center.y;
                  msg.Marker_data[2]=Data->angle;
              }
              final_contour=final_contour->h_next;
         }
         cvBoxPoints(rect,vertices);
         cvLine(frame,cvPointFrom32f(vertices[0]),cvPointFrom32f(vertices[1]),cvScalarAll(255),2);
         cvLine(frame,cvPointFrom32f(vertices[1]),cvPointFrom32f(vertices[2]),cvScalarAll(255),2);
         cvLine(frame,cvPointFrom32f(vertices[2]),cvPointFrom32f(vertices[3]),cvScalarAll(255),2);
         cvLine(frame,cvPointFrom32f(vertices[3]),cvPointFrom32f(vertices[0]),cvScalarAll(255),2);
         ROS_INFO("center x :[%f]",msg.Marker_data[0]);
         ROS_INFO("center y :[%f]",msg.Marker_data[1]);
         ROS_INFO("angle : [%f]",msg.Marker_data[2]);
         marker.publish(msg);
         cvShowImage("final Image",frame);
         char c=cvWaitKey(33);
         if  (c==27)
         break;
         ros::spinOnce();
         ++count;
         looprate.sleep();
     }
     cvDestroyWindow("Image Actual");
     cvDestroyWindow("final Image");
     free(Data);
}

/*
 * 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];
//.........這裏部分代碼省略.........

double findShadow(IplImage *l_img, int hue,int sat,int val,int threshold, double blobLowLimit,double blobHighLimit){
	// Input HSV value of color blob your seeking, acceptable threshold of that color, and Min and Max blob sizes beeing sought out. 
// Input HSV value of color blob your seeking, acceptable threshold of that color, and Min and Max blob sizes beeing sought out. 
	//Ouput: pointer to data array, size[#ofblobs*3+1]; Format data=[Number of Blobs, Area1,X of center1, y of center1, Area2,X of center2,y of center2,...,areaN,X of centerN, Y of centerN];
    

    

	// Image variables
	IplImage* local_copy = cvCloneImage(l_img);
	IplImage* imageSmooth = cvCreateImage( cvGetSize(l_img),8,3);//Gausian Filtered image
	IplImage* imageSuperSmooth = cvCreateImage( cvGetSize(l_img),8,3);//Gausian Filtered image
	IplImage* imageHSV = cvCreateImage( cvGetSize(l_img),8,3); //HSV image
	IplImage* i1 = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
	IplImage* i2 = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
	IplImage* i_ts = cvCreateImage( cvGetSize(l_img),8,1);//desired color filtered image
	IplImage* planeH = cvCreateImage(cvGetSize(l_img),8,1); //Hue
	IplImage* planeS = cvCreateImage(cvGetSize(l_img),8,1); //Saturation
	IplImage* planeV = cvCreateImage(cvGetSize(l_img),8,1); //Brightness
	IplImage* planeSmoothV = cvCreateImage(cvGetSize(l_img),8,1); //Brightness
	IplImage* imageSmoothHSV = cvCreateImage( cvGetSize(l_img),8,3); //HSV image
	IplImage* obsdetmask = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
	IplImage* obsdetmask_dil = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
	IplImage* obsdetmask_b = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
	IplImage* obsdetmask_bdil = cvCreateImage( cvGetSize(l_img),8,1); //Obs det mask
	//Blob variables
	CBlobResult mask_bls;
	CBlob	mask_bl;
	CBlobResult blobs;
	CBlob blob;
	CBlobResult blobs1;
	CBlob blob1;
	CBlobGetXCenter getXCenter;
	CBlobGetYCenter getYCenter;
	//Output Variable
	//Gausian Filter
	cvSmooth(l_img,imageSmooth,CV_GAUSSIAN,13,13,0,0);
	cvSmooth(l_img,imageSuperSmooth,CV_GAUSSIAN,41,41,0,0);
	//cvShowImage("View2a",imageSmooth);
	
	
	
	//Covert RGB to HSV
	cvCvtColor(imageSmooth,imageHSV,CV_BGR2HSV);
	cvCvtColor(imageSuperSmooth,imageSmoothHSV,CV_BGR2HSV);
	cvCvtPixToPlane(imageSuperSmooth,NULL,NULL,planeSmoothV,0);
	cvCvtPixToPlane(imageHSV, planeH,planeS,planeV,0);//Extract the 3 color components
	cvSetImageROI(imageHSV,cvRect(0,imageHSV->height/3,imageHSV->width,imageHSV->height*2/3));
	IplImage* planeH1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Hue
	IplImage* planeS1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Saturation
	IplImage* planeV1 = cvCreateImage(cvGetSize(imageHSV),8,1); //Brightness
	cvCvtPixToPlane(imageHSV, planeH1,planeS1,planeV1,0);//Extract the 3 color components
	cvResetImageROI(imageHSV);
	
	
	cvShowImage("Dark_Value",planeV);
	cvShowImage("Dark_Sat",planeS);
	cvShowImage("Dark_Hue",planeH);
	cvSet(obsdetmask, cvScalar(0,0,0));
	cv::waitKey(3);
	
	
	int maxDark = 0;
	int minDark = 255;
	int minDarknessValue=0;
	int maxDarknessValue = 0;
	int midDarknessValue = 0;
	//Filter image for desired Color, output image with only desired color highlighted remaining
	for( int y = 0; y < planeH1->height; y++ ){
		unsigned char* h = &CV_IMAGE_ELEM( planeH1, unsigned char, y, 0 );
		unsigned char* s = &CV_IMAGE_ELEM( planeS1, unsigned char, y, 0 );
		unsigned char* v = &CV_IMAGE_ELEM( planeV1, unsigned char, y, 0 );
		for( int x = 0; x < planeH1->width*planeH1->nChannels; x += planeH1->nChannels ){
		  //if(x<5){ROS_INFO("hsv[x] is %d,%d,%d",h[x],v[x],x]);}
			//int f= HSV_filter(h[x],s[x],v[x],threshold,minDarknessValue,maxDarknessValue,midDarknessValue,hue,sat,val);
			int diff = abs((h[x]-hue));
			if(((diff < threshold)||(v[x]<MIN_BRIGHT)||(s[x]<MIN_SAT)))
			{ 
			  ((uchar *)(obsdetmask->imageData + (y+planeH->height-planeH1->height)*obsdetmask->widthStep))[x]=255;
			   if(v[x]<minDark)
			   {minDark=v[x];}
			    if(v[x]>maxDark)
			    {maxDark=v[x];}
			}
			else
			{
			  ((uchar *)(obsdetmask->imageData + (y+planeH->height-planeH1->height)*obsdetmask->widthStep))[x]=0;
			}
		}
	}//debug
	cvDilate(obsdetmask,obsdetmask_dil,NULL,1);
	cvShowImage("Dark_ObsDetPre",obsdetmask_dil);
	mask_bls = CBlobResult(obsdetmask_dil,NULL,0);
	mask_bls.Filter(mask_bls,B_EXCLUDE,CBlobGetArea(),B_LESS,MASK_MIN_BLOB); // Filter Blobs with min and max size
	mask_bls.GetNthBlob( CBlobGetArea(), 0, mask_bl );
	cvSet(obsdetmask_b, cvScalar(0,0,0));
	mask_bl.FillBlob(obsdetmask_b,CV_RGB(255,255,255));
	cvDilate(obsdetmask_b,obsdetmask_bdil,NULL,5);
	cvShowImage("Dark_ObsDet",obsdetmask_bdil);
	cvWaitKey(3);
//.........這裏部分代碼省略.........