def callback(data): publish_people = people_pub.get_num_connections() > 0 if publish_people: fetch_list = ['part_affinity_fields', 'key_points'] feed_dict = {'key_point_threshold': pose_params['key_point_threshold']} for st in data.sparse_tensors: feed_dict[st.name] = [decode_sparse_tensor(st)] t0 = rospy.Time.now() outputs = pose_detector.compute(fetch_list, feed_dict) t1 = rospy.Time.now() rospy.loginfo('Mid-stage to stage4: {} sec'.format((t1-t0).to_sec())) #people = extract_people(heat_map[0], affinity[0]) affinity = outputs[0][0] keypoints = outputs[1][:,1:] people = connect_parts(affinity, keypoints, limbs, line_division=pose_params['line_division'], threshold=pose_params['affinity_threshold']) h, w = affinity.shape[:2] people = [ { pose_detector._part_names[k]: ((keypoints[v][1]+0.5)/w, (keypoints[v][0]+0.5)/h) \ for k,v in person.items() } for person in people ] t2 = rospy.Time.now() time_pub.publish(t2-t1) rospy.loginfo("Post processing: {} sec".format((t2-t1).to_sec())) rospy.loginfo("Total: {} sec".format((t2-t0).to_sec())) print("Found {} people.".format(len(people))) msg = PersonArray() msg.header = data.header msg.people = [Person(body_parts=[KeyPoint(name=k, x=x, y=y) \ for k,(x,y) in p.items()]) \ for p in people] people_pub.publish(msg)
def callback(data): if people_pub.get_num_connections() > 0 or stage1_pub.get_num_connections() > 0: try: cv_image = bridge.imgmsg_to_cv2(data, 'rgb8') except CvBridgeError as e: rospy.logerr(e) return if people_pub.get_num_connections() > 0: persons = pose_detector.detect_keypoints(cv_image, **pose_params) msg = PersonArray(header=data.header) msg.people = [Person(body_parts=[KeyPoint(name=k, x=x, y=y) \ for k,(x,y) in p.items()]) \ for p in persons] people_pub.publish(msg) if stage1_pub.get_num_connections() > 0: msg = SparseTensorArray() msg.header = data.header fetch_list = [ pose_detector.end_points['stage0'], pose_detector.end_points['stage1_L2'] ] feed_dict = { pose_detector.ph_x: cv_image / 255. } outputs = pose_detector.sess.run(fetch_list, feed_dict) stage0 = encode_sparse_tensors(outputs[0], threshold=0.1) stage0.name = 'stage0' stage1_L2 = encode_sparse_tensors(outputs[0], threshold=0.1) stage1_L2.name = 'stage1_L2' msg.sparse_tensors = [stage0, stage1_L2] stage1_pub.publish(msg)
def detect_people(req): try: cv_image = bridge.imgmsg_to_cv2(req.image, 'rgb8') except CvBridgeError as e: rospy.logerr(e) return None persons = pose_detector.detect_keypoints(cv_image, **pose_params) msg = DetectPeopleResponse() msg.people = [Person(body_parts=[KeyPoint(name=k, x=x, y=y) \ for k,(x,y) in p.items()]) \ for p in persons] return msg
def callback(data): if not people_pub.get_num_connections(): return try: cv_image = bridge.imgmsg_to_cv2(data, 'rgb8') except CvBridgeError as e: rospy.logerr(e) return persons = pose_detector.detect_keypoints(cv_image, **pose_params) msg = PersonArray(header=data.header) msg.people = [Person(body_parts=[KeyPoint(name=k, x=x, y=y) \ for k,(x,y) in p.items()]) \ for p in persons] people_pub.publish(msg)
def compute(req): # determine tensor to feed if req.input == 'image': input_stage = 0 cv_image = bridge.imgmsg_to_cv2(req.image, 'rgb8') cv_image = cv_image/255. if pose_detector.input_shape is not None: cv_image = cv2.resize(cv_image, pose_detector.input_shape[::-1]) image_batch = np.expand_dims(cv_image, 0) feed_dict = {pose_detector.ph_x: image_batch} else: if not req.input.startswith('stage'): raise ValueError('Argument "input" must be "image" or starts with "stage".') if req.input[5:] not in ['1', '2', '3', '4', '5', '6']: raise ValueError('Argument "input" specifies illegal stage.') input_stage = int(req.input[5:]) feed_dict = { pose_detector.end_points[req.input+'_L1']: [decode_sparse_tensor(req.affinity_field)], pose_detector.end_points[req.input+'_L2']: [decode_sparse_tensor(req.confidence_map)], pose_detector.end_points['stage0']: [decode_sparse_tensor(req.feature_map)] } feed_dict[pose_detector.ph_threshold] = pose_params['key_point_threshold'] # determine tensor to fetch if req.output == 'people': output_stage = 6 fetch_list = [pose_detector.heat_map, pose_detector.affinity, pose_detector.keypoints] rospy.loginfo('Start processing.') heat_map, affinity, keypoints = pose_detector.sess.run(fetch_list, feed_dict) rospy.loginfo('Done.') # TODO: Scale is not always 8. It varies according to the preprocessing. scale_x = 8. scale_y = 8. inlier_lists = [] for _,y,x,c in keypoints: x = x*scale_x + scale_x/2 y = y*scale_y + scale_y/2 inlier_lists.append((x,y)) persons = connect_parts(affinity[0], keypoints[:,1:], pose_detector.limbs, line_division = pose_params['line_division'], threshold = pose_params['affinity_threshold']) persons = [{pose_detector.part_names[k]:inlier_lists[v] \ for k,v in person.items()} for person in persons] msg = PersonArray() msg.people = [Person(body_parts=[KeyPoint(name=k, x=x, y=y) \ for k,(x,y) in p.items()]) \ for p in persons] return ComputeResponse(people=msg) else: if not req.output.startswith('stage'): raise ValueError('Argument "output" must be "people" or starts with "stage".') if req.output[5:] not in ['1', '2', '3', '4', '5', '6']: raise ValueError('Argument "output" specifies illegal stage.') output_stage = int(req.output[5:]) if output_stage < input_stage: raise ValueError('Output stage must be greater than input stage.') fetch_list = [ pose_detector.end_points[req.output+'_L1'], pose_detector.end_points[req.output+'_L2'], pose_detector.end_points['stage0'] ] affinity, heat_map, feat_map = pose_detector.sess.run(fetch_list, feed_dict) return ComputeResponse(feature_map=encode_sparse_tensor(feat_map[0], threshold=0.2), affinity_field=encode_sparse_tensor(affinity[0]), confidence_map=encode_sparse_tensor(heat_map[0]))