class ColorObjectMatcher(ObjectMatcher):
def __init__(self):
super(ColorObjectMatcher, self).__init__('/color_object_matcher')
rospy.Subscriber('~input', Image, self._cb_image)
self._pub_recog = rospy.Publisher('~output', ObjectRecognition,
queue_size=1)
self._pub_debug = rospy.Publisher(
'~debug', ColorHistogram, queue_size=1)
self.query_image = Image()
self.estimator = ColorHistogramFeatures()
self.estimator.load_data()
def _cb_image(self, msg):
""" Callback function fo Subscribers to listen sensor_msgs/Image """
self.query_image = msg
def predict_now(self):
query_image = self.query_image
object_list = get_object_list()
probs = self.match(object_list)
matched_idx = np.argmax(probs)
# prepare message
res = ObjectRecognition()
res.header.stamp = query_image.header.stamp
res.matched = object_list[matched_idx]
res.probability = probs[matched_idx]
res.candidates = object_list
res.probabilities = probs
return res
def match(self, obj_names):
stamp = rospy.Time.now()
while (self.query_image.header.stamp < stamp) or (self.query_image.height == 0):
rospy.sleep(0.3)
query_image = self.query_image
# convert image
bridge = cv_bridge.CvBridge()
input_image = bridge.imgmsg_to_cv2(query_image, 'rgb8')
hist = self.estimator.color_hist(input_image)
self._pub_debug.publish(
ColorHistogram(header=query_image.header, histogram=hist))
object_list = get_object_list()
obj_indices = [object_list.index(o) for o in obj_names]
obj_probs = self.estimator.predict(input_image)[0][obj_indices]
return obj_probs / obj_probs.sum()
def spin_once(self):
res = self.predict_now()
if res is None:
return
self._pub_recog.publish(res)
def spin(self):
rate = rospy.Rate(rospy.get_param('rate', 1))
while not rospy.is_shutdown():
self.spin_once()
rate.sleep()
class ColorObjectMatcher(ConnectionBasedTransport):
def __init__(self):
super(ColorObjectMatcher, self).__init__()
self._pub = self.advertise('~output', ClassificationResult,
queue_size=1)
self.estimator = ColorHistogramFeatures()
self.estimator.load_data()
def subscribe(self):
self.sub_img = message_filters.Subscriber('~input', Image)
self.sub_label = message_filters.Subscriber('~input/label', Image)
queue_size = rospy.get_param('~queue_size', 100)
if rospy.get_param('~approximate_sync', False):
sync = message_filters.ApproximateTimeSynchronizer(
[self.sub_img, self.sub_label], queue_size=queue_size,
slop=0.1)
else:
sync = message_filters.TimeSynchronizer(
[self.sub_img, self.sub_label], queue_size=queue_size)
sync.registerCallback(self._predict)
def unsubscribe(self):
self.sub_img.sub.unregister()
self.sub_label.sub.unregister()
def _predict(self, img_msg, label_msg):
# convert image
bridge = cv_bridge.CvBridge()
input_image = bridge.imgmsg_to_cv2(img_msg, 'rgb8')
input_label = bridge.imgmsg_to_cv2(label_msg)
# predict
region_imgs = []
for l in np.unique(input_label):
if l == 0: # bg_label
continue
mask = (input_label == l)
region = jsk_recognition_utils.bounding_rect_of_mask(
input_image, mask)
region_imgs.append(region)
y_proba = self.estimator.predict(region_imgs)
target_names = np.array(jsk_apc2015_common.get_object_list())
y_pred = np.argmax(y_proba, axis=-1)
label_proba = [p[i] for p, i in zip(y_proba, y_pred)]
# prepare message
res = ClassificationResult()
res.header = img_msg.header
res.labels = y_pred
res.label_names = target_names[y_pred]
res.label_proba = label_proba
res.probabilities = y_proba.reshape(-1)
res.classifier = '<jsk_2015_05_baxter_apc.ColorHistogramFeatures>'
res.target_names = target_names
self._pub.publish(res)
def __init__(self):
super(ColorObjectMatcher, self).__init__('/color_object_matcher')
rospy.Subscriber('~input', Image, self._cb_image)
self._pub_recog = rospy.Publisher('~output', ObjectRecognition,
queue_size=1)
self._pub_debug = rospy.Publisher(
'~debug', ColorHistogram, queue_size=1)
self.query_image = Image()
self.estimator = ColorHistogramFeatures()
self.estimator.load_data()
def __init__(self):
super(ColorObjectMatcher, self).__init__('/color_object_matcher')
rospy.Subscriber('~input', Image, self._cb_image)
self._pub_recog = rospy.Publisher('~output', ObjectRecognition,
queue_size=1)
self.query_image = Image()
self.estimator = ColorHistogramFeatures()
self.estimator.load_data()
def __init__(self):
super(ColorObjectMatcher, self).__init__()
self._pub = self.advertise('~output', ClassificationResult,
queue_size=1)
self.estimator = ColorHistogramFeatures()
self.estimator.load_data()
