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