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')