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