def image_cb(self, msg): img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='rgb8') resized_img = cv2.resize(img, (self.resized_W, self.resized_H)) H, W, _ = img.shape y_scale = self.resized_H / H x_scale = self.resized_W / W poses, _ = self.engine.DetectPosesInImage(resized_img.astype(np.uint8)) poses_msg = PeoplePoseArray() poses_msg.header = msg.header points = [] visibles = [] for pose in poses: if pose.score < self.score_thresh: continue pose_msg = PeoplePose() point = [] visible = [] for lbl, keypoint in pose.keypoints.items(): resized_key_y, resized_key_x = keypoint.yx key_y = resized_key_y / y_scale key_x = resized_key_x / x_scale point.append((key_y, key_x)) if keypoint.score < self.joint_score_thresh: visible.append(False) continue pose_msg.limb_names.append(lbl) pose_msg.scores.append(keypoint.score) pose_msg.poses.append(Pose(position=Point(x=key_x, y=key_y))) visible.append(True) poses_msg.poses.append(pose_msg) points.append(point) visibles.append(visible) self.pub_pose.publish(poses_msg) points = np.array(points, dtype=np.int32) visibles = np.array(visibles, dtype=np.bool) if self.visualize: fig = plt.figure(tight_layout={'pad': 0}) ax = plt.Axes(fig, [0., 0., 1., 1.]) ax.axis('off') fig.add_axes(ax) vis_point(img.transpose((2, 0, 1)), points, visibles, ax=ax) fig.canvas.draw() w, h = fig.canvas.get_width_height() vis_img = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8) vis_img.shape = (h, w, 3) fig.clf() plt.close() vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8') # BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA vis_msg.step = int(vis_msg.step) vis_msg.header = msg.header self.pub_image.publish(vis_msg)
def _cb_with_depth_info(self, img_msg, depth_msg, camera_info_msg): br = cv_bridge.CvBridge() img = br.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') depth_img = br.imgmsg_to_cv2(depth_msg, 'passthrough') if depth_msg.encoding == '16UC1': depth_img = np.asarray(depth_img, dtype=np.float32) depth_img /= 1000 # convert metric: mm -> m elif depth_msg.encoding != '32FC1': rospy.logerr('Unsupported depth encoding: %s' % depth_msg.encoding) people_joint_positions, all_peaks = self.pose_estimate(img) if self.use_hand: people_joint_positions = self.hand_estimate( img, people_joint_positions) people_pose_msg = PeoplePoseArray() people_pose_msg.header = img_msg.header people_pose_2d_msg = self._create_2d_people_pose_array_msgs( people_joint_positions, img_msg.header) # calculate xyz-position fx = camera_info_msg.K[0] fy = camera_info_msg.K[4] cx = camera_info_msg.K[2] cy = camera_info_msg.K[5] for person_joint_positions in people_joint_positions: pose_msg = PeoplePose() for joint_pos in person_joint_positions: if joint_pos['score'] < 0: continue if 0 <= joint_pos['y'] < depth_img.shape[0] and\ 0 <= joint_pos['x'] < depth_img.shape[1]: z = float(depth_img[int(joint_pos['y'])][int( joint_pos['x'])]) else: continue if np.isnan(z): continue x = (joint_pos['x'] - cx) * z / fx y = (joint_pos['y'] - cy) * z / fy pose_msg.limb_names.append(joint_pos['limb']) pose_msg.scores.append(joint_pos['score']) pose_msg.poses.append( Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(w=1))) people_pose_msg.poses.append(pose_msg) self.pose_2d_pub.publish(people_pose_2d_msg) self.pose_pub.publish(people_pose_msg) if self.visualize: vis_img = self._draw_joints(img, people_joint_positions, all_peaks) vis_msg = br.cv2_to_imgmsg(vis_img, encoding='bgr8') vis_msg.header.stamp = img_msg.header.stamp self.image_pub.publish(vis_msg)
def _cb_with_depth_info(self, img_msg, depth_msg, camera_info_msg): br = cv_bridge.CvBridge() img = br.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') depth_img = br.imgmsg_to_cv2(depth_msg, 'passthrough') if depth_msg.encoding == '16UC1': depth_img = np.asarray(depth_img, dtype=np.float32) depth_img /= 1000 # convert metric: mm -> m elif depth_msg.encoding != '32FC1': rospy.logerr('Unsupported depth encoding: %s' % depth_msg.encoding) people_joint_positions, all_peaks = self.pose_estimate(img) if self.use_hand: people_joint_positions = self.hand_estimate( img, people_joint_positions) people_pose_msg = PeoplePoseArray() people_pose_msg.header = img_msg.header people_pose_2d_msg = self._create_2d_people_pose_array_msgs( people_joint_positions, img_msg.header) # calculate xyz-position fx = camera_info_msg.K[0] fy = camera_info_msg.K[4] cx = camera_info_msg.K[2] cy = camera_info_msg.K[5] for person_joint_positions in people_joint_positions: pose_msg = PeoplePose() for joint_pos in person_joint_positions: if joint_pos['score'] < 0: continue if 0 <= joint_pos['y'] < depth_img.shape[0] and\ 0 <= joint_pos['x'] < depth_img.shape[1]: z = float(depth_img[int(joint_pos['y'])][int(joint_pos['x'])]) else: continue if np.isnan(z): continue x = (joint_pos['x'] - cx) * z / fx y = (joint_pos['y'] - cy) * z / fy pose_msg.limb_names.append(joint_pos['limb']) pose_msg.scores.append(joint_pos['score']) pose_msg.poses.append(Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(w=1))) people_pose_msg.poses.append(pose_msg) self.pose_2d_pub.publish(people_pose_2d_msg) self.pose_pub.publish(people_pose_msg) if self.visualize: vis_img = self._draw_joints(img, people_joint_positions, all_peaks) vis_msg = br.cv2_to_imgmsg(vis_img, encoding='bgr8') vis_msg.header.stamp = img_msg.header.stamp self.image_pub.publish(vis_msg)
def image_cb(self, msg): img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='rgb8') resized_img = cv2.resize(img, (641, 481)) poses, _ = self.engine.DetectPosesInImage(resized_img.astype(np.uint8)) poses_msg = PeoplePoseArray() poses_msg.header = msg.header points = [] visibles = [] for pose in poses: if pose.score < self.score_thresh: continue pose_msg = PeoplePose() point = [] visible = [] for lbl, keypoint in pose.keypoints.items(): point.append(keypoint.yx) if keypoint.score < self.joint_score_thresh: visible.append(False) continue pose_msg.limb_names.append(lbl) pose_msg.scores.append(keypoint.score) pose_msg.poses.append( Pose(position=Point(x=keypoint.yx[1], y=keypoint.yx[0]))) visible.append(True) poses_msg.poses.append(pose_msg) points.append(point) visibles.append(visible) self.pub_pose.publish(poses_msg) points = np.array(points, dtype=np.int32) visibles = np.array(visibles, dtype=np.bool) if self.visualize: vis_img = resized_img.transpose(2, 0, 1) vis_point(vis_img, points, visibles) fig = plt.gcf() fig.canvas.draw() w, h = fig.canvas.get_width_height() vis_img = np.fromstring( fig.canvas.tostring_rgb(), dtype=np.uint8) fig.clf() vis_img.shape = (h, w, 3) plt.close() vis_msg = self.bridge.cv2_to_imgmsg(vis_img, 'rgb8') # BUG: https://answers.ros.org/question/316362/sensor_msgsimage-generates-float-instead-of-int-with-python3/ # NOQA vis_msg.step = int(vis_msg.step) vis_msg.header = msg.header self.pub_image.publish(vis_msg)
def _cb_with_depth_info(self, img_msg, depth_msg, camera_info_msg): br = cv_bridge.CvBridge() img = br.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') depth_img = br.imgmsg_to_cv2(depth_msg, 'passthrough') if depth_msg.encoding == '16UC1': depth_img = np.asarray(depth_img, dtype=np.float32) depth_img /= 1000 # convert metric: mm -> m elif depth_msg.encoding != '32FC1': rospy.logerr('Unsupported depth encoding: %s' % depth_msg.encoding) people_joint_positions, all_peaks = self.pose_estimate(img) if self.use_hand: people_joint_positions = self.hand_estimate( img, people_joint_positions) people_pose_msg = PeoplePoseArray() people_pose_msg.header = img_msg.header people_pose_2d_msg = self._create_2d_people_pose_array_msgs( people_joint_positions, img_msg.header) skeleton_msgs = HumanSkeletonArray(header=img_msg.header) # calculate xyz-position fx = camera_info_msg.K[0] fy = camera_info_msg.K[4] cx = camera_info_msg.K[2] cy = camera_info_msg.K[5] for person_joint_positions in people_joint_positions: pose_msg = PeoplePose() skeleton_msg = HumanSkeleton(header=img_msg.header) for joint_pos in person_joint_positions: if joint_pos['score'] < 0: continue if 0 <= joint_pos['y'] < depth_img.shape[0] and\ 0 <= joint_pos['x'] < depth_img.shape[1]: z = float(depth_img[int(joint_pos['y'])][int(joint_pos['x'])]) else: continue if np.isnan(z): continue x = (joint_pos['x'] - cx) * z / fx y = (joint_pos['y'] - cy) * z / fy pose_msg.limb_names.append(joint_pos['limb']) pose_msg.scores.append(joint_pos['score']) pose_msg.poses.append(Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(w=1))) people_pose_msg.poses.append(pose_msg) for i, conn in enumerate(self.limb_sequence): j1_name = self.index2limbname[conn[0] - 1] j2_name = self.index2limbname[conn[1] - 1] if j1_name not in pose_msg.limb_names \ or j2_name not in pose_msg.limb_names: continue j1_index = pose_msg.limb_names.index(j1_name) j2_index = pose_msg.limb_names.index(j2_name) bone_name = '{}->{}'.format(j1_name, j2_name) bone = Segment( start_point=pose_msg.poses[j1_index].position, end_point=pose_msg.poses[j2_index].position) skeleton_msg.bones.append(bone) skeleton_msg.bone_names.append(bone_name) skeleton_msgs.skeletons.append(skeleton_msg) self.pose_2d_pub.publish(people_pose_2d_msg) self.pose_pub.publish(people_pose_msg) self.skeleton_pub.publish(skeleton_msgs) if self.visualize: vis_img = self._draw_joints(img, people_joint_positions, all_peaks) vis_msg = br.cv2_to_imgmsg(vis_img, encoding='bgr8') vis_msg.header.stamp = img_msg.header.stamp self.image_pub.publish(vis_msg)