def cb(msg):
out_msg = ClusterPointIndices()
out_msg.header = msg.header
for _ in range(len(msg.polygons)):
indices = PointIndices()
out_msg.cluster_indices.append(indices)
pub.publish(out_msg)
def indices_publisher(self, msg):
msg_indices = ClusterPointIndices(header=msg.header)
for rect in msg.rects:
H = self.image.height
W = self.image.width
bbox = [rect.y, rect.x, rect.height + rect.y, rect.width + rect.x]
indices = np.arange(H * W).reshape(H,
W)[bbox[0]:bbox[2],
bbox[1]:bbox[3]].reshape(-1)
indices_msg = PointIndices(header=msg.header, indices=indices)
msg_indices.cluster_indices.append(indices_msg)
self.pub_indices.publish(msg_indices)
def callback(box_array, cluster_indices):
if len(box_array.boxes) > 0:
min_index = 0
min_distance = 100000
for box, i in zip(box_array.boxes, range(len(box_array.boxes))):
distance = math.sqrt(box.pose.position.x * box.pose.position.x +
box.pose.position.y * box.pose.position.y +
box.pose.position.z * box.pose.position.z)
if min_distance > distance:
min_distance = distance
min_index = i
result_box_array = BoundingBoxArray()
result_box_array.header = box_array.header
result_box_array.boxes.append(box_array.boxes[min_index])
pub_box_array.publish(result_box_array)
pub_indices.publish(cluster_indices.cluster_indices[min_index])
result_cluster_indices = ClusterPointIndices()
result_cluster_indices.header = cluster_indices.header
result_cluster_indices.cluster_indices = [cluster_indices.cluster_indices[min_index]]
pub_cluster_indices.publish(result_cluster_indices)
else:
rospy.logwarn("No bounding box input")
def callback(box_array, cluster_indices):
if len(box_array.boxes) > 0:
min_index = 0
min_distance = 100000
for box, i in zip(box_array.boxes, range(len(box_array.boxes))):
distance = math.sqrt(box.pose.position.x * box.pose.position.x +
box.pose.position.y * box.pose.position.y +
box.pose.position.z * box.pose.position.z)
if min_distance > distance:
min_distance = distance
min_index = i
result_box_array = BoundingBoxArray()
result_box_array.header = box_array.header
result_box_array.boxes.append(box_array.boxes[min_index])
pub_box_array.publish(result_box_array)
pub_indices.publish(cluster_indices.cluster_indices[min_index])
result_cluster_indices = ClusterPointIndices()
result_cluster_indices.header = cluster_indices.header
result_cluster_indices.cluster_indices = [
cluster_indices.cluster_indices[min_index]
]
pub_cluster_indices.publish(result_cluster_indices)
else:
rospy.logwarn("No bounding box input")
def callback(self, imgmsg):
bridge = cv_bridge.CvBridge()
img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
img_chw = img.transpose(2, 0, 1) # C, H, W
bboxes, masks, labels, scores = self.model.predict([img_chw])
bboxes = bboxes[0]
masks = masks[0]
labels = labels[0]
scores = scores[0]
msg_indices = ClusterPointIndices(header=imgmsg.header)
msg_labels = LabelArray(header=imgmsg.header)
# -1: label for background
lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
for ins_id, (mask, label) in enumerate(zip(masks, labels)):
indices = np.where(mask.flatten())[0]
indices_msg = PointIndices(header=imgmsg.header, indices=indices)
msg_indices.cluster_indices.append(indices_msg)
class_name = self.fg_class_names[label]
msg_labels.labels.append(Label(id=label, name=class_name))
lbl_cls[mask] = label
lbl_ins[mask] = ins_id # instance_id
self.pub_indices.publish(msg_indices)
self.pub_labels.publish(msg_labels)
msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
self.pub_lbl_cls.publish(msg_lbl_cls)
self.pub_lbl_ins.publish(msg_lbl_ins)
if self.pub_viz.get_num_connections() > 0:
n_fg_class = len(self.fg_class_names)
captions = [
'{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
]
viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
img,
bboxes,
labels + 1,
n_class=n_fg_class + 1,
masks=masks,
captions=captions)
msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
msg_viz.header = imgmsg.header
self.pub_viz.publish(msg_viz)
def _convert(self, rects_msg, img_height=None, img_width=None):
H = self.img_height if img_height is None else img_height
W = self.img_width if img_width is None else img_width
cpi_msg = ClusterPointIndices(header=rects_msg.header)
for rect in rects_msg.rects:
indices_msg = PointIndices(header=rects_msg.header)
ymin = max(0, int(np.floor(rect.y)))
xmin = max(0, int(np.floor(rect.x)))
ymax = min(H, int(np.ceil(rect.y + rect.height)))
xmax = min(W, int(np.ceil(rect.x + rect.width)))
indices = [
range(W * y + xmin, W * y + xmax) for y in range(ymin, ymax)
]
indices_msg.indices = np.array(indices, dtype=np.int32).flatten()
cpi_msg.cluster_indices.append(indices_msg)
self.pub.publish(cpi_msg)
def test_conversion(self):
self.dynparam.update_configuration({'index': 1})
cluster_indices_msg = ClusterPointIndices()
while self.msg is None:
cluster_indices_msg.header.stamp = rospy.Time.now()
for indices in self.cluster_indices:
cluster_indices_msg.cluster_indices.append(
PointIndices(
header=cluster_indices_msg.header,
indices=indices,
)
)
self.pub.publish(cluster_indices_msg)
rospy.sleep(0.1)
self.assertTupleEqual(self.msg.indices, self.cluster_indices[1])
def image_cb(self, msg):
if self.profiling:
rospy.loginfo(
"callback start: incomming msg is %s msec behind" %
((rospy.Time.now() - msg.header.stamp).to_sec() * 1000.0))
tprev = time.time()
try:
# transform image to RGB, float, CHW
img = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8")
img = np.asarray(img, dtype=np.float32)
img = img.transpose(2, 0, 1) # (H, W, C) -> (C, H, W)
except Exception as e:
rospy.logerr("Failed to convert image: %s" % str(e))
return
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("convert",
(tcur - tprev) * 1000))
tprev = tcur
if self.gpu >= 0:
chainer.cuda.get_device_from_id(self.gpu).use()
bboxes, labels, scores = self.model.predict([img])
bboxes, labels, scores = bboxes[0], labels[0], scores[0]
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("predict",
(tcur - tprev) * 1000))
tprev = tcur
labels_msg = LabelArray(header=msg.header)
for l in labels:
l_name = self.label_names[l]
labels_msg.labels.append(Label(id=l, name=l_name))
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make labels msg",
(tcur - tprev) * 1000))
cluster_indices_msg = ClusterPointIndices(header=msg.header)
H = img.shape[1]
W = img.shape[2]
for bbox in bboxes:
ymin = max(0, int(np.floor(bbox[0])))
xmin = max(0, int(np.floor(bbox[1])))
ymax = min(H, int(np.ceil(bbox[2])))
xmax = min(W, int(np.ceil(bbox[3])))
indices = [
range(W * y + xmin, W * y + xmax) for y in range(ymin, ymax)
]
indices = np.array(indices, dtype=np.int32).flatten()
indices_msg = PointIndices(header=msg.header, indices=indices)
cluster_indices_msg.cluster_indices.append(indices_msg)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make cluster_indices msg",
(tcur - tprev) * 1000))
tprev = tcur
rect_msg = RectArray(header=msg.header)
for bbox in bboxes:
rect = Rect(x=bbox[1],
y=bbox[0],
width=bbox[3] - bbox[1],
height=bbox[2] - bbox[0])
rect_msg.rects.append(rect)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make rect msg",
(tcur - tprev) * 1000))
tprev = tcur
cls_msg = ClassificationResult(
header=msg.header,
classifier=self.classifier_name,
target_names=self.label_names,
labels=labels,
label_names=[self.label_names[l] for l in labels],
label_proba=scores,
)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make cls msg",
(tcur - tprev) * 1000))
tprev = tcur
self.pub_labels.publish(labels_msg)
self.pub_indices.publish(cluster_indices_msg)
self.pub_rects.publish(rect_msg)
self.pub_class.publish(cls_msg)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("publish msg",
(tcur - tprev) * 1000))
tprev = tcur
if self.visualize:
self.publish_bbox_image(img, bboxes, labels, scores, msg.header)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("callback end",
(tcur - tprev) * 1000))
tprev = tcur
def callback(self, imgmsg):
bridge = cv_bridge.CvBridge()
img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
img_chw = img.transpose(2, 0, 1) # C, H, W
if self.gpu >= 0:
chainer.cuda.get_device_from_id(self.gpu).use()
bboxes, masks, labels, scores = self.model.predict([img_chw])
bboxes = bboxes[0]
masks = masks[0]
labels = labels[0]
scores = scores[0]
msg_indices = ClusterPointIndices(header=imgmsg.header)
msg_labels = LabelArray(header=imgmsg.header)
# -1: label for background
lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
for ins_id, (mask, label) in enumerate(zip(masks, labels)):
indices = np.where(mask.flatten())[0]
indices_msg = PointIndices(header=imgmsg.header, indices=indices)
msg_indices.cluster_indices.append(indices_msg)
class_name = self.fg_class_names[label]
msg_labels.labels.append(Label(id=label, name=class_name))
lbl_cls[mask] = label
lbl_ins[mask] = ins_id # instance_id
self.pub_indices.publish(msg_indices)
self.pub_labels.publish(msg_labels)
msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
self.pub_lbl_cls.publish(msg_lbl_cls)
self.pub_lbl_ins.publish(msg_lbl_ins)
cls_msg = ClassificationResult(
header=imgmsg.header,
classifier=self.classifier_name,
target_names=self.fg_class_names,
labels=labels,
label_names=[self.fg_class_names[l] for l in labels],
label_proba=scores,
)
rects_msg = RectArray(header=imgmsg.header)
for bbox in bboxes:
rect = Rect(x=bbox[1],
y=bbox[0],
width=bbox[3] - bbox[1],
height=bbox[2] - bbox[0])
rects_msg.rects.append(rect)
self.pub_rects.publish(rects_msg)
self.pub_class.publish(cls_msg)
if self.pub_viz.get_num_connections() > 0:
n_fg_class = len(self.fg_class_names)
captions = [
'{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
]
viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
img,
bboxes,
labels + 1,
n_class=n_fg_class + 1,
masks=masks,
captions=captions)
msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
msg_viz.header = imgmsg.header
self.pub_viz.publish(msg_viz)
def _decompose(self, *cpi_msgs):
out_msg = ClusterPointIndices()
out_msg.header = cpi_msgs[0].header
for cpi_msg in cpi_msgs:
out_msg.cluster_indices.extend(cpi_msg.cluster_indices)
self._pub.publish(out_msg)