def draw_fft(self, frame, fft_data, min_bpm, max_bpm): w = frame.width h = int(frame.height * Annotator.FFT_HEIGHT) x = 0 y = frame.height max_magnitude = max(d[1][0] for d in fft_data) def get_position(i): point_x = int(w * (float(fft_data[i][0] - min_bpm) / float(max_bpm - min_bpm))) point_y = int(y - ((h * fft_data[i][1][0]) / max_magnitude)) return point_x, point_y line = [get_position(i) for i in range(len(fft_data))] cv.PolyLine(frame, [line], False, self.get_colour()[0], 3) # Label the largest bin max_bin = max(range(len(fft_data)), key=(lambda i: fft_data[i][1][0])) x,y = get_position(max_bin) c = self.get_colour() text = "%0.1f"%fft_data[max_bin][0] cv.PutText(frame, text, (x,y), self.small_font_outline, c[1]) cv.PutText(frame, text, (x,y), self.small_font, c[0]) # Pulse ring r = Annotator.SMALL_PULSE_SIZE phase = int(((fft_data[max_bin][1][1] % (2*numpy.pi)) / numpy.pi) * 180) cv.Ellipse(frame, (int(x-(r*1.5)),int(y-r)), (int(r),int(r)), 0, 90, 90-phase, c[1], Annotator.THIN+Annotator.BORDER) cv.Ellipse(frame, (int(x-(r*1.5)),int(y-r)), (int(r),int(r)), 0, 90, 90-phase, c[0], Annotator.THIN)
def render(self, window):
with self.lock:
if self.image and self.image_time + rospy.Duration(2.0) > rospy.Time.now() and self.info_time + rospy.Duration(2.0) > rospy.Time.now():
cv.Resize(self.bridge.imgmsg_to_cv(self.image, 'rgb8'), window)
interval = min(1,(self.interval / self.max_interval))
cv.Rectangle(window,
(int(0.05*window.width), int(window.height*0.9)),
(int(interval*window.width*0.9+0.05*window.width), int(window.height*0.95)),
(0, interval*255, (1-interval)*255), thickness=-1)
cv.Rectangle(window,
(int(0.05*window.width), int(window.height*0.9)),
(int(window.width*0.9+0.05*window.width), int(window.height*0.95)),
(0, interval*255, (1-interval)*255))
cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * 0.1)), self.font1, (0,0,255))
if self.features and self.features.header.stamp + rospy.Duration(4.0) > self.image.header.stamp:
w_scaling = float (window.width) / self.image.width
h_scaling = float (window.height) / self.image.height
if self.features.success:
corner_color = (0,255,0)
for cur_pt in self.features.image_points:
cv.Circle(window, (int(cur_pt.x*w_scaling), int(cur_pt.y*h_scaling)), int(w_scaling*5), corner_color)
else:
window = add_text(window, ["Could not detect", "checkerboard"], False)
else:
window = add_text(window, ["Timed out waiting", "for checkerboard"], False)
else:
# Generate random white noise (for fun)
noise = numpy.random.rand(window.height, window.width)*256
numpy.asarray(window)[:,:,0] = noise;
numpy.asarray(window)[:,:,1] = noise;
numpy.asarray(window)[:,:,2] = noise;
cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * .95)), self.font, (0,0,255))
def publish_debug(img, results):
imgsize = cv.GetSize(img)
sizelist = [cv.GetSize(tmp[1]) for tmp in templates]
width = max(imgsize[0], sum([s[0] for s in sizelist]))
height = imgsize[1] + max([s[1] for s in sizelist])
output = cv.CreateImage((width, height), cv.IPL_DEPTH_8U, 1)
cv.Zero(output)
cur_x = 0
view_rect = (0, height-imgsize[1], imgsize[0], imgsize[1])
cv.SetImageROI(output, view_rect)
cv.Copy(img, output)
for template in templates:
size = cv.GetSize(template[1])
cv.SetImageROI(output, (cur_x, 0, size[0], size[1]))
cv.Copy(template[1], output)
cur_x += size[0]
#cv.PutText(output, tempname, (0,size[1]-16), font, cv.CV_RGB(255,255,255))
#cv.PutText(output, str(tempthre)+'<'+str(status[1]), (0,size[1]-8), font, cv.CV_RGB(255,255,255))
for _,status in [s for s in results if s[0] == template[3]]:
print status
cv.PutText(output, template[3], (0,size[1]-42), font, cv.CV_RGB(255,255,255))
cv.PutText(output, "%7.5f"%(status[0]), (0,size[1]-24), font, cv.CV_RGB(255,255,255))
cv.PutText(output, "%7.5f"%(status[2]), (0,size[1]-8), font, cv.CV_RGB(255,255,255))
if status[3] :
cv.Rectangle(output, (0, 0), size, cv.RGB(255,255,255), 9)
cv.SetImageROI(output, view_rect)
for _,status in [s for s in results if s[0] == template[3]]:
pt2 = (status[1][0]+size[0], status[1][1]+size[1])
if status[3] :
cv.Rectangle(output, status[1], pt2, cv.RGB(255,255,255), 5)
cv.ResetImageROI(output)
debug_pub.publish(bridge.cv_to_imgmsg(output, encoding="passthrough"))
def setup(flipped, capture, thehandcolor):
"""Initializes camera and finds initial skin tone"""
#creates initial window and prepares text
color = (40, 0, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0)
textsize1 = (cv.GetSize(flipped)[0] / 2 - 150,
cv.GetSize(flipped)[1] / 2 - 140)
textsize2 = (cv.GetSize(flipped)[0] / 2 - 150,
cv.GetSize(flipped)[1] / 2 - 110)
point1 = (cv.GetSize(flipped)[0] / 2 - 25, cv.GetSize(flipped)[1] / 2 - 25)
point2 = (cv.GetSize(flipped)[0] / 2 + 25, cv.GetSize(flipped)[1] / 2 + 25)
#until Enter is pressed
while (cv.WaitKey(10) != 10):
#captures live video, and draws sub-box and text
frame = cv.QueryFrame(capture)
cv.Copy(frame, flipped)
cv.Flip(flipped, flipped, 1)
cv.Rectangle(flipped, point1, point2, color, 2)
cv.PutText(flipped, "Put your hand in the box ", textsize1, font,
color)
cv.PutText(flipped, "and press enter", textsize2, font, color)
cv.ShowImage("w2", flipped)
#Creates sub-image inside box, and returns average color in box
sub = cv.GetSubRect(flipped, (cv.GetSize(flipped)[0] / 2 - 25,
cv.GetSize(flipped)[1] / 2 - 25, 50, 50))
cv.Set(thehandcolor, cv.Avg(sub))
return cv.Avg(sub)
def step(self, show):
r_image = cv.QueryFrame(self.capture)
if Camera.EXTERNAL:
self.image = cv.CreateImage((r_image.height, r_image.width), r_image.depth, r_image.channels)
cv.Transpose(r_image, self.image)
cv.Flip(self.image, self.image, flipMode=0)
else:
self.image = r_image
self.detect_faces()
for (x,y,w,h) in self.faces:
cv.PutText(self.image, "LOOK AT THIS IDIOT", (0, 30), Camera.FONT, Camera.RED)
cv.PutText(self.image, str(time.time()), (self.image.width - 275, self.image.height - 20), Camera.FONT, Camera.RED)
cv.Line(self.image, (0, y+h/2), (self.image.width, y+h/2), Camera.RED)
cv.Line(self.image, (x+w/2, 0), (x+w/2, self.image.height), Camera.RED)
cv.Circle(self.image, (x+w/2, y+h/2), min(h/2, w/2), Camera.RED, thickness=2)
cv.Circle(self.image, (x+w/2, y+h/2), min(h/8, w/8), Camera.RED)
#arrow(self.image, (200, 40), (x, y), Camera.WHITE)
if show:
print self.faces
cv.ShowImage("w1", self.image)
cv.WaitKey(1)
return self.image
def redraw_monocular(self, drawable):
width, height = cv.GetSize(drawable.scrib)
display = cv.CreateMat(max(480, height), width + 100, cv.CV_8UC3)
cv.Zero(display)
cv.Copy(drawable.scrib, cv.GetSubRect(display, (0, 0, width, height)))
cv.Set(cv.GetSubRect(display, (width, 0, 100, height)),
(255, 255, 255))
self.buttons(display)
if not self.c.calibrated:
if drawable.params:
for i, (label, lo, hi, progress) in enumerate(drawable.params):
(w, _), _ = cv.GetTextSize(label, self.font)
cv.PutText(display, label,
(width + (100 - w) / 2, self.y(i)), self.font,
(0, 0, 0))
color = (0, 255, 0)
if progress < 1.0:
color = (0, int(progress * 255.), 255)
cv.Line(display, (int(width + lo * 100), self.y(i) + 20),
(int(width + hi * 100), self.y(i) + 20), color, 4)
else:
cv.PutText(display, "lin.", (width, self.y(0)), self.font,
(0, 0, 0))
linerror = drawable.linear_error
if linerror < 0:
msg = "?"
else:
msg = "%.2f" % linerror
#print "linear", linerror
cv.PutText(display, msg, (width, self.y(1)), self.font, (0, 0, 0))
self.show(display)
def draw_pizza(self):
print "Number of boxes:", len(self.Boxes)
for Box in self.Boxes:
line_color = (random.randint(0, 255), random.randint(0, 255),
random.randint(0, 255))
cv.Line(self.debug_frame, Box.corner1, Box.corner2, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner1, Box.corner3, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner3, Box.corner4, line_color, 10,
cv.CV_AA, 0)
cv.Line(self.debug_frame, Box.corner2, Box.corner4, line_color, 10,
cv.CV_AA, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 2, 1)
cv.PutText(self.debug_frame, str("1"),
(int(Box.corner1[0]), int(Box.corner1[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("2"),
(int(Box.corner2[0]), int(Box.corner2[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("3"),
(int(Box.corner3[0]), int(Box.corner3[1])), font,
(0, 0, 255))
cv.PutText(self.debug_frame, str("4"),
(int(Box.corner4[0]), int(Box.corner4[1])), font,
(0, 0, 255))
center = (int(Box.center[0]), int(Box.center[1]))
cv.Circle(self.debug_frame, center, 15, (255, 0, 0), 2, 8, 0)
def write(self, msg, loc, lineheight=20, color=None, outline=True):
'''Write a string(msg) to the screen. This handles new lines like
butter, and defaults to outlineing the text'''
for i, line in enumerate(msg.splitlines()):
l = (loc[0], loc[1] + i * lineheight)
if outline:
cv.PutText(self.frame, line, l, self.getfont(outline), 0)
cv.PutText(self.frame, line, l, self.getfont(outline), self.color)
def runVideo(self):
frameCount = 0
x = self.xFont
y = self.yFont
font = self.videoFont
tom2Coords = self.tom2Coords
tom1Coords = self.tom1Coords
snareCoords = self.snareCoords
hihatCoords = self.hihatCoords
i = 2
while True:
if i % 2 == 0:
frame = cv.QueryFrame(capture)
self.redStick.drawBoundingCircle(frame)
self.blueStick.drawBoundingCircle(frame)
self.redStick.appendCentersList()
self.blueStick.appendCentersList()
self.redStick.findDelta()
self.blueStick.findDelta()
self.redStick.playSounds()
self.blueStick.playSounds()
cv.Rectangle(frame, (tom1Coords[0], tom1Coords[1]),
(tom1Coords[2], tom1Coords[3]), (255, 0, 0), 0)
cv.Rectangle(frame, (tom2Coords[0], tom2Coords[1]),
(tom2Coords[2], tom2Coords[3]), (0, 255, 0), 0)
cv.Rectangle(frame, (snareCoords[0], snareCoords[1]),
(snareCoords[2], snareCoords[3]), (0, 0, 255), 0)
cv.Rectangle(frame, (hihatCoords[0], hihatCoords[1]),
(hihatCoords[2], hihatCoords[3]), (125, 125, 0),
0)
cv.Flip(frame, frame, 1)
cv.PutText(frame, "Press q or esc to terminate.", (x, y), font,
(0, 255, 0))
cv.PutText(frame, "Current Velocities:", (x, y + 50), font,
(0, 255, 255))
cv.PutText(frame, str(self.redStick.delta), (x, y + 100), font,
(0, 0, 255))
cv.PutText(frame, str(self.blueStick.delta),
(x + 100, y + 100), font, (255, 0, 0))
cv.ShowImage("DrumMaster 9000!", frame)
key = cv.WaitKey(7)
if key == 27 or key == 113:
cv.DestroyAllWindows()
pygame.quit()
exit()
break
frameCount += 1
def sort_bins(self):
# promote candidate to confirmed if seen enough times, if it hasn't been seen, delete the bin
for candidate in self.candidates:
candidate.last_seen -= 1
if candidate.last_seen < self.last_seen_thresh:
self.candidates.remove(candidate)
print "lost"
continue
if candidate.seencount > self.min_seencount:
self.confirmed.append(candidate)
self.candidates.remove(candidate)
print "confirmed"
continue
self.min_perimeter = 500000
self.angles = []
for confirmed in self.confirmed:
if 0 < line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2 < self.min_perimeter:
self.min_perimeter = line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2
# print confirmed.angle/math.pi*180
self.angles.append(cv.Round(confirmed.angle / math.pi * 180 / 10) * 10)
# compare perimeter of existing bins. If a bin is too much bigger than the others, it is deleted. This is done to get rid of bins found based of 3 bins
for confirmed in self.confirmed:
if math.fabs(line_distance(confirmed.corner1, confirmed.corner3) * 2 + math.fabs(line_distance(confirmed.corner1, confirmed.corner2) * 2) - self.min_perimeter) > self.min_perimeter * self.perimeter_threshold and line_distance(confirmed.corner1, confirmed.corner3) * 2 + line_distance(confirmed.corner1, confirmed.corner2) * 2 > self.min_perimeter:
print "perimeter error (this is a good thing)"
print math.fabs(line_distance(confirmed.corner1, confirmed.corner3) * 2 + math.fabs(line_distance(confirmed.corner1, confirmed.corner2) * 2) - self.min_perimeter), "is greater than", self.min_perimeter * self.perimeter_threshold
print "yay?"
confirmed.last_seen -= 5
continue
confirmed.last_seen -= 1
if confirmed.last_seen < self.last_seen_thresh:
self.confirmed.remove(confirmed)
print "lost confirmed"
continue
# draw bins
line_color = (confirmed.corner1[1] / 2, confirmed.corner2[1] / 2, confirmed.corner4[1] / 2)
cv.Circle(self.debug_frame, (int(confirmed.midx), int(confirmed.midy)), 15, line_color, 2, 8, 0)
pt1 = (cv.Round(confirmed.corner1[0]), cv.Round(confirmed.corner1[1]))
pt2 = (cv.Round(confirmed.corner2[0]), cv.Round(confirmed.corner2[1]))
cv.Line(self.debug_frame, pt1, pt2, line_color, 1, cv.CV_AA, 0)
pt2 = (cv.Round(confirmed.corner2[0]), cv.Round(confirmed.corner2[1]))
pt4 = (cv.Round(confirmed.corner4[0]), cv.Round(confirmed.corner4[1]))
pt3 = (cv.Round(confirmed.corner3[0]), cv.Round(confirmed.corner3[1]))
cv.Line(self.debug_frame, pt2, pt4, line_color, 1, cv.CV_AA, 0)
cv.Line(self.debug_frame, pt3, pt4, line_color, 1, cv.CV_AA, 0)
cv.Line(self.debug_frame, pt1, pt3, line_color, 1, cv.CV_AA, 0)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, .6, .6, 0, 1, 1)
text_color = (0, 255, 0)
# print id and last_seen by each bin
cv.PutText(self.debug_frame, str(confirmed.id), (int(confirmed.midx), int(confirmed.midy)), font, confirmed.debug_color)
cv.PutText(self.debug_frame, str(confirmed.last_seen), (int(confirmed.midx - 20), int(confirmed.midy - 20)), font, confirmed.debug_color)
def draw_text_to_image():
""" A function to handle drawing things to the image """
# draw a rectangle under the text to make it more visible
cv.Rectangle(D.image, (0, 0), (100, 50), cv.RGB(255, 255, 255),
cv.CV_FILLED)
# place some text there
cv.PutText(D.image, D.state, (5, 10), D.font, cv.RGB(0, 0, 0))
# and some values
randomness = random.randint(-100, 100)
area = 4242 + randomness
area_string = str(area)
cv.PutText(D.image, area_string, (5, 25), D.font, cv.RGB(0, 0, 0))
def capture_draw():
img = cv.QueryFrame(capture)
# scale your big ole face down to something small
thumb = cv.CreateMat(img.height / SCALE, img.width / SCALE, cv.CV_8UC3)
cv.Resize(img, thumb)
faces = get_face_roi(thumb)
for (x, y, w, h), n in faces:
temp_offset = (x * SCALE, y * SCALE)
cv.SetImageROI(img,
((x) * SCALE, (y) * SCALE, (w) * SCALE, (h) * SCALE))
roi_image = cv.CreateImage(cv.GetSize(img), img.depth, img.nChannels)
cv.Copy(img, roi_image)
cv.ResetImageROI(img)
cv.Rectangle(img, (x * SCALE, y * SCALE),
(x * SCALE + w * SCALE, y * SCALE + h * SCALE),
(255, 0, 0))
cv.PutText(img, 'face', (x * SCALE, y * SCALE), font, (200, 200, 200))
FEATURE_SCALE = (float(roi_image.width) / ROI_TARGET_SIZE[0],
float(roi_image.height) / ROI_TARGET_SIZE[1])
roi_thumb = cv.CreateImage((int(roi_image.width / FEATURE_SCALE[0]),
int(roi_image.height / FEATURE_SCALE[1])),
cv.IPL_DEPTH_8U, 3)
cv.Resize(roi_image, roi_thumb)
features = get_features(roi_thumb)
cv.ShowImage("ROI", roi_image)
for name in features:
if features[name] != None:
for (x1, y1, w1, h1), n1 in features[name]:
cv.SetImageROI(
roi_image,
(x1 * FEATURE_SCALE[0], y1 * FEATURE_SCALE[1],
w1 * FEATURE_SCALE[0], h1 * FEATURE_SCALE[1]))
feature_image = cv.CreateImage(cv.GetSize(roi_image),
roi_image.depth,
roi_image.nChannels)
cv.Copy(roi_image, feature_image)
cv.ResetImageROI(feature_image)
cv.ShowImage(name, feature_image)
cv.PutText(img, name,
(temp_offset[0] + x1 * FEATURE_SCALE[0],
temp_offset[1] + y1 * FEATURE_SCALE[1]), font,
(200, 200, 200))
cv.Rectangle(
img, (temp_offset[0] + x1 * FEATURE_SCALE[0],
temp_offset[1] + y1 * FEATURE_SCALE[1]),
(temp_offset[0] +
(x1 + w1) * FEATURE_SCALE[0], temp_offset[1] +
(y1 + h1) * FEATURE_SCALE[1]), (0, 255, 255))
cv.ShowImage("Whole Image", img)
def process_image(self, msg):
try:
cvim = self.bridge.imgmsg_to_cv(msg, 'bgr8')
except CvBridgeError as err:
rospy.logerr(e)
return
if self.boxes: cv.PolyLine(cvim, self.boxes, True, (0, 0, 255))
if self.selected_obj_id:
cv.FillPoly(cvim, [self.boxes[self.selected_obj_id]],
(0, 0, 255, 0.5))
# display object names
if self.object_centers_msg:
for center, color, category in zip(
self.object_centers_msg.centers,
self.object_centers_msg.color_labels,
self.object_centers_msg.category_labels):
cv.PutText(cvim, color + ' ' + category,
(center.pixel.x, center.pixel.y), self.font,
(0, 0, 255))
# display trajector
if self.trajector_location:
cv.Circle(cvim,
self.trajector_location,
3, (255, 0, 0),
thickness=2,
lineType=cv.CV_AA,
shift=0)
cv.PutText(cvim, 'trajector', self.trajector_location, self.font,
(255, 0, 0))
# display selected landmark
if self.meaning:
lmk = self.meaning.args[0]
p_arr = self.lmk_geo_to_img_geo(lmk)
if isinstance(lmk.representation, PointRepresentation):
cv.Circle(cvim,
tuple(map(int, p_arr[0])),
3, (0, 255, 0),
thickness=2,
lineType=cv.CV_AA,
shift=0)
else:
pl = [[tuple(map(int, l)) for l in p_arr]]
cv.PolyLine(cvim, pl, True, (0, 255, 0), thickness=3)
cv.ShowImage('img', cvim)
cv.WaitKey(10)
def draw_on_image(D):
"""Draws circles on image at points p1 and p2."""
# Draw circles on the points you choose
cv.Circle(D.range, D.p1, 12, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p1, 10, D.black, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p1, 8, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p2, 12, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p2, 10, D.black, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p2, 8, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p3, 12, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p3, 10, D.black, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p3, 8, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p4, 12, D.white, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p4, 10, D.black, thickness=1, lineType=8, shift=0)
cv.Circle(D.range, D.p4, 8, D.white, thickness=1, lineType=8, shift=0)
#Draw box for text display
dx, dy = 5, 5 #Set up rectangle's position within window
lower_left_x, lower_left_y = 20, 42
#Display black border around box
bord_upper_left = (lower_left_x - dx - 3, lower_left_y - dy - 20 - 3)
bord_lower_right = (lower_left_x + dx + 120 + 3,
lower_left_y + dy + 50 + 3)
cv.Rectangle(D.range, bord_upper_left, bord_lower_right, D.black,
cv.CV_FILLED)
#Display white box
rect_upper_left = (lower_left_x - dx, lower_left_y - dy - 20)
rect_lower_right = (lower_left_x + dx + 120, lower_left_y + dy + 50)
cv.Rectangle(D.range, rect_upper_left, rect_lower_right, D.white,
cv.CV_FILLED)
#Calculate average depth
avgDepth = (D.depth1 + D.depth2 + D.depth3 + D.depth4) / 4.0
#Print text in info box
value_string = ("Avg. Depth: %.1f" % avgDepth) # formatted printing
string_lower_left = (lower_left_x, lower_left_y)
cv.PutText(D.range, value_string, string_lower_left, D.font, D.black)
value_string3 = ("Y Angle %.1f" % D.yAngle) # formatted printing
string_lower_left3 = (lower_left_x, lower_left_y + 20)
cv.PutText(D.range, value_string3, string_lower_left3, D.font, D.black)
value_string4 = ("X Angle %.1f" % D.xAngle) # formatted printing
string_lower_left4 = (lower_left_x, lower_left_y + 40)
cv.PutText(D.range, value_string4, string_lower_left4, D.font, D.black)
def DrawText(img, font):
global gp
color = (64,128,255,1)
message = "step = %f" % gp.step
cv.PutText(img,message,(10,600),font,color)
message = "x = %02.4f" % gp.x
cv.PutText(img,message,(10,610),font,color)
message = "y = %02.4f" % gp.y
cv.PutText(img,message,(10,620),font,color)
message = "z = %02.4f" % gp.z
cv.PutText(img,message,(10,630),font,color)
def image_callback(self,data):
"""
Callback for image topic subscription - finds blobs in image.
"""
#t0 = rospy.Time.now()
with self.lock:
blobs_list, blobs_image = self.blobFinder.findBlobs(data, create_image=True)
if len(blobs_list) == 2:
# If two blobs are found computer the distance between them and
# show it on the published image.
x0 = blobs_list[0]['centroid_x']
y0 = blobs_list[0]['centroid_y']
x1 = blobs_list[1]['centroid_x']
y1 = blobs_list[1]['centroid_y']
point_list = [(x0,y0),(x1,y1)]
dist = math.sqrt((x0-x1)**2 + (y0-y1)**2)
message = 'dist = {0:1.1f} px'.format(dist)
cv.PutText(blobs_image,message,(10,25),self.cv_text_font,self.text_color)
## Publish calibration progress image
blobs_rosimage = self.bridge.cv_to_imgmsg(blobs_image,encoding="passthrough")
self.image_pub.publish(blobs_rosimage)
else:
self.image_pub.publish(data)
def display_tracking(img_size, img1, img2, img_flow, homography):
for x in range(0, img_size[0], 100):
for y in range(0, img_size[1], 100):
cv.Circle(img1, (x, y), 3, (0, 255, 0, 0), -1, 8, 0)
point = cv.CreateMat(3, 1, cv.CV_64F)
point[0, 0] = x
point[1, 0] = y
point[2, 0] = 1
newpoint = cv.CreateMat(3, 1, cv.CV_64F)
cv.MatMul(homography, point, newpoint)
cv.Circle(img2, (int(newpoint[0, 0]), int(newpoint[1, 0])), 3,
(0, 255, 0, 0), -1, 8, 0)
cv.Line(img_flow, (x, y),
(int(newpoint[0, 0]), int(newpoint[1, 0])),
cv.CV_RGB(255, 0, 0), 2)
cv.Rectangle(img_flow, (0, 0), (150, 25), cv.CV_RGB(0, 0, 0), thickness=-1)
cv.PutText(
img_flow, "Good?: " + str(isHomographyGood(homography)), (0, 20),
cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.75, 0.75, thickness=2),
cv.CV_RGB(255, 255, 255))
cv.NamedWindow("Image1", 0)
cv.NamedWindow("Image2", 0)
cv.NamedWindow("Flow", 0)
cv.ResizeWindow("Image1", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ResizeWindow("Image2", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ResizeWindow("Flow", int(0.5 * img_size[0]), int(0.5 * img_size[1]))
cv.ShowImage("Image1", img1)
cv.ShowImage("Image2", img2)
cv.ShowImage("Flow", img_flow)
cv.WaitKey(0)
class test_vision_node:
def __init__(self):
rospy.init_node('test_vision_node')
""" Give the OpenCV display window a name. """
self.cv_window_name = "OpenCV Image"
""" Create the window and make it re-sizeable (second parameter = 0) """
cv.NamedWindow(self.cv_window_name, 0)
""" Create the cv_bridge object """
self.bridge = CvBridge()
""" Subscribe to the raw camera image topic """
self.image_sub = rospy.Subscriber("/camera/image_raw", Image,
self.callback)
def callback(self, data):
try:
""" Convert the raw image to OpenCV format """
cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
except CvBridgeError, e:
print e
""" Get the width and height of the image """
(width, height) = cv.GetSize(cv_image)
""" Overlay some text onto the image display """
text_font = cv.InitFont(cv.CV_FONT_HERSHEY_DUPLEX, 2, 2)
cv.PutText(cv_image, "OpenCV Image", (50, height / 2), text_font,
cv.RGB(255, 255, 0))
""" Refresh the image on the screen """
cv.ShowImage(self.cv_window_name, cv_image)
cv.WaitKey(3)
def pick_and_place(self):
print "30 pick_and_place"
n_piece = 0
while True and n_piece < 1:
n_piece += 1
iteration = 0
angle = 0.0
# use Hough circles to find pieces and select one piece
# "go to 19"
next_piece, angle = self.hough_it(n_piece, iteration)
error = 2 * self.piece_tolerance
print
print "Square number ", n_piece
print "==============="
# iterate to find next chess piece
# if hunting to and fro accept error in position
while error > self.piece_tolerance and iteration < 100:
iteration += 1
next_piece, angle, error = self.chess_piece_iterate(
n_piece, iteration, next_piece)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 1)
position = (30, 60)
s = "Tomando Piezas de ajedrez"
cv.PutText(cv.fromarray(self.cv_image), s, position, font,
self.white)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(self.cv_image,
encoding="bgr8")
self.pub.publish(msg)
def log(self, text=None, frame=None):
if self.measureDisplay:
if text is None or text == "reset" or text == "init":
if frame is not None and self.saveSamples:
file = "/tmp/photo" + "-" + datetime.datetime.now(
).strftime("%Y%m%d%H%M%S%f") + "-" + "init" + ".png"
cv.SaveImage(file, frame)
self.measure_init = datetime.datetime.now()
self.measure_next = self.measure_init
print "\nStart getting Samples.."
else:
if text != "end":
measure_now = datetime.datetime.now()
diff = measure_now - self.measure_next
print "\tSample: %s - %s micros" % (text,
str(diff.microseconds))
if frame is not None and self.saveSamples:
file = "/tmp/photo" + "-" + datetime.datetime.now(
).strftime("%Y%m%d%H%M%S%f") + "-" + text + ".png"
cv.SaveImage(file, frame)
self.measure_next = datetime.datetime.now()
else:
measure_now = datetime.datetime.now()
diff = measure_now - self.measure_init
print "Last Sample: %s micros" % (str(diff.microseconds))
if frame is not None and self.saveSamples:
cv.PutText(
frame, "Sample: " + time.strftime(
"%d-%m-%Y %H:%M:%S", time.localtime()) +
"-end", (10, cv.GetSize(frame)[1] - 10),
cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX, .3, .3,
0.0, 1, cv.CV_AA), (255, 255, 255))
cv.SaveImage(file, frame)
def getHsvRange(self):
self.x_co = 0
self.y_co = 0
cv.NamedWindow('camera feed', cv.CV_WINDOW_AUTOSIZE)
capture = cv.CaptureFromCAM(1)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5, 1, 0, 2, 8)
while True:
src = cv.QueryFrame(capture)
# src = cv.LoadImage('2012_automata.jpg')
cv.Smooth(src, src, cv.CV_BLUR, 3)
hsv = cv.CreateImage(cv.GetSize(src), src.depth, 3)
cv.CvtColor(src, hsv, cv.CV_BGR2HSV)
cv.SetMouseCallback("camera feed", self.on_mouse, 0)
s = cv.Get2D(hsv, self.y_co, self.x_co)
# print "H:", s[0], " S:", s[1], " V:", s[2]
cv.PutText(src,
str(s[0]) + "," + str(s[1]) + "," + str(s[2]),
(self.x_co, self.y_co), font, (55, 25, 255))
cv.ShowImage("camera feed", src)
if cv.WaitKey(10) == 27:
return (s[0], s[1], s[2])
break
def main():
while True:
cv.NamedWindow('a_window', cv.CV_WINDOW_AUTOSIZE)
#logfiles = sorted([ f for f in os.listdir(report_dirName) if f.startswith('image')])
#logfiles=GetLatestArchive('image*.jpg')
latest_folder = report_dirName + latest_file(name_start='Z',
name_end='') + '\\'
image = cv.LoadImage(
latest_folder +
latest_file(path=latest_folder, name_start='Z', name_end='.tif'),
cv.CV_LOAD_IMAGE_COLOR) # .jpg images are 4x times smaller
#img = cv2.imread(latest_folder+latest_file(path=latest_folder, name_start='', name_end='.tif'))
#gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
#img2=cv2.equalizeHist(gray)
#cvmat_img2=cv.fromarray(img2)
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 1, 1, 0, 3, 8)
newFrameImage8U = cv.CreateImage((image.width, image.height),
cv.IPL_DEPTH_8U,
3) # optional convert to 8U
cv.ConvertScale(image, newFrameImage8U) # optional
image = newFrameImage8U # optional
cv.PutText(image, "Counter:", (x, y), font, 255) #Draw the text
#cv.PutText(cvmat_img2,"Counter:", (x,y),font, 255)
cv.ShowImage('a_window', image) #Show the image
#cv.Waitkey(10000)
# open the latest xml-file in this folder and get the stage coordinates (x,y,z)
(stage_x, stage_y, stage_z) = return_xyz_coordinates(
latest_folder +
latest_file(path=latest_folder, name_start='', name_end='.xml'))
print 'stage coordinates x,y,z:', stage_x, stage_y, stage_z
if cv.WaitKey(10) == 27:
break
cv.DestroyWindow("a_window")
def draw_ent(self, ent):
if not ent: return
x, y = intPoint(ent.pos)
radius = 30
#cv.Circle(self.image, intPoint((x,y)), 8, color, -1)
o = ent.orientation
D = 30
cv.Circle(self.image, intPoint((x + D * cos(o), y + D * sin(o))), 6,
(200, 200, 200), -1)
# Draw estimated robot boundaries
points = ent.points
for p1, p2 in zip(points[1:] + points[:1], points[:-1] + points[-1:]):
cv.Line(self.image, intPoint(p1), intPoint(p2), (200, 200, 0), 1,
cv.CV_AA)
cv.Circle(self.image, intPoint(p1), 2, (255, 255, 100), -1)
if ent.text is not None:
x, y = (x - 42, y + 42)
for i, line in enumerate(ent.text):
cv.PutText(self.image, line, (x, y + 12 * i), self.Font,
(0, 200, 200))
if ent.target is not None:
cv.Line(self.image, intPoint(ent.pos), intPoint(ent.target),
(180, 0, 180), 1, cv.CV_AA)
def showWindow(message):
print message
print " [press any key to continue]"
font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5)
cv.PutText(image, message, (10, 20), font, cv.RGB(150, 150, 150))
cv.ShowImage('window', image)
cv.WaitKey(10000)
def paint_estimates(self, img):
"paints the camera estimates"
circles = []
if 'cam1' in self._draw_estimates:
cv.Circle(img, self.cam1_estimate, 10, kc.cam1color, -1)
cv.Line(img, self.cam1_estimate, self.cam1center, kc.cam1color, 1, cv.CV_AA)
circles.append({'color':kc.cam1color, 'text':'Camera 1'})
if 'cam2' in self._draw_estimates:
cv.Circle(img, self.cam2_estimate, 10, kc.cam2color, -1)
cv.Line(img, self.cam2_estimate, self.cam2center, kc.cam2color, 1, cv.CV_AA)
circles.append({'color':kc.cam2color, 'text':'Camera 2'})
# cv.Circle(img, self.cam3_estimate, 10, kc.cam3color, -1)
# cv.Line(img, self.cam3_estimate, self.cam3center, kc.cam3color, 1, cv.CV_AA)
if 'kalman' in self._draw_estimates:
cv.Circle(img, self.kalman_estimate, 10, kc.kalmancolor, -1)
circles.append({'color':kc.kalmancolor, 'text':'Kalman'})
if 'average' in self._draw_estimates:
cv.Circle(img, self.average_estimate, 10, kc.averagecolor, -1)
circles.append({'color':kc.averagecolor, 'text':'Average'})
circles.reverse()
for cnt,c in enumerate(circles):
yloc = kc.img_size[1] - 20 - cnt*25
cloc = (200, yloc)
cv.Circle(img, cloc, 10, c['color'], -1)
tloc = (220, yloc+5)
cv.PutText(img, c['text'], tloc, self._font, kc.font_color)
def generate(ident, sp=SQ_SIZE):
size = (Square.GRID_SIZE + 4) * sp
img = cv.CreateImage((size, size), 8, 1)
rim = [(0, 0), (size, 0), (size, size), (0, size)]
border = [(sp, sp), (size - sp, sp), (size - sp, size - sp),
(sp, size - sp)]
inside = [(sp * 2, sp * 2), (sp * 5, sp * 2), (sp * 5, sp * 5),
(sp * 2, sp * 5)]
square = lambda (x, y): [((2 + x) * sp, (2 + y) * sp),
((3 + x) * sp, (2 + y) * sp),
((3 + x) * sp, (3 + y) * sp),
((2 + x) * sp, (3 + y) * sp)]
corners = [square((0, 0))]
#square(size-sp*2,sp*2),
#square(size-sp*2,size-sp*2), square(sp*2,size-sp*2)]
code = []
id = ident
for p in Square.code_points:
if ident % 2 == 0: code.append(square(p))
ident /= 2
cv.FillPoly(img, [rim], (255, 255, 255))
cv.FillPoly(img, [border], (0, 0, 0))
cv.FillPoly(img, [inside], (255, 255, 255))
cv.FillPoly(img, corners + code, (0, 0, 0))
font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX_SMALL, 0.7, 0.7)
cv.PutText(img, "%d" % id, (0, 15), font, (50, 50, 50))
return img
def show_velocity(self, value, valspan, label, y):
cv.PutText(self.image, label+':', (SENSOR_TEXT_X, y), self.font, SENSOR_LABEL_COLOR_BGR)
bar_x1 = SENSOR_BAR_X + SENSOR_BAR_MAX_HEIGHT
bar_y1 = y + SENSOR_BAR_Y_OFFSET
bar_x2 = bar_x1 + int(value / valspan * SENSOR_BAR_MAX_HEIGHT)
bar_y2 = y - SENSOR_BAR_WIDTH + SENSOR_BAR_Y_OFFSET
bar_color = SENSOR_NEGATIVE_COLOR_BGR if value < 0 else SENSOR_POSITIVE_COLOR_BGR
cv.Rectangle(self.image, (bar_x1, bar_y1), (bar_x2, bar_y2), bar_color, cv.CV_FILLED)
def draw_info(self):
cv.PutText(
self.image, "X-Offset: %s Y-Offset: %s Size: %s Stretch: %s" % (
self.xoffset,
self.yoffset,
self.dia,
self.stretch,
), (10, 15), self.cvfont, cv.RGB(255, 255, 255))
def redraw_stereo(self, drawable):
width, height = cv.GetSize(drawable.lscrib)
display = cv.CreateMat(max(480, height), 2 * width + 100, cv.CV_8UC3)
cv.Zero(display)
cv.Copy(drawable.lscrib, cv.GetSubRect(display, (0, 0, width, height)))
cv.Copy(drawable.rscrib,
cv.GetSubRect(display, (width, 0, width, height)))
cv.Set(cv.GetSubRect(display, (2 * width, 0, 100, height)),
(255, 255, 255))
self.buttons(display)
if not self.c.calibrated:
if drawable.params:
for i, (label, lo, hi, progress) in enumerate(drawable.params):
(w, _), _ = cv.GetTextSize(label, self.font)
cv.PutText(display, label,
(2 * width + (100 - w) / 2, self.y(i)),
self.font, (0, 0, 0))
color = (0, 255, 0)
if progress < 1.0:
color = (0, int(progress * 255.), 255)
cv.Line(display,
(int(2 * width + lo * 100), self.y(i) + 20),
(int(2 * width + hi * 100), self.y(i) + 20), color,
4)
else:
cv.PutText(display, "epi.", (2 * width, self.y(0)), self.font,
(0, 0, 0))
if drawable.epierror == -1:
msg = "?"
else:
msg = "%.2f" % drawable.epierror
cv.PutText(display, msg, (2 * width, self.y(1)), self.font,
(0, 0, 0))
# TODO dim is never set anywhere. Supposed to be observed chessboard size?
if drawable.dim != -1:
cv.PutText(display, "dim", (2 * width, self.y(2)), self.font,
(0, 0, 0))
cv.PutText(display, "%.3f" % drawable.dim,
(2 * width, self.y(3)), self.font, (0, 0, 0))
self.show(display)
def draw_on_image(D):
"""Displays an info box and necessary text in the image window."""
#Set up rectangle's position within window
lower_left_x = 20
lower_left_y = 42
dx = 5
dy = 5
#Display border for rectangle
#Border is a black rectangle under white text rectangle
bord_upper_left = (lower_left_x - dx - 3, lower_left_y - dy - 20 - 3)
bord_lower_right = (lower_left_x + dx + 160 + 3,
lower_left_y + dy + 50 + 3)
cv.Rectangle(D.image, bord_upper_left, bord_lower_right, D.black,
cv.CV_FILLED)
#Display white rectangle under text
rect_upper_left = (lower_left_x - dx, lower_left_y - dy - 20)
rect_lower_right = (lower_left_x + dx + 160, lower_left_y + dy + 50)
cv.Rectangle(D.image, rect_upper_left, rect_lower_right, D.white,
cv.CV_FILLED)
# Build Strings
targetA = ("Target Area: %.lf" % D.target_size)
targetCoords = ("Distance: %.lf " % 4)
state = ("Current State: " + R.curState)
# Position strings in a box so they won't overlap
firstLineString = (lower_left_x, lower_left_y)
secondLineString = (lower_left_x, lower_left_y + 20)
thirdLineString = (lower_left_x, lower_left_y + 40)
#desiredHeading = ("Des: %.2f" % desHeadVar)
cv.PutText(D.image, targetA, firstLineString, D.font, cv.RGB(0, 0, 255))
cv.PutText(D.image, targetCoords, secondLineString, D.font,
cv.RGB(0, 0, 255))
cv.PutText(D.image, state, thirdLineString, D.font, cv.RGB(0, 0, 255))
#cv.PutText(D.image, desiredHeading, thirdLineString, D.font, cv.RGB(0,0,255))
cv.Circle(D.image, (320, 240),
10,
cv.RGB(255, 0, 0),
thickness=1,
lineType=8,
shift=0)