def draw_fore_info(self, img_fore, bbox, bbox_init, speed, angle, kcurvs, thumb_n_point, mid_fing_pos):
img_fore = cv2.cvtColor(img_fore, cv2.COLOR_GRAY2BGR)
fnt = cv2.FONT_HERSHEY_PLAIN
fnt_size = 2.4
clr = RGB(255, 0, 0)
state = 'INIT' if self.state == TrackerState.INIT else 'RUN'
cv2.putText(img_fore, 'State: %s' % state, (30, 30) , fnt, fnt_size, clr)
bbox_str = 'Speed: %03d' % speed
cv2.putText(img_fore, bbox_str, (30, 60) , fnt, fnt_size, clr)
#print self.turn_angle_init, angle
if self.state == TrackerState.RUN:
turn_str = 'Turn: %03d deg.' % (int((self.turn_angle_init - angle) * 180 / np.pi))
else:
turn_str = 'Turn: %03d deg.' % 0
cv2.putText(img_fore, turn_str, (30, 90) , fnt, fnt_size, clr)
bbox_p1 = (bbox[1], bbox[0])
bbox_p2 = (bbox[1] + bbox[3], bbox[0] + bbox[2])
cv2.rectangle(img_fore, bbox_p1, bbox_p2, RGB(0, 255, 0), 5)
bbox_cx = bbox[1] + bbox[3] / 2
bbox_cy = bbox[0] + bbox[2] / 2
draw_cross(img_fore, (bbox_cx, bbox_cy), 30, RGB(0, 255, 255))
for i in range(len(kcurvs)):
kc = kcurvs[i]
tnp = thumb_n_point[i]
for j in tnp:
pass
#draw_cross(img_fore, kc[j][0], 30, RGB(255, 255, 0))
if bbox_init:
bbox_p1 = (bbox_init[1], bbox_init[0])
bbox_p2 = (bbox_init[1] + bbox_init[3], bbox_init[0] + bbox_init[2])
cv2.rectangle(img_fore, bbox_p1, bbox_p2, RGB(255, 0, 0), 5)
if self.recog_cnt > 0:
p1 = (int(self.hand_area[0].x), int(self.hand_area[0].y))
p2 = (int(self.hand_area[1].x), int(self.hand_area[1].y))
cv2.rectangle(img_fore, p1, p2, RGB(0, 0, 255), 2)
cv2.imwrite('out_fore/img_fore_%03d.png' % self.depth_frame_cnt, img_fore)
return img_fore
resp.poses.append(pose_wrt_camera)
if DEBUG:
print 'NOT FOUND'
if DEBUG:
cv2.imwrite('cub.png', self.img)
if DETECT_METHOD == DETECT_METHOD_CONTOUR:
cv2.imwrite('back_%04d.png' % self.rgb_frame_cnt, back)
for c in cube_rects:
cv2.rectangle(conts_img, (c[0] - 3, c[1] - 3), \
(c[0] + c[2], \
c[1] + c[3]), RGB(255,255,255))
cv2.imwrite('conts_%04d.png' % self.rgb_frame_cnt, conts_img)
for c in cube_rects:
cv2.rectangle(self.img, c[0:2], (c[0] + c[2], c[1] + c[3]), RGB(255,255,255))
draw_cross(self.img, (c[0] + c[2] / 2, c[1] + c[3] / 2), 10, SCALAR_WHITE, 1)
cv2.imwrite('result_%04d.png' % self.rgb_frame_cnt, self.img)
if DETECT_PERF:
self.dperf_file.write('%d %d %d %d\n' % cube_rects[0])
return resp
def run(self):
rospy.spin()
def load_hue_histograms(self):
for h in self.known_histograms.keys():
hist = Histogram()
hist.load(self.known_histograms[h][0])
self.known_histograms[h][1] = hist
def draw_debug_info(self, img_orig, img_fore, bbox, bbox_init, conts, img_contours, defects, defects_max, finger_tips, pt_fings, thumb_n_point, mid_fing_pos, kcurvs, hand_kcurv_i, hulls):
if len(conts) > 0:
if len(defects_max) > 0:
if len(defects_max[0]) > 0:
p1 = tuple(conts[0][defects_max[0][0][0]][0])
p2 = tuple(conts[0][defects_max[0][0][1]][0])
if bbox_init == None:
bbox_init = (-1, -1, -1, -1)
result_str = 'bi %03d %03d %03d %03d b %03d %03d %03d %03d p1 %03d %03d p2 %03d %03d\n' % (bbox_init + bbox + p1 + p2)
self.result_file.write(result_str)
#print finger_tips
img_curv = img_contours.copy()
for cf_i in range(len(finger_tips)):
f = finger_tips[cf_i]
for p in f:
cv2.circle(img_contours, tuple(kcurvs[cf_i][p][0]), 10, RGB(0, 255, 0))
for k in range(len(f)):
for l in range(len(f)):
pass
#cv2.line(img_contours, tuple(kcurvs[cf_i][f[k]][0]), tuple(kcurvs[cf_i][f[l]][0]), RGB(255,255,255))
for i in range(len(hulls)):
p1 = tuple(conts[i][defects_max[i][0][0]][0])
p2 = tuple(conts[i][defects_max[i][0][1]][0])
cv2.line(img_contours, p1, p2, RGB(0, 255, 255))
cx = bbox[1] + bbox[3] / 2
cy = bbox[0] + bbox[2] / 2
draw_cross(img_contours, (cx, cy), 30, RGB(0, 255, 0))
for pt_fing in pt_fings:
draw_cross(img_contours, pt_fing, 30, RGB(0, 255, 0))
'''
for d_i in range(len(defects)):
defs = defects[d_i]
cont = conts[d_i]
for j in range(len(defs)):
def_pt = defs[j][0]
depth_pt_ind = def_pt[2]
depth_pt = cont[depth_pt_ind][0]
draw_cross(img_contours, depth_pt, 30, RGB(255, 155, 100))
#print 'defs', defs
#print 'cont', cont
'''
'''
if hand_kcurv_i == -1:
hand_recog = 'No Hand'
else:
hand_recog = 'Hand'
'''
#draw_debug_messages(img_contours, [self.kcurv_stat_str], (100, 200))
#draw_debug_messages(img_contours, [hand_recog], (100, 100))
cv2.imwrite('out_cont/img_contours_%03d.png' % self.depth_frame_cnt, img_contours)
for i in range(len(kcurvs)):
kc = kcurvs[i]
l = len(kc)
for pt_i in range(1, len(kc) - 1):
pt0 = kc[pt_i - 1]
pt1 = kc[pt_i]
pt2 = kc[pt_i + 1]
cv2.line(img_curv, tuple(pt0[0]), tuple(pt1[0]), RGB(0, 255, 0))
cv2.putText(img_curv, '%.3f' % pt1[1], tuple(pt1[0]), cv2.FONT_HERSHEY_PLAIN, 0.7, RGB(0, 255, 0))
cv2.line(img_curv, tuple(pt1[0]), tuple(pt2[0]), RGB(0, 255, 0))
cv2.imwrite('out_kcurv/img_kcurvs_%03d.png' % self.depth_frame_cnt, img_curv)
#self.draw_fore_info(img_fore, bbox, bbox_init, speed, kcurvs, thumb_n_point, mid_fing_pos)
#print thumb_n_point
img_hulls = 255 * np.ones([480, 640, 3], dtype='uint8')
self.draw_convex_hulls(img_hulls, hulls)
cv2.imwrite('out_hull/img_hulls_%03d.png' % self.depth_frame_cnt, img_hulls)
img_orig_cm = (100 * img_orig * 255 / 200).astype('uint8')
