/***********************************************************************/ /** @name imageglobal.c Contains global image processing routines. @author Birgit Graf, UWA, 1998, modified 2000 (Mk3/4) @author Thomas Braunl, UWA, 1998 (HSV) */ /***********************************************************************/ /*@{*/ #include #include #include "imageglobal.h" #include "general.h" #include "driveglobal.h" /** Ball coordinates when ball has been detected (position on field) */ float ball_x, ball_y; /** factor to calculate y position from pixels to meters depending on x position (achieved by getting width in m of object that has width of screen at selected y positions. fact = val(m)/imagerows) */ float yfact[imagerows] = /* camPos=MIDDLE (angle 150) */ {0.0106, 0.0093, 0.0080, 0.0069, 0.0065, 0.0061, 0.0058, 0.0054, 0.0050, 0.0048, 0.0045, 0.0043, 0.0040, 0.0039, 0.0039, 0.0038, 0.0037, 0.0036, 0.0036, 0.0035, 0.0035, 0.0034, 0.0034, 0.0033, 0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0030, 0.0029, 0.0029, 0.0028, 0.0028, 0.0027, 0.0027, 0.0026, 0.0026, 0.0025, 0.0024, 0.0024, 0.0023, 0.0023, 0.0023, 0.0022, 0.0022, 0.0022, 0.0021, 0.0021, 0.0020, 0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019, 0.0018, 0.0018, 0.0018, 0.0018, 0.0017, 0.0017}; /* camPos=UP (angle 225) */ /* {0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0110, 0.0100, 0.0090, 0.0083, 0.0075, 0.0068, 0.0060, 0.0057, 0.0055, 0.0053, 0.0051, 0.0050, 0.0049, 0.0048, 0.0046, 0.0045, 0.0044, 0.0043, 0.0041, 0.0040, 0.0039, 0.0038, 0.0037, 0.0036, 0.0035, 0.0034, 0.0034, 0.0033, 0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0029, 0.0029, 0.0028, 0.0027, 0.0027, 0.0026, 0.0025, 0.0024, 0.0023};*/ /* camPos=DOWN (angle 0) */ /* {0.0026, 0.0026, 0.0026, 0.0026, 0.0025, 0.0025, 0.0025, 0.0025, 0.0024, 0.0024, 0.0024, 0.0023, 0.0023, 0.0023, 0.0023, 0.0022, 0.0022, 0.0022, 0.0022, 0.0021, 0.0021, 0.0021, 0.0021, 0.0021, 0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019, 0.0018, 0.0018, 0.0018, 0.0018, 0.0017, 0.0017, 0.0017, 0.0017, 0.0016, 0.0016, 0.0016, 0.0015, 0.0015, 0.0015, 0.0015, 0.0014, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000};*/ /** factor to get x position in meters from x position in pixels */ float x2m[imagerows] = /* camPos=MIDDLE (angle 150) */ {0.520, 0.460, 0.400, 0.340, 0.320, 0.300, 0.280, 0.260, 0.240, 0.227, 0.215, 0.202, 0.190, 0.182, 0.174, 0.166, 0.158, 0.150, 0.145, 0.140, 0.135, 0.130, 0.125, 0.121, 0.117, 0.113, 0.109, 0.105, 0.101, 0.097, 0.094, 0.090, 0.087, 0.084, 0.081, 0.078, 0.076, 0.073, 0.071, 0.068, 0.066, 0.063, 0.061, 0.059, 0.057, 0.055, 0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037, 0.035, 0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021}; /* camPos=UP (angle 255) */ /* {0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.900, 0.810, 0.750, 0.690, 0.630, 0.570, 0.510, 0.450, 0.412, 0.375, 0.337, 0.300, 0.287, 0.274, 0.261, 0.248, 0.235, 0.226, 0.217, 0.208, 0.200, 0.192, 0.184, 0.176, 0.168, 0.160, 0.155, 0.150, 0.145, 0.140, 0.135, 0.131, 0.127, 0.123, 0.119, 0.115, 0.111, 0.107, 0.103, 0.100, 0.096, 0.092, 0.088, 0.084, 0.080, 0.076, 0.072};*/ /* camPos=DOWN (angle 0) */ /*{0.095, 0.095, 0.092, 0.090, 0.087, 0.084, 0.081, 0.078, 0.075, 0.072, 0.070, 0.067, 0.065, 0.063, 0.061, 0.059, 0.057, 0.055, 0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037, 0.035, 0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021, 0.019, 0.017, 0.015, 0.013, 0.012, 0.011, 0.010, 0.009, 0.008, 0.007, 0.006, 0.005, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000};*/ /** ball distance in meters depending on x and y position in pixels */ /* float ball_dist[imagerows / 2][imagecolumns]; */ /** threshold for comparison of colours to ballcolour */ int thresh_ball = 5; /** threshold for comparison of colours to goalcolour */ int thresh_goal = 10; /** ball colour */ int BallColour = 60; /** goal colour */ int GoalColour = 220; /** permanent flag to select whether robot is attacking blue or yellow goal */ int drive_to_blue_goal = TRUE; /***********************************************************************/ /** Init camera. Called by main(). @see main() */ /***********************************************************************/ void Init_Cam() { int camera; /*int bright, hue, sat;*/ /* int i,x,y; */ camera = CAMInit(WIDE); if (camera < COLCAM) error("No colour camera!"); else { if (camera == NOCAM) error("No camera!\n"); else if (camera == INITERROR) error("CAMInit!\n"); } /* set brightness and hue */ /* CAMGet(&bright, &hue, &sat); CAMMode(NOAUTOBRIGHTNESS); CAMSet(135, 130, sat); */ /* set up table for ball distance */ /* for (i = 0; i < 3; i++) */ /* for(x = 0; x < imagerows / 2; x++) */ /* for(y = 0; y < imagecolumns; y++) */ /* ball_dist[i][x][y] = hypot(y2m[i][x], x*xfact[i][x]); */ } /***********************************************************************/ /** Change camera parameters. Changes brightness, hue and saturation in case default values aren't good enough (e.g. different light conditions). Function as in RoBiOS-Setup, but without camera-initialisation, works only for colour-camera. Called by main(). @see main() */ /***********************************************************************/ void set_cam_parameters () { int boc[] = {0, 0, 0, 0}; int ind = 0; image greyimg; colimage img; int end_proc = FALSE; LCDClear(); LCDMenu(" + "," - ","Nxt","END"); LCDPrintf("Camera param.:\n"); while (!end_proc) { CAMGet(&boc[0], &boc[1], &boc[2]); if (boc[3] == 0) CAMMode(NOAUTOBRIGHTNESS); else CAMMode(AUTOBRIGHTNESS); LCDSetPos(2, 0); LCDPrintf("Brigh.: %3.d\n", boc[0]); LCDPrintf("Hue : %3.d\n", boc[1]); LCDPrintf("Satur.: %3.d\n", boc[2]); CAMGetColFrame(&img, FALSE); IPColor2Grey(&img, &greyimg); LCDPrintf("AutoBr.: "); if (boc[3] == 0) LCDPrintf("OFF\n"); else LCDPrintf("ON \n"); LCDSetChar(2 + ind, 15, '*'); LCDPutGraphic (&greyimg); switch (KEYRead()) { case KEY1: boc[ind] ++; if (ind == 0 || ind == 3) { if (ind == 0) { if (boc[ind] > 179) boc[ind] = 0; } else if (boc[ind] > 1) boc[ind] = 0; } else if (boc[ind] > 255) boc[ind] = 0; CAMSet(boc[0], boc[1], boc[2]); break; case KEY2: boc[ind] --; if (ind == 0 || ind == 3) { if (ind == 0) { if (boc[ind] < 0) boc[ind] = 179; } else if (boc[ind] < 0) boc[ind] = 1; } else if (boc[ind] < 0) boc[ind] = 255; CAMSet(boc[0], boc[1], boc[2]); break; case KEY3: LCDSetChar(2 + ind, 15, ' '); ind ++; if (ind > 3) ind = 0; break; case KEY4: LCDClear(); end_proc = TRUE; break; default: break; } } } /***********************************************************************/ /** Change parameters for image processing. Changes thresholds which are used to select ball/goal coloured pixels from others and size of ball/goal. Called by main(). @see main() */ /***********************************************************************/ void set_img_parameters() { int ind = 0; int end_proc = FALSE; LCDClear(); LCDMenu("CHG", "NXT", " ", "END"); while (!end_proc) { LCDSetPos(0, 0); LCDPrintf("Image parameters\n"); LCDSetPos(2, 0); LCDPrintf("Thresh ball\n"); LCDSetPos(3, 0); if (!I_am_goalie()) LCDPrintf("Thresh goal\n"); LCDSetChar(2 + ind, 15, '*'); switch (KEYGet()) { case KEY1: switch(ind) { case 0: thresh_ball = set_iparam("Thresh ball", 0, thresh_ball, 100, 1); break; case 1: thresh_goal = set_iparam("Thresh goal", 0, thresh_goal, 100, 1); break; default: break; } LCDMenu("CHG", "NXT", " ", "END"); break; case KEY2: LCDSetChar(2 + ind, 15, ' '); ind ++; if (I_am_goalie()) if (ind > 0) ind = 0; if (ind > 1) ind = 0; break; case KEY4: LCDClear(); end_proc = TRUE; break; default: break; } } } /***********************************************************************/ /** Change RBG to HSV -- use hue only. Thomas Braunl, UWA 1998. Called by HSVhue() and set_colour(). @see HSVhue() @see set_colour() @param r,g,b rgb value of single pixel @return hue for single RGB value */ /***********************************************************************/ #define MIN(a,b) (ab?a:b) int RGBtoHue(BYTE r, BYTE g, BYTE b) { int hue /*,sat, val*/, delta, max, min; max = MAX(r, MAX(g,b)); min = MIN(r, MIN(g,b)); delta = max - min; hue =0; /* initialise hue*/ /* val = max; if (max != 0) sat = delta / max; else sat = 0; if (sat == 0) hue = NO_HUE; */ if (2 * delta <= max) hue = NO_HUE; else { if (r == max) hue = 42 + 42*(g-b) / delta; /* 1*42 */ else if (g == max) hue = 126 + 42*(b-r) / delta; /* 3*42 */ else if (b == max) hue = 210 + 42*(r-g) / delta; /* 5*42 */ /* now: hue is in range [0..252] */ } return hue; } /***********************************************************************/ /** Convert an rgb image to hue in grayscale. Thomas Braunl, UWA 1998. Called by find_object(). @see find_object() @param imageIn, imageOut input / output picture */ /***********************************************************************/ void HSVhue(BYTE *imageIn, BYTE *imageOut) { int i,j; for(i=0, j=0; i < imagerows*imagecolumns; i++, j+=3) imageOut[i] = RGBtoHue(imageIn[j],imageIn[j+1],imageIn[j+2]); } /***********************************************************************/ /** Set Ball/Goal colour. Set ball/goal colour to colour detected in middle of picture (mean value of 5x5 area around middle of picture). Called by set_ball_colour() and set_goal_colour(). @see set_ball_colour() @see set_goal_colour() @param img colour picture @return mean colour of image middle and surroundings */ /***********************************************************************/ int set_colour(colimage img) { int row, column; int count = 0; int my_hue, hue; my_hue = 0; for (row = imagerows/2 - 2; row < (imagerows/2 + 3); row ++) for (column = imagecolumns/2 - 2; column < imagecolumns/2 + 3; column ++) { hue = RGBtoHue(img[row][column][0], img[row][column][1], img[row][column][2]); if (hue != NO_HUE) { my_hue += hue; count ++; } } if (count != 0) my_hue /= count; LCDClear(); LCDMenu("", "", "", "OK"); LCDSetPos(7,0); LCDPrintf("HUE: %3d\n", my_hue); KEYWait(KEY4); LCDClear(); return my_hue; } /***********************************************************************/ /** Set Ball colour. Called by main(). @see main() @param colour[] array of 3 values which contain r,g,b-values for colour @param img colour picture @return detected colour in referenced array */ /***********************************************************************/ void set_ball_colour(colimage img) { BallColour = set_colour(img); } /***********************************************************************/ /** Set Goal colour. Called by main(). @see main() @param colour[] array of 3 values which contain r,g,b-values for colour @param img colour picture @return detected colour in referenced array */ /***********************************************************************/ void set_goal_colour(colimage img) { GoalColour = set_colour(img); drive_to_blue_goal = GoalColour > 150; } /***********************************************************************/ /** Compare colours. Check if 2 colour-values are alike (difference < thresh). Called by find_object(). @see find_object() @param val1, val2 colours to compare @param thresh threshold for comparison @return TRUE or FLASE, depending on comparison of colour values */ /***********************************************************************/ int alike(int val1, int val2, int thresh) { int distance; if (val1 != NO_HUE && val2!= NO_HUE) { //val1 and val2 can have values between 0..252 //we have 253 hues in total and 126 is the middle //we need to calculate the 'distance' between //these two hues distance = abs(val1-val2); //if the distance is larger than 126 than the other //way round is shorter if (distance > 126) distance = 253 - distance; if (distance <= thresh) return TRUE; } return FALSE; } /***********************************************************************/ /** Mark object. Convert colour picture into greyscale (for LCD output), mark ball/goal position by drawing a vertical and horizontal white line through it. Ball and it's size is displayed by black lines. Called by find_object(). @see find_object() @param x_middle, y_middle pixel-coordinates of object @param object_size width of marking */ /***********************************************************************/ void mark_object(image greyimg, int x_middle, int y_middle, int object_size) { int row, column; /* mark object position: WHITE */ for (row = 1; row < imagerows; row ++) greyimg[row][y_middle] = WHITE; for (column = 1; column < imagecolumns; column ++) greyimg[x_middle][column] = WHITE; /* mark object size: BLACK */ for (row = x_middle - 2; row < x_middle + 3; row ++) { greyimg[row][y_middle - object_size/2] = BLACK; greyimg[row][y_middle + object_size/2] = BLACK; } for (column = y_middle - object_size / 2 + 1; column < y_middle + object_size / 2; column ++) greyimg[x_middle][column] = BLACK; } /***********************************************************************/ /** Search for ball/goal. Search for reagions in rows, who's mean colour value fits the colour of the ball/goal, return coordinates of its middle. Called by find_ball() and find_goal(). @see find_ball() @see find_goal() @param *x_middle, *y_middle pointers to pixel-coordinates of middle of ball/goal @param *object_size pointer to ball's/goal's size @param size minimum size of ball/goal @param colour colour of searched object (ball/goal) @param thresh threshold for colour-comparison @return TRUE or FALSE if ball/goal was found or not @return changed ball/goal coordinates and size in referenced variables */ /***********************************************************************/ int find_object(int *x_middle, int *y_middle, int *object_size, float bottom_size, int colour, int thresh) { colimage img; image greyimg, hue; int row, column, last_row = 0; int must_size; /* desired size of object depending on position on screen */ int first, last; /* first and last pixel of searched colour */ int know_first = FALSE; int know_last = FALSE; /* flags to indicate whether new object coloured pixel on right/left side has been searched yet */ int new_first = 0; int new_last = 0; /* new test area/test values */ int sum; /* sum of pixel values in examined area */ int count; /* no.of pixels with valid hue */ int mean; /* mean of pixel values in examined area */ int first_sum = 0; int last_sum = 0; /* sum of pixel values in new examined areas */ int is_ball; /* flag to determine whether detected object is ball/goal (not too big) */ CAMGetColFrame(&img, FALSE); HSVhue((BYTE *)img, (BYTE *) hue); /* converts whole image - CAN BE OPTIMIZED!! */ for (row = imagerows-1; row > last_row; row --) { if (yfact[row] != 0.0) { must_size = (int) (bottom_size / yfact[row]); if (must_size > imagecolumns - 2) must_size = imagecolumns - 2; } else must_size = imagecolumns - 2; /* size of ball/goal, depending on distance of object ball -> must_size = radius */ if (must_size < 5) { if (!I_am_goalie()) must_size = 5; else must_size = 3; } first = 1; /* first: index of first object coloured pixel in row */ last = imagecolumns - 1; /* last: index of first non object coloured pixel in row after series of object coloured ones */ while (!alike(hue[row][first], colour, thresh) && (first < (last - must_size / 2))) first += must_size / 2; if (alike(hue[row][first], colour, thresh) && (first < (last - must_size / 2))) { first -= must_size / 2; while (!alike(hue[row][first], colour, thresh)) first ++; /* go back to first pixel of object colour */ } while (!alike(hue[row][last - 1], colour, thresh) && (last > (first + must_size / 2))) last -= must_size / 2; if (alike(hue[row][first], colour, thresh) && (last > (first + must_size / 2))) { last += must_size / 2; while (!alike(hue[row][last], colour, thresh)) last --; /* go back to last pixel of object colour */ } if ((last - first) >= must_size) /* object big enough? */ { know_first = FALSE; /* reset flags */ know_last = FALSE; sum = 0; /* reset sum */ count = 0; for (column=first;column < last;column ++) /* check region from first to last */ if (hue[row][column] != NO_HUE) { sum += abs(hue[row][column] - BallColour); count ++; } } while((last - first) >= (must_size)) /* object big enough? */ { /* get mean value for region between columns "first" and "last" */ mean = sum / count; /* test if there is the desired object between columns "first" and "last" */ if (mean < thresh) { *object_size = last - first; *y_middle = first + *object_size / 2; *x_middle = row; is_ball = *object_size < 4 * must_size; /* object shoulden't be too big (might be goal instead of ball) */ if (!competition_mode) { IPColor2Grey(&img, &greyimg); if (is_ball) mark_object(greyimg, *x_middle, *y_middle, *object_size); LCDPutGraphic(&greyimg); } return is_ball; } /* ----- find new region to test ----- */ if (!know_first) { /* find next pixel of searched colour (from left) */ first_sum = 0; column = first; do { if (hue[row][column] != NO_HUE) first_sum += abs(hue[row][column] - BallColour); column ++; } while (!alike(hue[row][column], colour, thresh) && (column <= last)); new_first = column; know_first = TRUE; } if (!know_last) { /* find next pixel of searched colour (from right) */ last_sum = 0; column = last; do { if (hue[row][column] != NO_HUE) last_sum += abs(hue[row][column - 1] - BallColour); column --; } while (!alike(hue[row][column - 1], colour, thresh) && (column >= first)); new_last = column; know_last = TRUE; } if ((new_first - first) > (last - new_last)) /* new possible object region */ { first = new_first; sum -= first_sum; know_first = FALSE; } else { last = new_last; sum -= last_sum; know_last = FALSE; } } } if (!competition_mode) { IPColor2Grey(&img, &greyimg); LCDPutGraphic(&greyimg); } return FALSE; /* searched whole picture */ } /***********************************************************************/ /** Search for ball. Called by PPball_test(). @see PPball_test() @param *x, *y pointers to pixel-coordinates of middle of ball @param *size pointer to ball size @return TRUE or FALSE if ball was found or not @return changed ball coordinates and size in referenced variables */ /***********************************************************************/ int find_ball(int *x, int *y, int *size) { return find_object(x, y, size, BALLSIZE, BallColour, thresh_ball); } /***********************************************************************/ /** Search for ball. Called by drive_to_goal(). @see drive_to_goal() @param *x, *y pointers to pixel-coordinates of middle of goal @param *size pointer to goal size @return TRUE or FALSE if goal was found or not @return changed goal coordinates and size in referenced variables */ /***********************************************************************/ int find_goal(int *x, int *y, int *size) { return find_object(x, y, size, 0.6 * GOALWIDTH, GoalColour, thresh_goal); } /***********************************************************************/ /** Get ball coordinates. Get position of ball on field as found out in image processing routines and orientation of ball towards goal. Called by PPdrive_field() and PPdrive_goal(). @see PPdrive_field() @see PPdrive_goal() @param *ball pointer to array containing coordinates of ball @return ball position and orientation towards goal in referenced parameter */ /***********************************************************************/ void get_ball_coord(PositionType *ball) { ball->x = ball_x; ball->y = ball_y; ball->phi = -atan2(ball_y, LENGTH - ball_x); /* neg: angle ball to goal */ } /***********************************************************************/ /** Get ball colour. Get current values for ball colour. Called by main(). @see main() @param colour array containing colour values for ball @return current colour in referenced parameter */ /***********************************************************************/ int get_ball_col() { return BallColour; } /***********************************************************************/ /** Get goal colour. Get current values for goal colour. Called by main(). @see main() @param colour array containing colour values for goal @return current colour in referenced parameter */ /***********************************************************************/ int get_goal_col(int *blue_goal) { *blue_goal = drive_to_blue_goal; return GoalColour; } /*@}*/