bof_histogram_extractor.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 """
00004 Extract BoF histogram in realtime.
00005 """
00006 
00007 import gzip
00008 import cPickle as pickle
00009 from distutils.version import StrictVersion
00010 from pkg_resources import get_distribution
00011 
00012 import numpy as np
00013 from sklearn.preprocessing import normalize
00014 
00015 import cv_bridge
00016 from jsk_recognition_msgs.msg import VectorArray
00017 from jsk_recognition_utils import decompose_descriptors_with_label
00018 from jsk_topic_tools import ConnectionBasedTransport
00019 import message_filters
00020 from posedetection_msgs.msg import Feature0D
00021 from sensor_msgs.msg import Image
00022 import rospy
00023 
00024 
00025 class BoFHistogramExtractor(ConnectionBasedTransport):
00026     def __init__(self):
00027         super(BoFHistogramExtractor, self).__init__()
00028 
00029         self.queue_size = rospy.get_param('~queue_size', 10)
00030 
00031         # load bag of features
00032         rospy.loginfo('Loading BoF data')
00033         bof_data = rospy.get_param('~bof_data', None)
00034         if bof_data is None:
00035             quit()
00036         with gzip.open(bof_data, 'rb') as f:
00037             self.bof = pickle.load(f)
00038         if (StrictVersion(get_distribution('scikit-learn').version) >=
00039                 StrictVersion('0.17.0')):
00040             if 'n_jobs' not in self.bof.nn.__dict__:
00041                 # In scikit-learn>=0.17.0,
00042                 # sklearn.neighbors.NearestNeighbors needs 'n_jobs' attribute.
00043                 # https://github.com/jsk-ros-pkg/jsk_recognition/issues/1669
00044                 self.bof.nn.n_jobs = 1
00045 
00046         self._pub = self.advertise('~output', VectorArray, queue_size=1)
00047         rospy.loginfo('Initialized BoF histogram extractor')
00048 
00049     def subscribe(self):
00050         self._sub_feature = message_filters.Subscriber('~input', Feature0D)
00051         self._sub_label = message_filters.Subscriber('~input/label', Image)
00052         use_async = rospy.get_param('~approximate_sync', False)
00053         if use_async:
00054             slop = rospy.get_param('~slop', 0.1)
00055             sync = message_filters.ApproximateTimeSynchronizer(
00056                 [self._sub_feature, self._sub_label],
00057                 queue_size=self.queue_size, slop=slop)
00058         else:
00059             sync = message_filters.TimeSynchronizer(
00060                 [self._sub_feature, self._sub_label],
00061                 queue_size=self.queue_size)
00062         rospy.logdebug('~approximate_sync: {}'.format(use_async))
00063         sync.registerCallback(self._apply)
00064 
00065     def unsubscribe(self):
00066         self._sub_feature.unregister()
00067         self._sub_label.unregister()
00068 
00069     def _apply(self, feature_msg, label_msg):
00070         bridge = cv_bridge.CvBridge()
00071         label = bridge.imgmsg_to_cv2(label_msg)
00072         desc = np.array(feature_msg.descriptors)
00073         desc = desc.reshape((-1, feature_msg.descriptor_dim))
00074         pos = np.array(feature_msg.positions)
00075         pos = pos.reshape((-1, 2))
00076         if label.sum() == 0:
00077             rospy.logdebug('Skip image with only background label')
00078             return
00079         decomposed = decompose_descriptors_with_label(
00080             descriptors=desc, positions=pos, label_img=label,
00081             skip_zero_label=True)
00082         X = np.array(decomposed.values())
00083         if X.size == 0:
00084             return
00085         X = self.bof.transform(X)
00086         normalize(X, copy=False)
00087         self._pub.publish(
00088             VectorArray(
00089                 header=feature_msg.header,
00090                 vector_dim=X.shape[1],
00091                 data=X.reshape(-1),
00092             ))
00093 
00094 
00095 if __name__ == '__main__':
00096     rospy.init_node('bof_histogram_extractor')
00097     extractor = BoFHistogramExtractor()
00098     rospy.spin()


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Tue Jul 2 2019 19:41:07