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