4 Extract BoF histogram in realtime. 8 import cPickle
as pickle
9 from distutils.version
import StrictVersion
10 from pkg_resources
import get_distribution
13 from sklearn.preprocessing
import normalize
16 from jsk_recognition_msgs.msg
import VectorArray
17 from jsk_recognition_utils
import decompose_descriptors_with_label
18 from jsk_topic_tools
import ConnectionBasedTransport
19 import message_filters
20 from posedetection_msgs.msg
import Feature0D
21 from sensor_msgs.msg
import Image
27 super(BoFHistogramExtractor, self).
__init__()
32 rospy.loginfo(
'Loading BoF data')
33 bof_data = rospy.get_param(
'~bof_data',
None)
36 with gzip.open(bof_data,
'rb')
as f:
37 self.
bof = pickle.load(f)
38 if (StrictVersion(get_distribution(
'scikit-learn').version) >=
39 StrictVersion(
'0.17.0')):
40 if 'n_jobs' not in self.bof.nn.__dict__:
44 self.bof.nn.n_jobs = 1
46 self.
_pub = self.advertise(
'~output', VectorArray, queue_size=1)
47 rospy.loginfo(
'Initialized BoF histogram extractor')
52 use_async = rospy.get_param(
'~approximate_sync',
False)
54 slop = rospy.get_param(
'~slop', 0.1)
55 sync = message_filters.ApproximateTimeSynchronizer(
62 rospy.logdebug(
'~approximate_sync: {}'.format(use_async))
63 sync.registerCallback(self.
_apply)
66 self._sub_feature.unregister()
67 self._sub_label.unregister()
69 def _apply(self, feature_msg, label_msg):
70 bridge = cv_bridge.CvBridge()
71 label = bridge.imgmsg_to_cv2(label_msg)
72 desc = np.array(feature_msg.descriptors)
73 desc = desc.reshape((-1, feature_msg.descriptor_dim))
74 pos = np.array(feature_msg.positions)
75 pos = pos.reshape((-1, 2))
77 rospy.logdebug(
'Skip image with only background label')
79 decomposed = decompose_descriptors_with_label(
80 descriptors=desc, positions=pos, label_img=label,
82 X = np.array(decomposed.values())
85 X = self.bof.transform(X)
86 normalize(X, copy=
False)
89 header=feature_msg.header,
90 vector_dim=X.shape[1],
95 if __name__ ==
'__main__':
96 rospy.init_node(
'bof_histogram_extractor')