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 image_cb(self, msg): if not hasattr(self, 'engine'): return if self.transport_hint == 'compressed': np_arr = np.fromstring(msg.data, np.uint8) img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) img = img[:, :, ::-1] else: img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='rgb8') points, key_names, visibles, bboxes, labels, scores \ = self._estimate(img) poses_msg = PeoplePoseArray(header=msg.header) rects_msg = RectArray(header=msg.header) for point, key_name, visible, bbox, label, score in zip( points, key_names, visibles, bboxes, labels, scores): pose_msg = PeoplePose() for pt, key_nm, vs, sc in zip(point, key_name, visible, score): if vs: key_y, key_x = pt pose_msg.limb_names.append(key_nm) pose_msg.scores.append(sc) pose_msg.poses.append( Pose(position=Point(x=key_x, y=key_y))) poses_msg.poses.append(pose_msg) y_min, x_min, y_max, x_max = bbox rect = Rect(x=x_min, y=y_min, width=x_max - x_min, height=y_max - y_min) rects_msg.rects.append(rect) cls_msg = ClassificationResult( header=msg.header, classifier=self.classifier_name, target_names=self.label_names, labels=labels, label_names=[self.label_names[lbl] for lbl in labels], label_proba=[np.average(score) for score in scores]) self.pub_pose.publish(poses_msg) self.pub_rects.publish(rects_msg) self.pub_class.publish(cls_msg) if self.enable_visualization: with self.lock: self.img = img self.header = msg.header self.points = points self.visibles = visibles
def _create_2d_people_pose_array_msgs(self, people_joint_positions, header): people_pose_msg = PeoplePoseArray(header=header) for person_joint_positions in people_joint_positions: pose_msg = PeoplePose() for joint_pos in person_joint_positions: if joint_pos['score'] < 0: continue pose_msg.limb_names.append(joint_pos['limb']) pose_msg.scores.append(joint_pos['score']) pose_msg.poses.append(Pose(position=Point(x=joint_pos['x'], y=joint_pos['y'], z=0))) people_pose_msg.poses.append(pose_msg) return people_pose_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 publish(self, tracked_bodies3D, timestamp): people_pose_array = PeoplePoseArray() people_pose_array.header.frame_id = "head_mount_kinect2_rgb_optical_frame" people_pose_array.header.stamp = timestamp for body_id, body in tracked_bodies3D.items(): people_pose = PeoplePose() for part_id, part in body.body.items(): point = Point(x=part[0][0], y=part[0][1], z=part[0][2]) people_pose.limb_names.append( str(body_id) + ':' + str(part_id)) people_pose.scores.append(part[1]) people_pose.poses.append(Pose(position=point)) people_pose_array.poses.append(people_pose) self.pose3D_pub.publish(people_pose_array)
def _create_people_pose_array_msgs(self, people_joint_positions, header): people_pose_msg = PeoplePoseArray(header=header) for i, person_joint_positions in enumerate(people_joint_positions): pose_msg = PeoplePose() for joint_pose in person_joint_positions: pose_msg.limb_names.append(str(i)) pose_msg.scores.append(0.0) q_xyzw = tf.transformations.quaternion_from_matrix(joint_pose) pose_msg.poses.append( Pose(position=Point(x=joint_pose[0, 3], y=joint_pose[1, 3], z=joint_pose[2, 3]), orientation=Quaternion(x=q_xyzw[0], y=q_xyzw[1], z=q_xyzw[2], w=q_xyzw[3]))) people_pose_msg.poses.append(pose_msg) return people_pose_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)