This repository has been archived by the owner on Jun 12, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 47
/
mask_rcnn_instance_segmentation.py
executable file
·130 lines (107 loc) · 3.98 KB
/
mask_rcnn_instance_segmentation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#!/usr/bin/env python
from chainercv.links.model.fpn import MaskRCNNFPNResNet50
import gdown
import numpy as np
import morefusion
import cv_bridge
from morefusion_panda_ycb_video.msg import ObjectClass
from morefusion_panda_ycb_video.msg import ObjectClassArray
import rospy
from sensor_msgs.msg import Image
from topic_tools import LazyTransport
class MaskRCNNInstanceSegmentationNode(LazyTransport):
def __init__(self):
super().__init__()
self._class_names = morefusion.datasets.ycb_video.class_names
self._blacklist = [5, 10, 12] # missing in DRL
self._one_instance_per_class = True
# logs.20191003.bg_composite_false
pretrained_model = gdown.cached_download(
url="https://drive.google.com/uc?id=1MS2OgvjYF6aPIDjDr_fU-IhPN700Cv4V", # NOQA
md5="f169417a5bab67e8b48337b2a341e890",
)
self._model = MaskRCNNFPNResNet50(
n_fg_class=len(self._class_names[1:]),
pretrained_model=pretrained_model,
)
self._model.score_thresh = 0.75
self._model.to_gpu()
self._pub_cls = self.advertise(
"~output/class", ObjectClassArray, queue_size=1
)
self._pub_ins = self.advertise(
"~output/label_ins", Image, queue_size=1
)
self._post_init() # FIXME
def subscribe(self):
self._sub = rospy.Subscriber(
"~input",
Image,
callback=self.callback,
queue_size=1,
buff_size=2 ** 24,
)
def unsubscribe(self):
self._sub.unregister()
def callback(self, imgmsg):
bridge = cv_bridge.CvBridge()
rgb = bridge.imgmsg_to_cv2(imgmsg, desired_encoding="rgb8")
masks, labels, confs = self._model.predict(
[rgb.astype(np.float32).transpose(2, 0, 1)]
)
masks = masks[0]
labels = labels[0]
confs = confs[0]
class_ids = labels + 1
del labels
if self._blacklist:
keep = ~np.isin(class_ids, self._blacklist)
masks = masks[keep]
class_ids = class_ids[keep]
confs = confs[keep]
if len(class_ids) > 0:
keep = masks.sum(axis=(1, 2)) > 0
class_ids = class_ids[keep]
masks = masks[keep]
confs = confs[keep]
if len(class_ids) > 0:
uniq, counts = np.unique(class_ids, return_counts=True)
keep = []
for cls_id, count in zip(uniq, counts):
if count == 1:
index = np.argwhere(class_ids == cls_id)[0, 0]
else:
index = np.argmax(confs[class_ids == cls_id])
keep.append(index)
class_ids = class_ids[keep]
masks = masks[keep]
confs = confs[keep]
if len(class_ids) > 0:
sort = np.argsort(confs)
class_ids = class_ids[sort]
masks = masks[sort]
confs = confs[sort]
instance_ids = np.arange(0, len(masks))
label_ins = np.full(rgb.shape[:2], -1, dtype=np.int32)
for ins_id, mask in zip(instance_ids, masks):
label_ins[mask] = ins_id
ins_msg = bridge.cv2_to_imgmsg(label_ins)
ins_msg.header = imgmsg.header
self._pub_ins.publish(ins_msg)
instance_ids_active = np.unique(label_ins)
keep = np.isin(instance_ids, instance_ids_active)
instance_ids = instance_ids[keep]
class_ids = class_ids[keep]
confs = confs[keep]
cls_msg = ObjectClassArray(header=imgmsg.header)
for ins_id, cls_id, conf in zip(instance_ids, class_ids, confs):
cls_msg.classes.append(
ObjectClass(
instance_id=ins_id, class_id=cls_id, confidence=conf,
)
)
self._pub_cls.publish(cls_msg)
if __name__ == "__main__":
rospy.init_node("mask_rcnn_instance_segmentation")
MaskRCNNInstanceSegmentationNode()
rospy.spin()