#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
import os
import gzip
import cPickle as pickle

import numpy as np
import yaml

import rospy
from posedetection_msgs.msg import ImageFeature0D
from jsk_2014_picking_challenge.srv import ObjectMatch, ObjectMatchResponse

from sift_matcher_oneimg import SiftMatcherOneImg


class SiftMatcher(object):
    def __init__(self):
        # load object list
        dirname = os.path.dirname(os.path.abspath(__file__))
        ymlfile = os.path.join(dirname, '../data/object_list.yml')
        self.object_list = yaml.load(open(ymlfile))

        rospy.loginfo('Loading sift data')
        self.all_siftdata = self._load_all_siftdata()

        rospy.Service('/semi/sift_matcher', ObjectMatch, self._cb_matcher)
        sub_imgfeature = rospy.Subscriber('/ImageFeature0D', ImageFeature0D,
                                          self._cb_imgfeature)
        rospy.loginfo('Ready to reseive match request')

    def _cb_matcher(self, req):
        probs = self._get_object_probability(req.objects)
        return ObjectMatchResponse(probabilities=probs)

    def _cb_imgfeature(self, msg):
        self.query_features = msg.features

    def _get_object_probability(self, obj_names):
        """Get object match probabilities"""
        query_features = self.query_features
        n_matches = []
        for obj_name in obj_names:
            if obj_name not in self.object_list:
                n_matches.append(0)
                continue
            # find best match in train features
            siftdata = self.all_siftdata.get(obj_name, None)
            if siftdata is None:  # does not exists data file
                n_matches.append(0)
                continue
            train_matches = []
            for train_des in siftdata['descriptors']:
                matches = SiftMatcherOneImg.find_match(
                    query_features.descriptors, train_des)
                train_matches.append(len(matches))
            n_matches.append(max(train_matches))  # best match
        n_matches = np.array(n_matches)
        return n_matches / n_matches.max()

    def _load_all_siftdata(self):
        """Load sift data of all objects"""
        object_list = self.object_list
        all_siftdata = {obj_name: self.load_siftdata(obj_name)
                        for obj_name in object_list}
        return all_siftdata

    @staticmethod
    def load_siftdata(obj_name):
        """Load sift data from pkl file"""
        dirname = os.path.dirname(os.path.abspath('__file__'))
        datafile = os.path.join(dirname, '../data/siftdata',
            obj_name+'.pkl.gz')
        if not os.path.exists(datafile):
            return  # does not exists
        with gzip.open(datafile, 'rb') as f:
            return pickle.load(f)


def main():
    rospy.init_node('sift_matcher')
    sm = SiftMatcher()
    rospy.spin()


if __name__ == '__main__':
    main()


@@ -571,25 +571,24 @@
 lfile))%0A
-%0A
         
 rospy.lo
@@ -583,96 +583,32 @@
     
-rospy.loginfo('Loading 
+self.
 sift
- 
 data
-')%0A        self.all_siftdata = self._load_all_siftdata()
+_cache = %7B%7D
 %0A%0A  
@@ -680,16 +680,82 @@
 atcher)%0A
+        rospy.wait_for_message('/ImageFeature0D', ImageFeature0D)%0A
         
@@ -890,96 +890,95 @@
 re)%0A
-   
+%0A
     
- rospy.loginfo('Ready to reseive match request')%0A%0A    def _cb_matcher(self, req):
+def _cb_matcher(self, req):%0A        %22%22%22Callback function for sift match request%22%22%22
 %0A   
@@ -1124,16 +1124,88 @@
 , msg):%0A
+        %22%22%22Callback function of Subscribers to listen ImageFeature0D%22%22%22%0A
         
@@ -1624,19 +1624,23 @@
  = self.
-all
+_handle
 _siftdat
@@ -1644,12 +1644,14 @@
 data
-.get
+_cache
 (obj
@@ -1659,14 +1659,8 @@
 name
-, None
 )%0A  
@@ -2094,16 +2094,17 @@
 t match%0A
+%0A
         
@@ -2131,24 +2131,119 @@
 (n_matches)%0A
+        if n_matches.max() == 0:%0A            return np.zeros(len(n_matches))%0A        else:%0A    
         retu
@@ -2287,16 +2287,14 @@
 ef _
-load_all
+handle
 _sif
@@ -2298,21 +2298,37 @@
 siftdata
-(self
+_cache(self, obj_name
 ):%0A     
@@ -2325,38 +2325,33 @@
 me):%0A        %22%22%22
-Load s
+S
 ift data of all 
@@ -2347,180 +2347,557 @@
 ata 
-of all objects%22%22%22%0A        object_list = self.object_list%0A        all_siftdata = %7Bobj_name: self.load_siftdata(obj_name)%0A                        for obj_name in object_list%7D
+cache handler%22%22%22%0A        siftdata_cache = self.siftdata_cache%0A        if obj_name in siftdata_cache:%0A            return siftdata_cache%5Bobj_name%5D%0A%0A        rospy.loginfo('Loading siftdata: %7Bobj%7D'.format(obj=obj_name))%0A        siftdata = self.load_siftdata(obj_name)%0A        if siftdata is None:%0A            return%0A%0A        if len(siftdata_cache) %3E= 3:  # cache size is 3%0A            rm = np.random.choice(siftdata_cache.keys())%0A            del siftdata_cache%5Brm%5D%0A        siftdata_cache%5Bobj_name%5D = siftdata%0A        self.siftdata_cache = siftdata_cache
 %0A   
@@ -2912,12 +2912,8 @@
 urn 
-all_
 sift

add cache handler #132