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)
