def draw_target(self, frame, track_box):
if track_box:
self.tracker_center_x = float(min(frame.width, max(0, track_box[0][0] - track_box[1][0] / 2)) + \
track_box[1][0] / 2)
self.tracker_center_y = float(min(frame.height, max(0, track_box[0][1] - track_box[1][1] / 2)) + \
track_box[1][1] / 2)
if not math.isnan(self.tracker_center_x) and not math.isnan(
self.tracker_center_y):
tracker_center_x = int(self.tracker_center_x)
tracker_center_y = int(self.tracker_center_y)
cv.Circle(frame, (tracker_center_x, tracker_center_y), 5,
CENTROID_COLOR, 1)
cv.Circle(frame, (self.target_x, self.target_y), 5, CENTROID_COLOR, 2)
target_ellipse = ((self.target_x, self.target_y), (75, 75), 0.0)
cv.EllipseBox(frame, target_ellipse, cv.CV_RGB(*TARGET_ELLIPSE_COLOR),
5, cv.CV_AA, 0)
cv.Circle(frame, (self.base_center_x, self.base_center_y), 5,
CENTROID_COLOR, 2)
base_ellipse = ((self.base_center_x, self.base_center_y), (75, 75),
0.0)
cv.EllipseBox(frame, base_ellipse, cv.CV_RGB(*BASE_COLOR), 5, cv.CV_AA,
0)
cv.Circle(frame, (self.obst_center_x, self.obst_center_y), 5,
CENTROID_COLOR, 2)
obst_ellipse = ((self.obst_center_x, self.obst_center_y), (75, 75),
0.0)
cv.EllipseBox(frame, obst_ellipse, cv.CV_RGB(*OBST_COLOR), 5, cv.CV_AA,
0)
def run(self):
hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0, 180)], 1)
HOST, PORT = 'localhost', 5000
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((HOST, PORT))
while not self.quit:
frame = cv.QueryFrame(self.capture)
track_box = None
self.update_hue(frame)
# Compute back projection
backproject = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.CalcArrBackProject([self.hue], backproject, hist)
if self.track_window and is_rect_nonzero(self.track_window):
camshift = cv.CamShift(backproject, self.track_window,
STOP_CRITERIA)
(iters, (area, value, rect), track_box) = camshift
self.track_window = rect
if self.drag_start and is_rect_nonzero(self.selection):
self.draw_mouse_drag_area(frame)
self.recompute_histogram(hist)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox(frame, track_box, cv.CV_RGB(0, 255, 255), 3,
cv.CV_AA, 0)
if track_box:
self.update_message(track_box)
sock.send(json.dumps(self.message) + "\n")
self.draw_target(frame, track_box)
self.update_windows(frame, backproject, hist)
self.handle_keyboard_input()
track_box = None
def run(self):
while True:
frame = cv.QueryFrame( self.capture)
# Run the cam-shift
if self.track_window and is_rect_nonzero(self.track_window):
crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
# If mouse is pressed, highlight the current selected rectangle
# and recompute the histogram
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(frame, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(frame, frame, 0.5)
cv.Copy(save, sub)
x,y,w,h = self.selection
cv.Rectangle(frame, (x,y), (x+w,y+h), (255,255,255))
sel = cv.GetSubRect(self.hue, self.selection )
cv.CalcArrHist( [sel], hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue( hist)
if max_val != 0:
cv.ConvertScale(hist.bins, hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox( frame, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 )
cv.ShowImage("Output",frame)
cv.WaitKey(0)
def run(self):
hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0, 180)], 1)
backproject_mode = False
while True:
frame = cv.QueryFrame(self.capture)
# Convert to HSV and keep the hue
hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
self.hue = cv.CreateImage(cv.GetSize(frame), 8, 1)
print(self.hue)
cv.Split(hsv, self.hue, None, None, None)
# Compute back projection
backproject = cv.CreateImage(cv.GetSize(frame), 8, 1)
# Run the cam-shift
cv.CalcArrBackProject([self.hue], backproject, hist)
if self.track_window and is_rect_nonzero(self.track_window):
crit = (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect),
track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
# If mouse is pressed, highlight the current selected rectangle
# and recompute the histogram
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(frame, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(frame, frame, 0.5)
cv.Copy(save, sub)
x, y, w, h = self.selection
cv.Rectangle(frame, (x, y), (x + w, y + h), (255, 255, 255))
sel = cv.GetSubRect(self.hue, self.selection)
cv.CalcArrHist([sel], hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue(hist)
if max_val != 0:
cv.ConvertScale(hist.bins, hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox(frame, track_box, cv.CV_RGB(255, 0, 0), 3,
cv.CV_AA, 0)
if not backproject_mode:
#frame=cv.Flip(frame)
cv.ShowImage("CamShiftDemo", frame)
else:
cv.ShowImage("CamShiftDemo", backproject)
cv.ShowImage("Histogram", self.hue_histogram_as_image(hist))
c = cv.WaitKey(7)
if c == 27:
break
elif c == ord("b"):
backproject_mode = not backproject_mode
def add_features(self, cv_image, face):
""" Look for any new features around the current feature cloud """
""" Create the ROI mask"""
roi = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
""" Begin with all black pixels """
cv.Zero(roi)
""" Get the coordinates and dimensions of the current track box """
try:
((x, y), (w, h), a) = face.track_box
except:
logger.info("Track box has shrunk to zero...")
return
""" Expand the track box to look for new features """
w = int(face.expand_roi * w)
h = int(face.expand_roi * h)
roi_box = ((x, y), (w, h), a)
""" Create a filled white ellipse within the track_box to define the ROI. """
cv.EllipseBox(roi, roi_box, cv.CV_RGB(255, 255, 255), cv.CV_FILLED)
""" Create the temporary scratchpad images """
eig = cv.CreateImage(cv.GetSize(self.grey), 32, 1)
temp = cv.CreateImage(cv.GetSize(self.grey), 32, 1)
if self.feature_type == 0:
""" Get the new features using Good Features to Track """
features = cv.GoodFeaturesToTrack(self.grey,
eig,
temp,
self.max_count,
self.quality,
self.good_feature_distance,
mask=roi,
blockSize=3,
useHarris=0,
k=0.04)
elif self.feature_type == 1:
""" Get the new features using SURF """
features = []
(surf_features, descriptors) = cv.ExtractSURF(
self.grey, roi, cv.CreateMemStorage(0),
(0, self.surf_hessian_quality, 3, 1))
for feature in surf_features:
features.append(feature[0])
""" Append new features to the current list if they are not too
far from the current cluster """
for new_feature in features:
try:
distance = self.distance_to_cluster(new_feature, face.features)
if distance > self.add_feature_distance:
face.features.append(new_feature)
except:
pass
""" Remove duplicate features """
face.features = list(set(face.features))
def do_camshift(self, cv_image):
""" Get the image size """
image_size = cv.GetSize(cv_image)
image_width = image_size[0]
image_height = image_size[1]
""" Convert to HSV and keep the hue """
hsv = cv.CreateImage(image_size, 8, 3)
cv.CvtColor(cv_image, hsv, cv.CV_BGR2HSV)
self.hue = cv.CreateImage(image_size, 8, 1)
cv.Split(hsv, self.hue, None, None, None)
""" Compute back projection """
backproject = cv.CreateImage(image_size, 8, 1)
""" Run the cam-shift algorithm """
cv.CalcArrBackProject( [self.hue], backproject, self.hist )
if self.track_window and is_rect_nonzero(self.track_window):
crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
""" If mouse is pressed, highlight the current selected rectangle
and recompute the histogram """
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(cv_image, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(cv_image, cv_image, 0.5)
cv.Copy(save, sub)
x,y,w,h = self.selection
cv.Rectangle(cv_image, (x,y), (x+w,y+h), (255,255,255))
sel = cv.GetSubRect(self.hue, self.selection )
cv.CalcArrHist( [sel], self.hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue(self.hist)
if max_val != 0:
cv.ConvertScale(self.hist.bins, self.hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox( cv_image, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 )
roi = RegionOfInterest()
roi.x_offset = int(min(image_width, max(0, track_box[0][0] - track_box[1][0] / 2)))
roi.y_offset = int(min(image_height, max(0, track_box[0][1] - track_box[1][1] / 2)))
roi.width = int(track_box[1][0])
roi.height = int(track_box[1][1])
self.ROI.publish(roi)
cv.ShowImage("Histogram", self.hue_histogram_as_image(self.hist))
if not self.backproject_mode:
return cv_image
else:
return backproject
def track_lk(self, cv_image, face):
feature_box = None
""" Initialize intermediate images if necessary """
if not face.pyramid:
face.grey = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
face.prev_grey = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
face.pyramid = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
face.prev_pyramid = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
face.features = []
""" Create a grey version of the image """
cv.CvtColor(cv_image, face.grey, cv.CV_BGR2GRAY)
""" Equalize the histogram to reduce lighting effects """
cv.EqualizeHist(face.grey, face.grey)
if face.track_box and face.features != []:
""" We have feature points, so track and display them """
""" Calculate the optical flow """
face.features, status, track_error = cv.CalcOpticalFlowPyrLK(
face.prev_grey, face.grey, face.prev_pyramid, face.pyramid,
face.features, (self.win_size, self.win_size), 3,
(cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 20, 0.01),
self.flags)
""" Keep only high status points """
face.features = [p for (st, p) in zip(status, face.features) if st]
elif face.track_box and self.is_rect_nonzero(face.track_box):
""" Get the initial features to track """
""" Create a mask image to be used to select the tracked points """
mask = cv.CreateImage(cv.GetSize(cv_image), 8, 1)
""" Begin with all black pixels """
cv.Zero(mask)
""" Get the coordinates and dimensions of the track box """
try:
x, y, w, h = face.track_box
except:
return None
if self.auto_face_tracking:
# """ For faces, the detect box tends to extend beyond the actual object so shrink it slightly """
# x = int(0.97 * x)
# y = int(0.97 * y)
# w = int(1 * w)
# h = int(1 * h)
""" Get the center of the track box (type CvRect) so we can create the
equivalent CvBox2D (rotated rectangle) required by EllipseBox below. """
center_x = int(x + w / 2)
center_y = int(y + h / 2)
roi_box = ((center_x, center_y), (w, h), 0)
""" Create a filled white ellipse within the track_box to define the ROI. """
cv.EllipseBox(mask, roi_box, cv.CV_RGB(255, 255, 255),
cv.CV_FILLED)
else:
""" For manually selected regions, just use a rectangle """
pt1 = (x, y)
pt2 = (x + w, y + h)
cv.Rectangle(mask, pt1, pt2, cv.CV_RGB(255, 255, 255),
cv.CV_FILLED)
""" Create the temporary scratchpad images """
eig = cv.CreateImage(cv.GetSize(self.grey), 32, 1)
temp = cv.CreateImage(cv.GetSize(self.grey), 32, 1)
if self.feature_type == 0:
""" Find keypoints to track using Good Features to Track """
face.features = cv.GoodFeaturesToTrack(
face.grey,
eig,
temp,
self.max_count,
self.quality,
self.good_feature_distance,
mask=mask,
blockSize=self.block_size,
useHarris=self.use_harris,
k=0.04)
elif self.feature_type == 1:
""" Get the new features using SURF """
(surf_features, descriptors) = cv.ExtractSURF(
face.grey, mask, cv.CreateMemStorage(0),
(0, self.surf_hessian_quality, 3, 1))
for feature in surf_features:
face.features.append(feature[0])
#
if self.auto_min_features:
""" Since the detect box is larger than the actual face
or desired patch, shrink the number of features by 10% """
face.min_features = int(len(face.features) * 0.9)
face.abs_min_features = int(0.5 * face.min_features)
""" Swapping the images """
face.prev_grey, face.grey = face.grey, face.prev_grey
face.prev_pyramid, face.pyramid = face.pyramid, face.prev_pyramid
""" If we have some features... """
if len(face.features) > 0:
""" The FitEllipse2 function below requires us to convert the feature array
into a CvMat matrix """
try:
self.feature_matrix = cv.CreateMat(1, len(face.features),
cv.CV_32SC2)
except:
pass
""" Draw the points as green circles and add them to the features matrix """
i = 0
for the_point in face.features:
if self.show_features:
cv.Circle(self.marker_image,
(int(the_point[0]), int(the_point[1])), 2,
(0, 255, 0, 0), cv.CV_FILLED, 8, 0)
try:
cv.Set2D(self.feature_matrix, 0, i,
(int(the_point[0]), int(the_point[1])))
except:
pass
i = i + 1
""" Draw the best fit ellipse around the feature points """
if len(face.features) > 6:
feature_box = cv.FitEllipse2(self.feature_matrix)
else:
feature_box = None
""" Publish the ROI for the tracked object """
# try:
# (roi_center, roi_size, roi_angle) = feature_box
# except:
# logger.info("Patch box has shrunk to zeros...")
# feature_box = None
# if feature_box and not self.drag_start and self.is_rect_nonzero(face.track_box):
# self.ROI = RegionOfInterest()
# self.ROI.x_offset = min(self.image_size[0], max(0, int(roi_center[0] - roi_size[0] / 2)))
# self.ROI.y_offset = min(self.image_size[1], max(0, int(roi_center[1] - roi_size[1] / 2)))
# self.ROI.width = min(self.image_size[0], int(roi_size[0]))
# self.ROI.height = min(self.image_size[1], int(roi_size[1]))
# self.pubROI.publish(self.ROI)
if feature_box is not None and len(face.features) > 0:
return feature_box
else:
return None
def OnIdle( self, ):
"""Request refresh of the context whenever idle.
track, get position, update camera, then redraw"""
hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0,180)], 1 )
backproject_mode = False
while True:
frame = cv.QueryFrame(self.capture)
# Convert to HSV and keep the hue
hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
self.hue = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.Split(hsv, self.hue, None, None, None)
# Compute back projection
backproject = cv.CreateImage(cv.GetSize(frame), 8, 1)
# Run the cam-shift
cv.CalcArrBackProject( [self.hue], backproject, hist )
if self.track_window and is_rect_nonzero(self.track_window):
crit = ( cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect), track_box) = cv.CamShift(backproject, self.track_window, crit)
self.track_window = rect
# If mouse is pressed, highlight the current selected rectangle
# and recompute the histogram
if self.drag_start and is_rect_nonzero(self.selection):
sub = cv.GetSubRect(frame, self.selection)
save = cv.CloneMat(sub)
cv.ConvertScale(frame, frame, 0.5)
cv.Copy(save, sub)
x,y,w,h = self.selection
cv.Rectangle(frame, (x,y), (x+w,y+h), (0,0,255))
sel = cv.GetSubRect(self.hue, self.selection )
cv.CalcArrHist( [sel], hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue( hist)
if max_val != 0:
cv.ConvertScale(hist.bins, hist.bins, 255. / max_val)
elif self.track_window and is_rect_nonzero(self.track_window):
cv.EllipseBox(frame, track_box, cv.CV_RGB(255,0,0), 3, cv.CV_AA, 0 )
# find centroid coordinate (x,y) and area (z)
selection_centroid = track_box[0]
global xposition
xposition = selection_centroid[0]
global yposition
yposition = selection_centroid[1]
width_height = track_box[1]
# writes output of coordinates to seed file if needed
# with open('seed.txt', 'a') as f:
# value = (xposition, yposition)
# s = str(value) + '\n'
# f.write(s)
# # f.write('end_of_session')
# f.close()
# print outs
print "x: " + str(xposition)
print "y: " + str(yposition)
selection_area = width_height[0]*width_height[1]
# print "The width is: " + str(width_height[0]) + " The height is: " + str(width_height[1])
# print "centroid is: " + str(selection_centroid)
# return "centroid is: " + str(selection_centroid)
print "area: " + str(selection_area)
# return "area is: " + str(selection_area)
if not backproject_mode:
cv.ShowImage( "CamShiftDemo", frame )
else:
cv.ShowImage( "CamShiftDemo", backproject)
cv.ShowImage( "Histogram", self.hue_histogram_as_image(hist))
c = cv.WaitKey(10)
if c == 27: # escape key
break
elif c == ord("b"): # show backproject mode with "b" key
backproject_mode = not backproject_mode
self.triggerRedraw(1)
return 1
def camshift_track(roi_selection, img):
""" setup_camshift:
- is_rect_nonzero(r)
- hue_histogram_as_image(hist)
- making_selection(roi_selection)
- create_hist()
- placeholder()
compute_camshift_centroid:
- isolate_hue
- compute_back_projection
- recompute_histogram(roi_selection)
- run_camshift
- draw ellipse
- find_centroid(track_box)
- show_hist
"""
# setup_camshift
def is_rect_nonzero(r):
(_, _, w, h) = r
return (w > 0) and (h > 0)
def hue_histogram_as_image(hist):
""" Returns representation of a hue histogram """
histimg_hsv = cv.CreateImage((320, 200), 8, 3)
mybins = cv.CloneMatND(hist.bins)
cv.Log(mybins, mybins)
(_, hi, _, _) = cv.MinMaxLoc(mybins)
cv.ConvertScale(mybins, mybins, 255. / hi)
w, h = cv.GetSize(histimg_hsv)
hdims = cv.GetDims(mybins)[0]
for x in range(w):
xh = (180 * x) / (w - 1) # hue sweeps from 0-180 across the image
val = int(mybins[int(hdims * x / w)] * h / 255)
cv.Rectangle(histimg_hsv, (x, 0), (x, h - val), (xh, 255, 64), -1)
cv.Rectangle(histimg_hsv, (x, h - val), (x, h), (xh, 255, 255), -1)
histimg = cv.CreateImage((320, 200), 8, 3)
cv.CvtColor(histimg_hsv, histimg, cv.CV_HSV2BGR)
return histimg
# making_selection
# print roi_selection[0]
# print roi_selection[1]
point1 = roi_selection[0]
point2 = roi_selection[1]
xmin = point1[0]
ymin = point1[1]
xmax = point2[0]
ymax = point2[1]
widthx = xmax - xmin
heighty = ymax - ymin
selection = (xmin, ymin, widthx, heighty)
# end of making_selection
# create_hist()
hist = cv.CreateHist([180], cv.CV_HIST_ARRAY, [(0, 180)], 1)
# end of create_hist
# placeholder()
backproject_mode = False
# end of placeholder
while True:
# capture = cv.CaptureFromCAM(0)
# frame = cv.QueryFrame(capture)
# isolate_hue
# Convert to HSV and keep the hue
hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
hue = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.Split(hsv, hue, None, None, None)
# end isolate_hue
# Compute back projection
backproject = cv.CreateImage(cv.GetSize(frame), 8, 1)
# end compute back projection
# highlight the current selected rectangle and recompute the histogram
w = xmax - xmin
h = ymax - ymax
xmin, ymin, w, h = selection
cv.Rectangle(frame, (xmin, ymin), (xmax, ymax), (0, 0, 255))
# end highlight
# Run the camshift
cv.CalcArrBackProject([hue], backproject, hist)
# end run camshift
# draw ellipse
print "selection is" + str(selection)
if selection and is_rect_nonzero(selection):
crit = (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 10, 1)
(iters, (area, value, rect),
track_box) = cv.CamShift(backproject, selection, crit)
selection = rect
sel = cv.GetSubRect(hue, selection)
cv.CalcArrHist([sel], hist, 0)
(_, max_val, _, _) = cv.GetMinMaxHistValue(hist)
if max_val != 0:
cv.ConvertScale(hist.bins, hist.bins, 255. / max_val)
elif selection and is_rect_nonzero(selection):
cv.EllipseBox(frame, track_box, cv.CV_RGB(255, 0, 0), 3, cv.CV_AA,
0)
# end draw ellipse
# find centroid coordinate (x,y) and area (z)
selection_centroid = track_box[0]
xposition = selection_centroid[0]
yposition = selection_centroid[1]
width_height = track_box[1]
# selection_area = width_height[0]*width_height[1]
face_centroid_camshift = (xposition, yposition)
# return face_centroid_camshift
# end find centroid
# show hist
if backproject_mode:
# cv.ShowImage( "CamShiftDemo", backproject)
cv.ShowImage("Histogram", self.hue_histogram_as_image(hist))
# end show hist
print "I made it this far"
# print face_centroid_camshift
return face_centroid_camshift
