def avg_bg_frames(self, frames, load_from_mem=False):
if load_from_mem:
self._ba = BackgroundAveraging(frames[i])
self._ba.load_bg_model()
return;
for i in xrange(len(frames)):
frames[i] = self._calibrate.undist_fisheye(frames[i])
frames[i] = self._calibrate.crop_frame(frames[i])
frames[i] = self._calibrate.undist_perspective(frames[i], corners_from_mem=self._corners_from_mem)
if i == 0: self._ba = BackgroundAveraging(frames[i])
self._ba.accumulate_background(frames[i])
self._ba.create_models_from_stats()
class Vision():
def __init__(self, bg, calibrate, config, fix_distortions=True, main_pitch=False, corners_from_mem=False):
self._bg = bg
self._corners_from_mem = corners_from_mem
self._frame = None
self._frme_hsv = None
self._foreground = None
self._annotated_mask = None
self._frame_dim = None
self._im_threshed_red = None
self._im_threshed_yellow = None
self._im_threshed_blue = None
self._im_threshed_green = None
self._im_threshed_black = None
self._ba = None
self._calibrate = calibrate
self._fix_distortions = fix_distortions
self._main_pitch = main_pitch
# Load colours default threshold values.
try:
colours = config['colours']
(self._red_colour_min_hsv, self._red_colour_max_hsv, self._red_colour_inv_hsv) = self.__load_colour('red', colours)
(self._yellow_colour_min_hsv, self._yellow_colour_max_hsv, self._yellow_colour_inv_hsv) = self.__load_colour('yellow', colours)
(self._blue_colour_min_hsv, self._blue_colour_max_hsv, self._blue_colour_inv_hsv) = self.__load_colour('blue', colours)
(self._black_colour_min_hsv, self._black_colour_max_hsv, self._black_dot_colour_inv_hsv) = self.__load_colour('black', colours)
(self._green_colour_min_hsv, self._green_colour_max_hsv, self._green_colour_inv_hsv) = self.__load_colour('green', colours)
except:
print 'incomlete config hashmap'
exit()
try:
# TODO Add AngleStabilizer thresholds.
self._robot_area_threshold = config['physical_thresholds']['robot_area']
self._black_dot_area_threshold = config['physical_thresholds']['black_dot_area']
self._black_dot_dist_threshold = config['physical_thresholds']['black_dot_dist']
self._ball_radius_threshold = config['physical_thresholds']['ball_radius']
self._ball_area_threshold = config['physical_thresholds']['ball_area']
self._plate_area_threshold = config['physical_thresholds']['plate_area']
self._blob_area_threshold = config['physical_thresholds']['blob_area']
self._pnp_dist_thresh = config['physical_thresholds']['pnp_dist']
self._cc_area_thresh = config['physical_thresholds']['cc_area']
self._max_foreground_obj_thresh = config['physical_thresholds']['max_foreground_obj']
(self._offset_top, self._offset_bottom, self._offset_left, self._offset_right) = config['physical_thresholds']['offsets']
except:
print 'incomlete config hashmap'
exit()
self._abs_diff_thresh = 30 if self._main_pitch else 20
self._ball_centre = None
self._ball_radius = None
# consider initialising angle depending on starting location.
self._yellow_robot_centre = None
self._yellow_robot_black_dot = None
self._yellow_robot_t_contour = None
self._yellow_robot_as = AngleStabilizer()
self._blue_robot_centre = None
self._blue_robot_black_dot = None
self._blue_robot_t_contour = None
self._blue_robot_as = AngleStabilizer()
self._CVX_WHITE = cv.Scalar(255,255,255)
def __load_colour(self, colour_name, colours):
colour = colours.get(colour_name, None)
if colour:
return (cv.Scalar(*colour['min_hsv']), cv.Scalar(*colour['max_hsv']), colour['inv_hsv'])
def avg_bg_frames(self, frames, load_from_mem=False):
if load_from_mem:
self._ba = BackgroundAveraging(frames[i])
self._ba.load_bg_model()
return;
for i in xrange(len(frames)):
frames[i] = self._calibrate.undist_fisheye(frames[i])
frames[i] = self._calibrate.crop_frame(frames[i])
frames[i] = self._calibrate.undist_perspective(frames[i], corners_from_mem=self._corners_from_mem)
if i == 0: self._ba = BackgroundAveraging(frames[i])
self._ba.accumulate_background(frames[i])
self._ba.create_models_from_stats()
def update(self, frame):
self._frame = frame
self._foreground = None
self._foreground_connected = None
self._annotated_mask = None
self._im_threshed_red = None
self._im_threshed_yellow = None
self._im_threshed_blue = None
self._im_threshed_green = None
self._im_threshed_black = None
self._prev_ball_centre = self._ball_centre
self._ball_centre = None
self._ball_radius = None
self._ball_roundness_metric = None
self._prev_yellow_robot_centre = self._yellow_robot_centre
self._yellow_robot_centre = None
self._prev_yellow_robot_black_dot = self._yellow_robot_black_dot
self._yellow_robot_black_dot = None
self._yellow_robot_t_contour = None
self._yellow_robot_t_area = None
self._yellow_plate_area = None
self._prev_blue_robot_centre = self._blue_robot_centre
self._blue_robot_centre = None
self._prev_blue_robot_black_dot = self._blue_robot_black_dot
self._blue_robot_black_dot = None
self._blue_robot_t_contour = None
self._blue_robot_t_area = None
self._blue_plate_area = None
if self._fix_distortions:
self._frame = self._calibrate.undist_fisheye(self._frame)
self._frame = self._calibrate.crop_frame(self._frame)
self._frame = self._calibrate.undist_perspective(self._frame, corners_from_mem=self._corners_from_mem)
self._frame_dim = cv.GetSize(self._frame)
self._annotated_mask = cv.CreateImage(self._frame_dim, 8, 3)
cv.XorS(self._annotated_mask, self._CVX_WHITE, self._annotated_mask)
self._frame_hsv = cv.CreateImage(self._frame_dim,8,3)
cv.CvtColor(self._frame, self._frame_hsv, cv.CV_BGR2HSV)
self.find_ball(self._frame_hsv)
cont = self.retrieve_foreground_contours()
self.classify_contours(cont)
def get_frame(self, name='source', annotated=True):
"""
name [optional] - the image name. can be chosen from:
bg, source, foreground
"""
if name == 'bg':
output_image = cv.CreateImage(self._frame_dim, 8, 3)
if annotated:
output_image = cv.CloneImage(self._bg)
self.draw_circle(output_image, self._ball_centre, self._ball_radius, colour=cv.CV_RGB(0,255,0))
self.draw_plate(output_image, self._blue_robot_t_contour, self._blue_robot_black_dot, self._blue_robot_centre, colour=cv.CV_RGB(0,0,255))
self.draw_plate(output_image, self._yellow_robot_t_contour, self._yellow_robot_black_dot, self._yellow_robot_centre, colour=cv.CV_RGB(0,255,0))
else: output_image = self._bg
return output_image
elif name == 'source':
output_image = cv.CreateImage(self._frame_dim, 8, 3)
if annotated:
cv.Line(self._frame, (self._offset_left,0), (self._offset_left, int(self._frame_dim[1])), cv.CV_RGB(0,255,255), 1)
cv.Line(self._frame, (int(self._frame_dim[0])-self._offset_right,0), (int(self._frame_dim[0])-self._offset_right, int(self._frame_dim[1])), cv.CV_RGB(0,255,255), 1)
cv.Line(self._frame, (0,self._offset_top), (int(self._frame_dim[0]), self._offset_top), cv.CV_RGB(0,255,255), 1)
cv.Line(self._frame, (0,int(self._frame_dim[1])-self._offset_bottom), (int(self._frame_dim[0]), int(self._frame_dim[1])-self._offset_bottom), cv.CV_RGB(0,255,255), 1)
#if self._ball_centre:
# dcentre = self.correct_height_dist(self._ball_centre[0], self._ball_centre[1])
# cv.Line(self._frame, (cv.Round(dcentre[0]), cv.Round(dcentre[1])), (cv.Round(dcentre[0]), cv.Round(dcentre[1])), cv.CV_RGB(0,255,255), 9)
#
#if self._blue_robot_centre:
# dcentre = self.correct_height_dist(self._blue_robot_centre[0], self._blue_robot_centre[1])
# cv.Line(self._frame, (cv.Round(dcentre[0]), cv.Round(dcentre[1])), (cv.Round(dcentre[0]), cv.Round(dcentre[1])), cv.CV_RGB(255,0,0), 9)
output_image = cv.CloneImage(self._frame)
self.draw_circle(output_image, self._ball_centre, self._ball_radius, colour=cv.CV_RGB(0,255,0))
self.draw_plate(output_image, self._blue_robot_t_contour, self._blue_robot_black_dot, self._blue_robot_centre, colour=cv.CV_RGB(255,255,0))
self.draw_plate(output_image, self._yellow_robot_t_contour, self._yellow_robot_black_dot, self._yellow_robot_centre, colour=cv.CV_RGB(0,0,255))
else: output_image = self._frame
return output_image
elif name == 'threshed_red': return self._im_threshed_red
elif name == 'threshed_yellow':
im_threshed = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(self._frame_hsv, self._yellow_colour_min_hsv, self._yellow_colour_max_hsv, im_threshed)
return im_threshed
elif name == 'threshed_blue':
im_threshed = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(self._frame_hsv, self._blue_colour_min_hsv, self._blue_colour_max_hsv, im_threshed)
return im_threshed
elif name == 'threshed_black':
im_threshed = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(self._frame_hsv, self._black_colour_min_hsv, self._black_colour_max_hsv, im_threshed)
return im_threshed
elif name == 'threshed_green':
im_threshed = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(self._frame_hsv, self._green_colour_min_hsv, self._green_colour_max_hsv, im_threshed)
return im_threshed
elif name == 'foreground': return self._foreground
elif name == 'foreground_connected': return self._foreground_connected
def correct_height_dist(self, x, y):
(width,height) = (self._frame_dim[0] / 2.0, self._frame_dim[1] / 2.0)
return (width + ((x - width)*self._pnp_dist_thresh), height + ((y - height)*self._pnp_dist_thresh))
def convert_xy(self, xy):
(pitch_width, pitch_height) = (244, 122)
(x, y) = (xy[0], xy[1])
x = (x - self._offset_left) / 1.0 / (self._frame_dim[0] - self._offset_left - self._offset_right) * pitch_width
y = (y - self._offset_top) / 1.0 / (self._frame_dim[1] - self._offset_top - self._offset_bottom) * pitch_height
return (x, pitch_height - y)
def __ball_found(self):
return self._ball_centre is not None and self._ball_radius is not None
def ___robot_found(self, colour='blue'):
if colour == 'blue': return self._blue_robot_centre is not None
elif colour == 'yellow': return self._yellow_robot_centre is not None
def get_ball(self):
"""
The following function returns the current location of the ball.
"""
if self.__ball_found(): return (self.convert_xy(self._ball_centre), self._ball_radius)
else: return None
def get_robot(self, colour='blue'): # (blue | yellow)
"""
Given a robot colour, returns a tuple where the first element is the centre
of the robot while the second element is the robot's angle.
"""
if colour == 'blue' and self.___robot_found(colour='blue'):
return (self.convert_xy(self.correct_height_dist(*self._blue_robot_centre)), self._blue_robot_as.get())
elif colour == 'yellow' and self.___robot_found(colour='yellow'):
return (self.convert_xy(self.correct_height_dist(*self._yellow_robot_centre)), self._yellow_robot_as.get())
else: return (None,None)
def is_round(self, blob, threshold):
"""
The method estimates the perimeter of a given blob and makes an educated guess
on how round is the object.
Since we know that the perimeter of a circle is given by:
Perimeter = 2 * Pi * Radius
And that the Area of a circle is given by:
Area = Pi * Radios^2
Combining the two formulas we get:
Perimeter^2 = 2^2 * Pi^2 * Radios^2
Perimeter^2 = 4 * Pi * Area
1 = (4 * Pi * Area) / Perimeter^2
That said, this formula designed for perfect circle. As we are usually working
with pixelated, noisy, and fragmentary images we would use the last formula
as a metric of how round is an object as opposed to whether it is round or
not.
"""
area = cv.ContourArea(blob)
perimeter = sum([euclidean_distance(blob[i - 1], blob[i]) for i in xrange(1, len(blob))])
metric = 4 * pi * area / (perimeter * perimeter)
if(metric > threshold):
(a, center, radius) = cv.MinEnclosingCircle(blob)
return ((cv.Round(center[0]), cv.Round(center[1])), cv.Round(radius), metric)
else: return None
def retrieve_foreground_contours(self):
if self._ba is None:
foreground = cv.CreateImage(self._frame_dim,8,3)
cv.AbsDiff(self._frame, self._bg, foreground)
foreground_gray = cv.CreateImage(self._frame_dim, 8, 1)
cv.CvtColor(foreground, foreground_gray, cv.CV_RGB2GRAY)
self._foreground = cv.CreateImage(self._frame_dim, 8, 3)
cv.CvtColor(foreground_gray, self._foreground, cv.CV_GRAY2BGR)
# Threshold the image
foreground_bw = cv.CreateImage(self._frame_dim, 8, 1)
cv.Threshold(foreground_gray, foreground_bw, self._abs_diff_thresh, 255, cv.CV_THRESH_BINARY)
# Preform morphology operations in order to clean the thresholded image.
tmp = cv.CreateImage(self._frame_dim, 8, 1)
element = cv.CreateStructuringElementEx(3,3,1,1,cv.CV_SHAPE_RECT)
cv.MorphologyEx(foreground_bw, foreground_bw, tmp, element , cv.CV_MOP_CLOSE, 3)
else:
foreground_bw = cv.CreateImage(self._frame_dim,8,1)
self._ba.background_diff(self._frame, foreground_bw)
self._foreground = cv.CloneImage(foreground_bw)
self._ba.connect_components(foreground_bw, area_threshold=self._cc_area_thresh)
self._foreground_connected = cv.CloneImage(foreground_bw)
# Find the blobs in the thresholded image.
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(foreground_bw, storage, cv.CV_RETR_EXTERNAL, cv.CV_CHAIN_APPROX_SIMPLE, (0,0))
return contours
def classify_contours(self, contours):
found_ball = False
found_blue_robot = False
found_yellow_robot = False
contourList = sorted(list(contour_iterator(contours)), key=lambda blob: cv.ContourArea(blob), reverse=True)
if not contourList: return False
for blob in contourList[:self._max_foreground_obj_thresh]:
if len(blob) >= 6 and (cv.ContourArea(blob) >= self._blob_area_threshold[0] and cv.ContourArea(blob) <= self._blob_area_threshold[1]):
blobHSV = cv.CreateImage(self._frame_dim,8,3)
(a, centre, radius) = cv.MinEnclosingCircle(blob)
self.draw_circle(blobHSV, centre, int(radius*2), colour=self._CVX_WHITE, filled=True)
cv.And(blobHSV, self._frame_hsv, blobHSV)
if not found_yellow_robot:
found_yellow_robot = self.find_robot(blobHSV, colour='yellow')
if not found_blue_robot:
found_blue_robot = self.find_robot(blobHSV, colour='blue')
if not found_yellow_robot:
self._yellow_robot_centre = None
self._yellow_robot_black_dot = None
self._yellow_robot_t_contour = None
if not found_blue_robot:
self._blue_robot_centre = None
self._blue_robot_black_dot = None
self._blue_robot_t_contour = None
def morphologycal_cleaning(self, imbw):
imbw_clean = cv.CreateImage(self._frame_dim,8,1)
tmp = cv.CreateImage(self._frame_dim,8,1)
element = cv.CreateStructuringElementEx(3,3,1,1,cv.CV_SHAPE_ELLIPSE)
cv.MorphologyEx(imbw, imbw_clean, tmp, element , cv.CV_MOP_CLOSE, 2)
cv.MorphologyEx(imbw_clean, imbw_clean, tmp, element , cv.CV_MOP_OPEN, 1)
return imbw_clean
def find_robot(self, imblob_hsv, colour='blue'):
blobThreshed = cv.CreateImage(self._frame_dim,8,1)
if colour == 'blue':
cv.InRangeS(imblob_hsv, self._blue_colour_min_hsv, self._blue_colour_max_hsv, blobThreshed)
blobThreshed02 = self.morphologycal_cleaning(blobThreshed)
if self._blue_colour_inv_hsv: cv.XorS(blobThreshed02, self._CVX_WHITE, blobThreshed02)
self._im_threshed_blue = cv.CloneImage(blobThreshed02)
elif colour == 'yellow':
cv.InRangeS(imblob_hsv, self._yellow_colour_min_hsv, self._yellow_colour_max_hsv, blobThreshed)
blobThreshed02 = self.morphologycal_cleaning(blobThreshed)
if self._yellow_colour_inv_hsv: cv.XorS(blobThreshed02, self._CVX_WHITE, blobThreshed02)
self._im_threshed_yellow = cv.CloneImage(blobThreshed02)
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(blobThreshed02, storage, cv.CV_RETR_LIST, cv.CV_CHAIN_APPROX_SIMPLE, (0,0))
# Return if no robot was found, else sort the blobs by area.
contourListTs = sorted((blob for blob in list(contour_iterator(contours)) if self._robot_area_threshold[1] >= cv.ContourArea(blob) >= self._robot_area_threshold[0]), reverse=True)
if not contourListTs: return False
angle = None
# After we know a 'T' was found we can look for a black circle within the
# same blob.
# TODO change behaviour if black dot wasn't found.
if colour == 'blue':
if self._prev_blue_robot_centre:
self._blue_robot_t_contour = sorted(contourListTs, key=lambda contour: euclidean_distance(self.calc_contour_centre(contour), self._prev_blue_robot_centre))[0]
else: self._blue_robot_t_contour = contourListTs[0]
self._blue_robot_t_area = cv.ContourArea(self._blue_robot_t_contour)
(blue_plate_corners, self._blue_plate_area) = self.find_plate(imblob_hsv)
if blue_plate_corners is not None:
(side_a, side_b, angle) = self.find_plate_orientation(blue_plate_corners, self._blue_robot_t_contour)
cv.Line(self._frame, (cv.Round(side_a[0][0]), cv.Round(side_a[0][1])), (cv.Round(side_a[0][0]), cv.Round(side_a[0][1])), cv.CV_RGB(0,255,0), 4)
cv.Line(self._frame, (cv.Round(side_a[1][0]), cv.Round(side_a[1][1])), (cv.Round(side_a[1][0]), cv.Round(side_a[1][1])), cv.CV_RGB(255,0,0), 4)
cv.Line(self._frame, (cv.Round(side_b[0][0]), cv.Round(side_b[0][1])), (cv.Round(side_b[0][0]), cv.Round(side_b[0][1])), cv.CV_RGB(0,255,0), 4)
cv.Line(self._frame, (cv.Round(side_b[1][0]), cv.Round(side_b[1][1])), (cv.Round(side_b[1][0]), cv.Round(side_b[1][1])), cv.CV_RGB(255,0,0), 4)
self._blue_robot_centre = self.calc_contour_centre(self._blue_robot_t_contour)
black_dot = self.find_black_dot(imblob_hsv, self._blue_robot_centre, 'blue')
if (not black_dot) and (angle is not None): self._blue_robot_as.add(angle, datetime.datetime.now())
return True
elif colour == 'yellow':
if self._prev_yellow_robot_centre:
self._yellow_robot_t_contour = sorted(contourListTs, key=lambda contour: euclidean_distance(self.calc_contour_centre(contour), self._prev_yellow_robot_centre))[0]
else: self._yellow_robot_t_contour = contourListTs[0]
self._yellow_robot_t_area = cv.ContourArea(self._yellow_robot_t_contour)
(yellow_plate_corners, self._yellow_plate_area) = self.find_plate(imblob_hsv)
if yellow_plate_corners is not None:
(side_a, side_b, angle) = self.find_plate_orientation(yellow_plate_corners, self._yellow_robot_t_contour)
cv.Line(self._frame, (cv.Round(side_a[0][0]), cv.Round(side_a[0][1])), (cv.Round(side_a[0][0]), cv.Round(side_a[0][1])), cv.CV_RGB(0,255,0), 4)
cv.Line(self._frame, (cv.Round(side_a[1][0]), cv.Round(side_a[1][1])), (cv.Round(side_a[1][0]), cv.Round(side_a[1][1])), cv.CV_RGB(255,0,0), 4)
cv.Line(self._frame, (cv.Round(side_b[0][0]), cv.Round(side_b[0][1])), (cv.Round(side_b[0][0]), cv.Round(side_b[0][1])), cv.CV_RGB(0,255,0), 4)
cv.Line(self._frame, (cv.Round(side_b[1][0]), cv.Round(side_b[1][1])), (cv.Round(side_b[1][0]), cv.Round(side_b[1][1])), cv.CV_RGB(255,0,0), 4)
self._yellow_robot_centre = self.calc_contour_centre(self._yellow_robot_t_contour)
black_dot = self.find_black_dot(imblob_hsv, self._yellow_robot_centre, 'yellow')
if (not black_dot) and (angle is not None): self._blue_robot_as.add(angle, datetime.datetime.now())
return True
def find_plate_orientation(self, plate_corners, t_contour):
box = cv.MinAreaRect2(t_contour)
points02 = [(cv.Round(pt[0]), cv.Round(pt[1])) for pt in cv.BoxPoints(box)]
front_pt = min([(sum([euclidean_distance(pt1, pt2) for pt2 in points02]), pt1) for pt1 in plate_corners])
plate_corners.remove(front_pt[1])
back_pts = sorted([(euclidean_distance(point, front_pt[1]), point) for point in plate_corners], reverse=True)
side_a = (front_pt[1], back_pts[1][1])
side_b = (back_pts[-1][1], back_pts[0][1])
angle = calc_angle(front_pt[1], back_pts[1][1])
return (side_a, side_b, angle)
def find_plate(self, imblob_hsv):
thresholded = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(imblob_hsv, self._green_colour_min_hsv, self._green_colour_max_hsv, thresholded)
tmp = cv.CreateImage(self._frame_dim,8,1)
element = cv.CreateStructuringElementEx(3,3,1,1,cv.CV_SHAPE_RECT)
cv.MorphologyEx(thresholded, thresholded, tmp, element , cv.CV_MOP_CLOSE, 1)
cv.MorphologyEx(thresholded, thresholded, tmp, element , cv.CV_MOP_OPEN, 1)
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(thresholded, storage, mode=cv.CV_RETR_EXTERNAL, method=cv.CV_CHAIN_APPROX_SIMPLE)
#convexHullList = [cv.ConvexHull2(contour, storage, cv.CV_CLOCKWISE, 1) for contour in contour_iterator(contours)]
#tmp_im = cv.CreateImage(self._frame_dim, 8, 1)
#[cv.DrawContours(tmp_im, convex, self._CVX_WHITE, self._CVX_WHITE, -1, cv.CV_FILLED, 8) for convex in convexHullList]
#del contours
#contours = cv.FindContours(tmp_im, storage, mode=cv.CV_RETR_EXTERNAL, method=cv.CV_CHAIN_APPROX_SIMPLE)
contourList = sorted((blob for blob in list(contour_iterator(contours)) if cv.ContourArea(blob) > self._plate_area_threshold[0] and cv.ContourArea(blob) < self._plate_area_threshold[1]), reverse=True)
if contourList == []: return (None,None)
box = cv.MinAreaRect2(contourList[0])
points = [(cv.Round(pt[0]), cv.Round(pt[1])) for pt in cv.BoxPoints(box)]
return (points, cv.ContourArea(contourList[0]))
def find_black_dot(self, imblob_hsv, robot_centre, colour):
smoothed_img = cv.CreateImage(self._frame_dim,8,3)
cv.Smooth(imblob_hsv, smoothed_img, smoothtype=cv.CV_GAUSSIAN, param1=5)
blobThreshed03 = cv.CreateImage(self._frame_dim,8,1)
cv.InRangeS(smoothed_img, self._black_colour_min_hsv, self._black_colour_max_hsv, blobThreshed03)
blobThreshed04 = self.morphologycal_cleaning(blobThreshed03)
storage = cv.CreateMemStorage(0)
contours = cv.FindContours(blobThreshed04, storage, cv.CV_RETR_LIST, cv.CV_CHAIN_APPROX_SIMPLE, (0,0))
contourListCircles = sorted((blob for blob in list(contour_iterator(contours)) if self._black_dot_area_threshold[1] >= cv.ContourArea(blob) >= self._black_dot_area_threshold[0]))
if not contourListCircles: return False
possible_circles = []
tmp_plate_circle = None
for blob in contourListCircles:
if len(blob) >= 4:
tmp_plate_circle = self.is_round(blob, 0.58)
if tmp_plate_circle is not None:
if colour == 'blue':
if self._black_dot_dist_threshold[1] >= euclidean_distance(tmp_plate_circle[0], self._blue_robot_centre) >= self._black_dot_dist_threshold[0]:
possible_circles.append(tmp_plate_circle)
elif colour == 'yellow':
if self._black_dot_dist_threshold[1] >= euclidean_distance(tmp_plate_circle[0], self._yellow_robot_centre) >= self._black_dot_dist_threshold[0]:
possible_circles.append(tmp_plate_circle)
if possible_circles != []:
if colour == 'blue':
if self._prev_blue_robot_black_dot is not None:
possible_circles = sorted(possible_circles, key=lambda circle: euclidean_distance(circle[0], self._prev_blue_robot_black_dot[0]))
if possible_circles == []: return False
self._blue_robot_black_dot = possible_circles[0]
if euclidean_distance(self._blue_robot_black_dot[0], self._prev_blue_robot_black_dot[0]) <= 50.0:
self._blue_robot_as.add(calc_angle(self._blue_robot_black_dot[0], self._blue_robot_centre), datetime.datetime.now())
else:
self._blue_robot_black_dot = self.best_circle(possible_circles)
self._blue_robot_as.add(calc_angle(self._blue_robot_black_dot[0], self._blue_robot_centre), datetime.datetime.now())
if colour == 'yellow':
if self._prev_yellow_robot_black_dot is not None:
possible_circles = sorted(possible_circles, key=lambda circle: euclidean_distance(circle[0], self._prev_yellow_robot_black_dot[0]))
if possible_circles == []: return False
self._yellow_robot_black_dot = possible_circles[0]
if euclidean_distance(self._yellow_robot_black_dot[0], self._prev_yellow_robot_black_dot[0]) <= 50.0:
self._yellow_robot_as.add(calc_angle(self._yellow_robot_black_dot[0], self._yellow_robot_centre), datetime.datetime.now())
else:
self._yellow_robot_black_dot = self.best_circle(possible_circles)
self._yellow_robot_as.add(calc_angle(self._yellow_robot_black_dot[0], self._yellow_robot_centre), datetime.datetime.now())
return True
else:
if colour == 'blue':
self._blue_robot_black_dot = None
return False
if colour == 'yellow':
self._yellow_robot_black_dot = None
return False
def best_circle(self, circles):
return min((abs(1 - i[2]), (i[0], i[1])) for i in circles)[1]
def calc_contour_centre(self, contour):
"""
Used in order to calculate the location of a robot.
"""
moments = cv.Moments(contour)
return (moments.m10 / moments.m00, moments.m01 / moments.m00)
def find_ball(self, frame_hsv):
imgThreshed = cv.CreateImage(self._frame_dim, 8, 1)
cv.InRangeS(frame_hsv, self._red_colour_min_hsv, self._red_colour_max_hsv, imgThreshed)
smoothedThreshed = self.morphologycal_cleaning(imgThreshed)
#smoothedThreshed = cv.CreateImage(self._frame_dim,8,1)
#cv.Smooth(imgThreshed, smoothedThreshed, smoothtype=cv.CV_GAUSSIAN, param1=5)
self._im_threshed_red = cv.CloneImage(smoothedThreshed)
storage = cv.CreateMemStorage(0)
cont = cv.FindContours(smoothedThreshed, storage, mode=cv.CV_RETR_LIST, method=cv.CV_CHAIN_APPROX_SIMPLE)
possible_balls = []
for blob in contour_iterator(cont):
if len(blob) >= 6:
tmp_ball = self.is_round(blob, 0.6)
if tmp_ball is not None and self._ball_area_threshold[1] >= cv.ContourArea(blob) >= self._ball_area_threshold[0]:
possible_balls.append(tmp_ball)
if possible_balls:
if self._prev_ball_centre: (self._ball_centre, self._ball_radius, self._ball_roundness_metric) = sorted(possible_balls, key=lambda ball: euclidean_distance(ball[0], self._prev_ball_centre))[0]
else: (self._ball_centre, self._ball_radius, self._ball_roundness_metric) = sorted(possible_balls, key=lambda ball: ball[2], reverse=True)[0]
return True
else:
(self._ball_centre, self._ball_radius) = (None,None)
return False
def round_coors(self, coors):
return (cv.Round(coors[0]), cv.Round(coors[1]))
def draw_plate(self, im, t_contour, black_dot, centre=None, colour=cv.CV_RGB(0,0,255)):
if t_contour: cv.DrawContours(im, t_contour, colour, colour, 0, 1, 8)
if black_dot:
cv.Line(im, self.round_coors(black_dot[0]), self.round_coors(centre), cv.CV_RGB(255,0,0))
self.draw_circle(im, black_dot[0], black_dot[1], colour)
def draw_circle(self, im, centre, radius, colour=cv.CV_RGB(255,255,0), filled=False):
if centre is not None and radius is not None:
cv.Circle(im, self.round_coors(centre), radius, colour, cv.CV_FILLED if filled else 1)
def set_hsv_values(self, h, s, v, colour, minmax):
setattr(self, "_%s_colour_%s_hsv" % (colour, minmax), cv.Scalar(h,s,v))
def get_hsv_values(self, colour, minmax):
return tuple(getattr(self, "_%s_colour_%s_hsv" % (colour, minmax)))
def set_threshold_values(self, value, name):
setattr(self, "_%s_threshold" % (name,), value)
def get_threshold_values(self, name):
return getattr(self, "_%s_threshold" % (name,))
def get_vision_values(self, name):
valid_names = [
'prev_ball_centre','ball_centre','ball_radius','ball_roundness_metric',
'prev_yellow_robot_centre','yellow_robot_centre','prev_yellow_robot_black_dot','yellow_robot_black_dot','yellow_robot_t_area',
'prev_blue_robot_centre','blue_robot_centre','prev_blue_robot_black_dot','blue_robot_black_dot','blue_robot_t_area',
'plate_area',
'offset_top', 'offset_bottom', 'offset_left', 'offset_right'
]
if name in valid_names: return getattr(self, "_"+name)
else: return None
