4 from distutils.version
import LooseVersion
9 import matplotlib.pyplot
as plt
12 import scipy.ndimage
as ndi
14 from skimage.color
import gray2rgb
15 from skimage.color
import label2rgb
16 from skimage.future.graph
import merge_hierarchical
17 from skimage.future.graph
import RAG
18 from skimage.future.graph.rag
import _add_edge_filter
19 from skimage.morphology.convex_hull
import convex_hull_image
20 from skimage.segmentation
import slic
21 from skimage.util
import img_as_uint
24 from jsk_topic_tools
import ConnectionBasedTransport
25 from jsk_topic_tools
import warn_no_remap
26 import message_filters
28 from sensor_msgs.msg
import Image
30 if LooseVersion(skimage.__version__) >=
'0.13.0':
31 from skimage.future.graph
import show_rag
33 from skimage.future.graph
import draw_rag
42 if LooseVersion(skimage.__version__) >=
'0.16.0':
49 fp = ndi.generate_binary_structure(labels.ndim, connectivity)
50 for d
in range(fp.ndim):
51 fp = fp.swapaxes(0, d)
53 fp = fp.swapaxes(0, d)
68 function=_add_edge_filter,
71 output=np.zeros(labels.shape, dtype=np.uint8),
72 extra_arguments=(graph,))
80 solidity = 1. * mask.sum() / convex_hull_image(mask).sum()
81 graph.node[n].update({
'labels': [n],
85 if LooseVersion(networkx.__version__) >=
'2':
86 edges_iter = graph.edges(data=
True)
88 edges_iter = graph.edges_iter(data=
True)
89 for x, y, d
in edges_iter:
90 new_mask = np.logical_or(graph.node[x][
'mask'], graph.node[y][
'mask'])
91 new_solidity = 1. * new_mask.sum() / convex_hull_image(new_mask).sum()
92 org_solidity = np.mean([graph.node[x][
'solidity'],
93 graph.node[y][
'solidity']])
94 d[
'weight'] = org_solidity / new_solidity
104 """Callback to handle merging nodes by recomputing solidity."""
105 org_solidity = np.mean([graph.node[src][
'solidity'],
106 graph.node[dst][
'solidity']])
107 new_mask1 = np.logical_or(graph.node[src][
'mask'], graph.node[n][
'mask'])
108 new_mask2 = np.logical_or(graph.node[dst][
'mask'], graph.node[n][
'mask'])
109 new_solidity1 = 1. * new_mask1.sum() / convex_hull_image(new_mask1).sum()
110 new_solidity2 = 1. * new_mask2.sum() / convex_hull_image(new_mask2).sum()
111 weight1 = org_solidity / new_solidity1
112 weight2 = org_solidity / new_solidity2
113 return {
'weight': min([weight1, weight2])}
117 """Callback called before merging two nodes of a solidity graph."""
118 new_mask = np.logical_or(graph.node[src][
'mask'], graph.node[dst][
'mask'])
119 graph.node[dst][
'mask'] = new_mask
120 graph.node[dst][
'solidity'] = \
121 1. * np.sum(new_mask) / np.sum(convex_hull_image(new_mask))
129 labels = slic(img, n_segments=n_segments, compactness=compactness)
131 n_labels = len(np.unique(labels))
133 mask = ndi.binary_closing(mask, structure=np.ones((3, 3)), iterations=1)
134 except IndexError
as e:
137 labels[mask == 0] = 0
138 if len(np.unique(labels)) < n_labels - 2:
139 sys.stderr.write(
'WARNING: number of label differs after masking.'
140 ' Maybe this is not good for RAG construction.\n')
145 closed_mask = ndi.binary_closing(
146 mask, structure=np.ones((3, 3)), iterations=8)
147 roi = ndi.find_objects(closed_mask, max_label=1)[0]
158 super(SolidityRagMerge, self).
__init__()
159 self.
pub = self.advertise(
'~output', Image, queue_size=5)
162 self.
pub_rag = self.advertise(
'~debug/rag', Image, queue_size=5)
163 self.
pub_slic = self.advertise(
'~debug/slic', Image, queue_size=5)
164 self.
pub_label = self.advertise(
'~debug/label_viz', Image,
172 self.
use_async = rospy.get_param(
'~approximate_sync',
False)
173 rospy.loginfo(
'~approximate_sync: {}'.format(self.
use_async))
175 sync = message_filters.ApproximateTimeSynchronizer(
176 [self.
sub, self.
sub_mask], queue_size=1000, slop=0.1)
180 sync.registerCallback(self.
sub_cb)
181 warn_no_remap(
'~input',
'~input/mask')
183 self.
sub = rospy.Subscriber(
'~input', Image, self.
sub_img_cb,
185 warn_no_remap(
'~input')
188 self.
sub.unregister()
193 self.
_apply(imgmsg, maskmsg)
199 bridge = cv_bridge.CvBridge()
200 img = bridge.imgmsg_to_cv2(imgmsg)
206 mask = np.ones(img.shape[:2], dtype=np.uint8)
208 n_segments=20, compactness=30)
211 labels = labels.astype(np.int32)
213 mask = bridge.imgmsg_to_cv2(maskmsg, desired_encoding=
'mono8')
214 mask = mask.reshape(mask.shape[:2])
217 roi_labels =
masked_slic(img=img[roi], mask=mask[roi],
218 n_segments=20, compactness=30)
219 if roi_labels
is None:
221 labels = np.zeros(mask.shape, dtype=np.int32)
222 labels[roi] = roi_labels
227 slic_labelmsg = bridge.cv2_to_imgmsg(labels)
228 slic_labelmsg.header = imgmsg.header
229 self.
pub_slic.publish(slic_labelmsg)
234 if LooseVersion(skimage.__version__) >=
'0.13.0':
235 fig, ax = plt.subplots(
236 figsize=(img.shape[1] * 0.01, img.shape[0] * 0.01))
237 show_rag(labels, g, img, ax=ax)
239 plt.subplots_adjust(0, 0, 1, 1)
241 w, h = fig.canvas.get_width_height()
242 rag_img = np.fromstring(
243 fig.canvas.tostring_rgb(), dtype=np.uint8)
244 rag_img.shape = (h, w, 3)
247 rag_img = draw_rag(labels, g, img)
248 rag_img = img_as_uint(rag_img)
249 rag_imgmsg = bridge.cv2_to_imgmsg(
250 rag_img.astype(np.uint8), encoding=
'rgb8')
251 rag_imgmsg.header = imgmsg.header
252 self.
pub_rag.publish(rag_imgmsg)
254 merged_labels = merge_hierarchical(
255 labels, g, thresh=1, rag_copy=
False,
257 merge_func=_solidity_merge_func,
258 weight_func=_solidity_weight_func)
260 merged_labels[mask == 0] = 0
261 merged_labelmsg = bridge.cv2_to_imgmsg(merged_labels.astype(np.int32))
262 merged_labelmsg.header = imgmsg.header
263 self.
pub.publish(merged_labelmsg)
266 out = (out * 255).astype(np.uint8)
267 out_msg = bridge.cv2_to_imgmsg(out, encoding=
'rgb8')
268 out_msg.header = imgmsg.header
272 if __name__ ==
'__main__':
273 rospy.init_node(
'solidity_rag_merge')