/***********************************************************************/ /** @name image.c Contains all necessary image processing routines. @author Birgit Graf, UWA, 1998 @author Thomas Braunl, UWA, 1998 (HSV) */ /***********************************************************************/ /*@{*/ #include "global.h" #include "general.h" #include "servos.h" #define NO_HUE 255 /** size at bottom of ball (where it should be detected) */ #define BALLSIZE 0.03 /** Ball coordinates when ball has been detected (position on field) */ float ball_x, ball_y; /** 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 = 70; /** 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 InitCam() { 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; /* int test = 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, delta, max, min; /*int sat, val; */ max = MAX(r, MAX(g,b)); min = MIN(r, MIN(g,b)); delta = max - min; /* 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 i;*/ 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; /*int i;*/ /* 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; 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, know_last; /* flag to indicate whether new object coloured pixel on right/left side has been searched yet */ int new_first, new_last; /* 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, last_sum; /* 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!! */ if (cam_pos == UP) if (I_am_goalie()) last_row = 15; /* others are too far away */ else last_row = 25; /* others are too far away */ else last_row = 5; for (row = imagerows-1; row > last_row; row --) { if (yfact[cam_pos][row] != 0.0) { must_size = (int) (bottom_size / yfact[cam_pos][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); } /***********************************************************************/ /** Transform local to global Position. change local dx, dy from robot position into global coordinates @author Thomas Braunl */ /***********************************************************************/ void local2global(PositionType pos, PositionType local, PositionType *global) { float cosphi, sinphi; if (I_am_goalie()) /* change coordinates for goalie */ { if (cam_pos == UP) { local.x *= 0.7; local.y *= 0.7; } pos.phi += PI / 2.0; } cosphi = cos(pos.phi); sinphi = sin(pos.phi); global->x = pos.x + cosphi*local.x + sinphi*local.y; global->y = pos.y + cosphi*local.y + sinphi*local.x; global->phi = 0.0; } /***********************************************************************/ /** Check for ball. Analyse picture by calling find_object and checking camera position, set see_ball_flag and got_ball_flag respectively. Process started by main(). @see main() */ /***********************************************************************/ void PPball_test() { PositionType pos; int x_middle = 0, y_middle = 0, ball_size = 0; /* parameters of ball */ /*float ball_angle;*/ /* angle to ball */ /* ball coordinates (robot's local/ global coord.syst.) */ PositionType local, global; /* float new_y;*/ /* threshold for moving camera down - to middle */ float thresh_middle_down = 0.06; /* threshold for moving camera to middle - up */ float thresh_up_middle = 0.15; int factor = 0; /* counts pictures without ball */ int frame_counter, new_counter; frame_counter = OSGetCount(); while(!end_flag) { /* LCDPrintf("ball\n"); */ new_counter = OSGetCount(); LCDSetPos(5, 10); /* print frame rate */ if (new_counter != frame_counter) LCDPrintf("f:%1.1f\n", 100.0 / (float)(new_counter - frame_counter)); frame_counter = new_counter; VWGetPosition(vw, &pos); got_ball_flag = FALSE; /* haven't caught it yet */ if (find_ball(&x_middle, &y_middle, &ball_size)) { /* ------ see ball -> set coordinates and see_ball_flag ------ */ local.y = (float)(imagecolumns / 2 - 1 - y_middle) * yfact[cam_pos][x_middle]; local.x = x2m[cam_pos][x_middle]; local.phi = 0.0; local2global(pos, local, &global); /* get global coordinates */ /* check whether ball is in field */ if ((global.x > 0.0) && (global.x < LENGTH) && (fabs(global.y) < WIDTH2)) { see_ball_flag = TRUE; /* really seeing ball -> set flag + coordinates */ ball_x = global.x; ball_y = global.y; factor = 3; /* allow factor pictures without ball before looking elsewhere */ if (local.x < thresh_middle_down) { if (I_am_goalie()) kick_ball(); /* kick it away */ else move_camera(DOWN); if ((local.x < 0.5 * thresh_middle_down) && (fabs(local.y) < thresh_middle_down)) { see_ball_flag = TRUE; /* don't need to drive to ball position */ got_ball_flag = TRUE; } } else if (local.x < thresh_up_middle) move_camera(MIDDLE); else move_camera(UP); } else /* no ball (found object out of field, propably goal) */ { if (cam_pos == DOWN) /* look up for ball */ move_camera(MIDDLE); else if (cam_pos == MIDDLE) { if (I_am_goalie()) kick_ball(); /* make sure ball is not in front of robot - kick it away before looking up */ move_camera(UP); } } } else /* no ball */ { if (factor) /* keep on looking to presumed ball position */ factor --; else { if (cam_pos == DOWN) /* look up for ball */ move_camera(MIDDLE); else if (cam_pos == MIDDLE) { if (I_am_goalie()) kick_ball(); /* make sure ball is not in front of robot - kick it away before looking up */ move_camera(UP); } } } OSReschedule(); } end_flag += 1; OSKill(0); } /***********************************************************************/ /** 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; } /*@}*/