Exemple #1
0
    void operator()( const blocked_range2d<size_t>& r ) const {
        for( size_t x=r.rows().begin(); x!=r.rows().end(); ++x )
        {
            for( size_t y=r.cols().begin(); y!=r.cols().end(); ++y ) 
            {
                if(my_cam->isYellow_small(x, y))
                    returnPixel1C(my_seg_yellow, x, y) = 255;
                else
                    returnPixel1C(my_seg_yellow, x, y) = 0;

                // if(my_cam->isGreen_small(x, y))
                //     returnPixel1C(my_seg_green, x, y) = 255;
                // else
                //     returnPixel1C(my_seg_green, x, y) = 0;


                // if(my_cam->isBlue_small(x, y))
                //  returnPixel1C(my_seg_blue, x, y) = 255;
                // else
                //  returnPixel1C(my_seg_blue, x, y) = 0;

                if(my_cam->isRed_small(x, y))
                    returnPixel1C(my_seg_red, x, y) = 255;
                else
                    returnPixel1C(my_seg_red, x, y) = 0;
            }
        }
    }
Exemple #2
0
CamControl::CamControl(CamCapture &cam) : IMAGE_WIDTH(cam.width()), IMAGE_HEIGHT(cam.height()), 
CENTRE_RECT_X1(IMAGE_WIDTH/4),	
CENTRE_RECT_Y1 (IMAGE_HEIGHT/4),
CENTRE_RECT_X2 ((IMAGE_WIDTH*3)/4),
CENTRE_RECT_Y2 ((IMAGE_HEIGHT*3)/4)
{
	pass_counter = 0;	
	state_of_motion = 0;
	prev_img_flag = 0;	
	
}
Exemple #3
0
GoalKeeperAction GoalKeeper::keeperUpdate(CamCapture &cam, HeadMotor &hm, CamControl* camcont, FeatureDetection* fd, MotionModel &mm)
{
	cam.getImage();
	fd->getLandmarks(cam, hm, mm);
	if(camcont->findBall(*fd,hm) != BALLFOUND)
		return STAY;
	double x1 = fd->ball.r*cos(deg2rad(fd->ball.theta));
	double y1 = fd->ball.r*sin(deg2rad(fd->ball.theta));
	printf("Ball Position >>>>>>>>>>>>>>>> x: %lf \t y: %lf\n", x1, y1);
	if(fd->ball.r > ACTIVATION_RADIUS)
		return STAY;
	if(fd->ball.r < CRITICAL_RADIUS)
	{
		printf("Reached CRITICAL_RADIUS\n");
		if(fd->ball.theta > 0)
			return FALLRIGHT;
		return FALLLEFT;
	}
	cvWaitKey(50);
	cam.getImage();
	fd->getLandmarks(cam, hm, mm);
	if(fd->ball.r > ACTIVATION_RADIUS)
		return STAY;
	double x2 = fd->ball.r*cos(deg2rad(fd->ball.theta));
	double y2 = fd->ball.r*sin(deg2rad(fd->ball.theta));
	printf("difference: %lf\n", x2 - x1);
	if(x2 < x1)
		return STAY;
	if(fabs(x2 - x1) < ACTIVATION_THRESHOLD)
		return STAY;
	double alpha = (x2*(y1 - y2))/(x1 - x2);
	double estGoalImpact = y2 - alpha;
	printf("Estimated Point of Impact: %lf\n", estGoalImpact);
	if(fabs(estGoalImpact) > GOAL_WIDTH/2.0)
		return STAY;
	if(estGoalImpact < 0)
		return FALLLEFT;
	return FALLRIGHT;
}
Exemple #4
0
FeatureDetection::FeatureDetection(CamCapture &cam): IMAGE_HEIGHT(cam.height_small()), IMAGE_WIDTH(cam.width_small())
{
    seg_red = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), 8, 1);
    seg_blue = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), 8, 1);
    seg_yellow = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), 8, 1);
    labelImg = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), IPL_DEPTH_LABEL, 1);
    labelImg_small = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), IPL_DEPTH_LABEL, 1);
    seg_white_count = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    seg_white = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    seg_black = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    seg_green = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    ballRatio=1.0;
    ballFound_var = false;
    tempnLand = 0;
    tempnObstacle = 0;
    o.clear();
    l.clear();
    constants.open("Source/lut/constants.dat",ios::binary);

}
Exemple #5
0
void getBoundary(CamCapture &camera)
{
    // IplImage* show_image3 = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), 8, 3);			//show full size rgb image
    // IplImage* show_image = cvCreateImage(cvSize(IMAGE_WIDTH, IMAGE_HEIGHT), 8, 1);			//show full size rgb image
    // cvZero(show_image);
    IplImage* probImage = cvCreateImage(cvSize(IMAGE_WIDTH/4,IMAGE_HEIGHT/4), 8, 1);		//probab of being inside field
    cvZero(probImage);

    int rowsum[IMAGE_HEIGHT/4] = {};


    for(int x=0; x<IMAGE_WIDTH/4; x++)
    {
        for(int y=0; y<IMAGE_HEIGHT/4; y++)
        {
            int r_count=0, g_count=0, w_count=0, b_count=0;

            if((x-IMAGE_WIDTH/8)*(x-IMAGE_WIDTH/8) + (-y+IMAGE_HEIGHT/8)*(-y+IMAGE_HEIGHT/8) < R*R/16)
            {
                for(int xx=0; xx<4; xx++)
                {
                    for(int yy=0; yy<4; yy++)
                    {
                        int tx = x*4 + xx;
                        int ty = y*4 + yy;
                        if(camera.isRed_small(tx, ty))
                            r_count++;
                        if(camera.isGreen_small(tx, ty))
                            g_count++;
                        if(camera.isWhite_small(tx, ty))
                            w_count++;
                        if(camera.isBlack_small(tx, ty))
                            b_count++;
                    }
                }
            }
            returnPixel1C(probImage, x, y) = (r_count*16 + g_count*8 + w_count + b_count);
            //white
            if(w_count == 16)
                returnPixel1C(seg_white_count1, x, y) = 255;
            else
                returnPixel1C(seg_white_count1, x, y) = (w_count*16)%256;
            if(w_count>4)
            {
                returnPixel1C(seg_white1, x, y) = 255;

#ifdef SHOW_SUBSAMPLE
                returnPixel3C(subSampled, x, y, 0) = 255;
                returnPixel3C(subSampled, x, y, 1) = 255;
                returnPixel3C(subSampled, x, y, 2) = 255;
#endif
            }

            //black
            if(b_count>4)
            {
                returnPixel1C(seg_black1, x, y) = 255;

#ifdef SHOW_SUBSAMPLE
                returnPixel3C(subSampled, x, y, 0) = 0;
                returnPixel3C(subSampled, x, y, 1) = 0;
                returnPixel3C(subSampled, x, y, 2) = 0;
#endif
            }

            //green
            if(g_count>4)
            {
                returnPixel1C(seg_green1, x, y) = 255;
#ifdef SHOW_SUBSAMPLE
                returnPixel3C(subSampled, x, y, 1) = 255;
#endif
            }

            rowsum[y] += pixelColor1C(probImage, x, y);
        }
    }//probability image created

    //binarization of image - white(255) inside field, black(0) outside
    IplImage* binaryImage = cvCreateImage(cvSize(IMAGE_WIDTH/4,IMAGE_HEIGHT/4), 8, 1);		//binary img inside field
    cvZero(binaryImage);
    int tpix, trow, twin; 		//thresholds
    tpix = 30;					//minimum required probab image
    trow = IMAGE_WIDTH*4.5;
    twin = 36*18;

    for(int x=0; x<IMAGE_WIDTH/4; x++)
    {
        for(int y=0; y<IMAGE_HEIGHT/4; y++)
        {
            //check if point inside fisheye image
            if((x-IMAGE_WIDTH/8)*(x-IMAGE_WIDTH/8) + (-y+IMAGE_HEIGHT/8)*(-y+IMAGE_HEIGHT/8) > R*R/16)
                continue;

            //check if pixel passes minimum threshold
            if(pixelColor1C(probImage, x, y) < tpix)
            {
                returnPixel1C(binaryImage, x, y) = 0;
                continue;
            }

            //check if rowsum passes minimum threshold
            if(rowsum[y] >= trow)
            {
                returnPixel1C(binaryImage, x, y) = 255;
                continue;
            }

            //else do the expensive test
            int sum = 0;
            for(int i = -4; i< 5; i++)
            {
                for(int j=-4; j<5; j++)
                {
                    if( (x+i)<IMAGE_WIDTH/4 && (x+i)>=0 && (y+j)<IMAGE_HEIGHT/4 && (y+j)>=0 )
                        sum += pixelColor1C(probImage, x+i, y+j);
                }
            }
            if (sum >= twin)
                returnPixel1C(binaryImage, x, y) = 255;
            else
                returnPixel1C(binaryImage, x, y) = 0;

        }
    }//binary image created
    // cvZero(show_image);
    // cvResize(binaryImage, show_image);
    // cvShowImage("binary", show_image);
    // cvWaitKey();

    //retrieving field boundary

    IplImage* histogram = cvCreateImage(cvSize(IMAGE_WIDTH/4, 1), 8, 1);			//image saves y boundary value for each x
    cvZero(histogram);
    returnPixel1C(histogram, 0, 0) = IMAGE_HEIGHT/4 - 1;
    for(int x = 1; x < IMAGE_WIDTH/4; x++)
    {
        int y=0, count=0;
        for(y = 0; y < IMAGE_HEIGHT/4; y++)
        {
            if(pixelColor1C(binaryImage, x, y))
            {
                count++;
                if(count==4)
                    break;
            }
            else
                count=0;
        }
        if(count == 4)
            returnPixel1C(histogram, x, 0) = y;
        else
            returnPixel1C(histogram, x, 0) = IMAGE_HEIGHT/4 - 1;

    }


    //smoothning and gap filling
    cvSmooth(histogram, histogram, CV_GAUSSIAN, 3);
    cvSmooth(histogram, histogram, CV_MEDIAN, 3);

    //local convex hull
    IplImage* boundary_img = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    cvZero(boundary_img);
    for(int x=0; x<IMAGE_WIDTH/4; x++)
    {
        bound[x] = pixelColor1C(histogram, x, 0);
    }

    int convexity;
    CvPoint lastOnHull = cvPoint(0, bound[0]);
    for(int x=1; x<IMAGE_WIDTH/4 - 1; x++)
    {
        convexity = 2*(bound[x] - bound[x-1]) - (bound[x+1] - bound[x-1]);

        if(convexity > 0)
            continue;
        else
        {
            cvLine(boundary_img, lastOnHull, cvPoint(x, bound[x]), cvScalar(255,255,0), 1);			//connect
            lastOnHull = cvPoint(x, bound[x]);
        }
    }
    // cvZero(show_image);
    // cvResize(boundary_img, show_image);
    // cvShowImage("boundary", show_image);
    //cvWaitKey();

    //releaase memory
    //cvReleaseImage(&show_image);
    cvReleaseImage(&boundary_img);
    cvReleaseImage(&binaryImage);
    cvReleaseImage(&probImage);
    cvReleaseImage(&histogram);
}
Exemple #6
0
void FeatureDetection::getInGreen(CamCapture &cam)
{
    // Make reduced image
    int rowsum[IMAGE_HEIGHT/4];
    IplImage* binary_image = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    IplImage* prob_image = cvCreateImage(cvSize(IMAGE_WIDTH/4, IMAGE_HEIGHT/4), 8, 1);
    cvZero(binary_image);
    cvZero(prob_image);

    cvZero(seg_white);
    cvZero(seg_black);
    cvZero(seg_green);
    cvZero(seg_white_count);
 //New Begin
    const int tmin = 30;
    const int trow = IMAGE_WIDTH*4.5;
    const int tsum = 36*18;//30*18;//36*18;
    for(int y = 0; y < IMAGE_HEIGHT/4; y++)
    {
        rowsum[y] = 0;
        for(int x = 0; x < IMAGE_WIDTH/4; x++)
        {
            int wcount = 0;
            int gcount = 0;
            int bcount = 0;
            int rcount = 0;
            for(int xx = 0; xx < 4; xx++)
            for(int yy=0; yy < 4; yy++)
            {
                int tx = x*4 + xx;
                int ty = y*4 + yy;
                if(cam.isGreen_small(tx, ty))
                    gcount++;
                if(cam.isWhite_small(tx, ty))
                 wcount++;
                if(cam.isBlack_small(tx, ty))
                 bcount++;
                if(cam.isRed_small(tx, ty))
                 rcount++;
            }
            returnPixel1C(prob_image, x, y) = wcount + bcount + rcount*16 + gcount*8;
            if(gcount > 4)
                returnPixel1C(prob_image, x, y) += gcount*2;
            if(wcount)
            {
                if(wcount==16)
                    returnPixel1C(seg_white_count, x, y) = 255;
                else    
                    returnPixel1C(seg_white_count, x, y) = (wcount*16)%256;
                if(wcount>4)
                    returnPixel1C(seg_white, x, y) = 255;
            }
            if(gcount>4)
            {
                returnPixel1C(seg_green, x, y) = 255;
            }
            if(bcount > 4)
            {
                returnPixel1C(seg_black, x, y) = 255;
            }
            if(rcount)
            {
                returnPixel1C(seg_red, x, y) = 255;
            }
            else
            {
    //          returnPixel1C(seg_black, x, y) = 255;
            }
            rowsum[y] += pixelColor1C(prob_image, x, y);
        }
    }
    //New End

    for(int y = 0; y < IMAGE_HEIGHT/4; y++)
    {
        for(int x = 0; x < IMAGE_WIDTH/4; x++)
        {
            //check if point inside fisheye image
            if((x-IMAGE_WIDTH/8)*(x-IMAGE_WIDTH/8) + (-y+IMAGE_HEIGHT/8)*(-y+IMAGE_HEIGHT/8) > 120*120/16)  //radius of image is 120 pixels
                continue;
            //for each pixel, first check tmin
            if(pixelColor1C(prob_image, x, y) < tmin)
            {
                returnPixel1C(binary_image, x, y) = 0;
                continue;
            }

            //Now, check row sum is above certain threshold
            if(rowsum[y] > trow)
            {
                returnPixel1C(binary_image, x, y) = 255;
                continue;   
            }

            //Now do the expensive test
            int sum = 0;
            for(int i = -4; i <5; i++)
            {
                for(int j = -4; j < 5; j++)
                {
                    if((x + i > 0)&&(y + j > 0)&&(x + i < IMAGE_WIDTH/4)&&(y + j < IMAGE_HEIGHT/4))
                    {
                        sum = sum + pixelColor1C(prob_image, x +i, y+j);
                    }
                }
            }

            if(sum >= tsum)
            {
                returnPixel1C(binary_image, x, y) = 255;
                continue;
            }
            else
            {
                returnPixel1C(binary_image, x, y) = 0;
                continue;
            }
        }
    }


    IplImage* histogram = cvCreateImage(cvSize(IMAGE_WIDTH/4, 1), 8, 1);

    for(int x = 0; x < IMAGE_WIDTH/4; x++)
    {
        int y=0, count = 0;
        for(y = 0; y < IMAGE_HEIGHT/4; y++)
        {
            if(pixelColor1C(binary_image, x, y))
            {
                count++;
                if(count == 4)
                    break;
            }
            else
                count = 0;
        }
        if(count == 4)
            returnPixel1C(histogram, x, 0) = y;
        else
            returnPixel1C(histogram, x, 0) = IMAGE_HEIGHT/4 - 1;
    }


    cvSmooth(histogram, histogram, CV_GAUSSIAN, 3);
    cvSmooth(histogram, histogram, CV_MEDIAN, 15);

    // for(int x = 0; x < IMAGE_WIDTH/4; x++)
    // {
    //     // for(int y = 0; y < returnPixel1C(histogram, x, 0); y++)
    //     // {
    //         // returnPixel1C(seg_white, x, y) = 0;
    //         // returnPixel1C(seg_black, x, y) = 0;
    //         // returnPixel1C(seg_white_count, x, y) = 0;
            
    //     // }

      
    //     // Uncomment this to see histogram boundary in seg_black
    //     // Caution: uncommenting this will cause segmentation faults in some cases! Uncomment only when testing
    //     int temp =  returnPixel1C(histogram, x, 0);
    //     if(temp > 1)
    //       returnPixel1C(seg_black, x,(int) (temp-1)) = 127;
    // }

    // cvShowImage("boundary", seg_black);
    
    // for(int x = 0; x < IMAGE_WIDTH; x++)
    // {
    //     for(int y = 0; y < returnPixel1C(histogram, x/4, 0)*4; y++)
    //     {
    //         returnPixel1C(seg_red, x, y) = 0;
    //     }
    // }
    #ifndef INTEL_BOARD_DISPLAY
    cvNamedWindow("RED");
    cvMoveWindow("RED",50,600);
    cvShowImage("RED", seg_red);
    #endif

    cvReleaseImage(&histogram);
    // convex corner algo?

    // IplImage* show_image = cvCreateImage(cvSize(IMAGE_WIDTH*2, IMAGE_HEIGHT*2), 8, 1);
    // cvResize(seg_black, show_image);
    // cvShowImage("Black", show_image);
    
    
    // cvResize(seg_white, show_image);
    // cvShowImage("White", show_image);
    // cvResize(seg_green, show_image);
    // cvShowImage("Green", show_image);
    // cvResize(binary_image, show_image);
    // cvShowImage("Binary", show_image);
    // cvResize(prob_image, show_image);
    // cvShowImage("Prob", show_image);
    // cvReleaseImage(&show_image);

    //Segmentation done
    cvReleaseImage(&binary_image);
    cvReleaseImage(&prob_image);

}