pr2_playpen: hist_analyzer_tmp.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python  
00002 # Calculating and displaying 2D Hue-Saturation histogram of a color image
00003 import roslib
00004 roslib.load_manifest('pr2_playpen')
00005 import rospy
00006 from pr2_playpen.srv import * #this is for Train and Check
00007 import sys
00008 import cv
00009 import numpy as np
00010 import cPickle
00011 from collections import deque
00012 from std_msgs.msg import String
00013 from sensor_msgs.msg import Image
00014 from cv_bridge import CvBridge, CvBridgeError
00015 
00016 class HistAnalyzer:
00017 
00018     def __init__(self, background_noise, mask, topic='/playpen_kinect/rgb/image_color'):
00019         rospy.Subscriber(topic, Image, self.get_img)
00020         self.check = rospy.Service("playpen_check_success_hist", Check, self.serv_success)
00021         self.train_empty = rospy.Service("playpen_train_hist", Train, self.serv_train)
00022         self.background_noise = background_noise
00023         self.h_bins = 30
00024         self.s_bins = 32
00025         self.h_ranges = [0, 180]
00026         self.s_ranges = [0, 255]
00027         self.ranges = [self.h_ranges, self.s_ranges]
00028         self.hist = None
00029         self.mask = mask
00030         self.avg_noise = None
00031         self.online_img = None
00032         self.bridge = CvBridge()
00033         size = cv.GetSize(background_noise[0])
00034         self.IavgF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00035         self.IdiffF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00036         self.IprevF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00037         self.IhiF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00038         self.IlowF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00039         self.Ilow1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00040         self.Ilow2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00041         self.Ilow3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00042         self.Ihi1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00043         self.Ihi2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00044         self.Ihi3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00045         cv.Zero(self.IavgF)
00046         cv.Zero(self.IdiffF)
00047         cv.Zero(self.IprevF)
00048         cv.Zero(self.IhiF)
00049         cv.Zero(self.IlowF)
00050         self.Icount = 0.00001
00051         self.Iscratch = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00052         self.Iscratch2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00053         self.Igray1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00054         self.Igray2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00055         self.Igray3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1)
00056         self.Imaskt = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
00057         cv.Zero(self.Iscratch)
00058         cv.Zero(self.Iscratch2)
00059         self.first = 1
00060 
00061     def accumulateBackground(self, img):
00062         cv.CvtScale(img, self.Iscratch, 1, 0)
00063         if (not self.first):
00064             cv.Acc(self.Iscratch, self.IavgF)
00065             cv.AbsDiff(self.Iscratch, self.IprevF, self.Iscratch2)
00066             cv.Acc(self.Iscratch2, self.IdiffF)
00067             self.Icount += 1.0
00068         self.first = 0
00069         cv.Copy(self.Iscratch, self.IprevF)
00070 
00071     def setHighThresh(self, thresh):
00072         cv.ConvertScale(self.IdiffF, self.Iscratch, thresh)
00073         cv.Add(self.Iscratch, self.IavgF, self.IhiF)
00074         #cv.Split(self.IhiF, self.Ihi1, self.Ihi2, self.Ihi3, None)
00075 
00076     def setLowThresh(self, thresh):
00077         cv.ConvertScale(self.IdiffF, self.Iscratch, thresh)
00078         cv.Sub(self.IavgF, self.Iscratch, self.IlowF)
00079         #cv.Split(self.IlowF, self.Ilow1, self.Ilow2, self.Ilow3, None)
00080 
00081 
00082     def createModelsfromStats(self):
00083         cv.ConvertScale(self.IavgF, self.IavgF, float(1.0/self.Icount))
00084         cv.ConvertScale(self.IdiffF, self.IdiffF, float(1.0/self.Icount))
00085 
00086         cv.AddS(self.IdiffF, cv.Scalar(1.0, 1.0, 1.0), self.IdiffF)
00087         self.setHighThresh(200.0)
00088         self.setLowThresh(200.0)
00089 
00090     def backgroundDiff(self, img, Imask):
00091         print "got into backgroundDiff"
00092         cv.CvtScale(img, self.Iscratch, 1, 0)
00093         #cv.Split(self.Iscratch, self.Igray1, self.Igray2, self.Igray3, None)
00094         #cv.InRange(self.Igray1, self.Ilow1, self.Ihi1, Imask)
00095         cv.InRange(self.Iscratch, self.IlowF, self.IhiF, Imask)
00096 
00097         # cv.InRange(self.Igray2, self.Ilow2, self.Ihi2, self.Imaskt)
00098         # cv.Or(Imask, self.Imaskt, Imask)
00099 
00100         # cv.InRange(self.Igray3, self.Ilow3, self.Ihi3, self.Imaskt)
00101         # cv.Or(Imask, self.Imaskt, Imask)
00102 
00103         cv.SubRS(Imask, 255, Imask)
00104         #cv.SaveImage('/home/mkillpack/Desktop/mask.png', Imask)
00105         #cv.Erode(Imask, Imask)
00106         return Imask
00107 
00108 
00109     def get_img(self, msg):
00110         try:
00111             self.online_img = self.bridge.imgmsg_to_cv(msg, "bgr8")
00112         except CvBridgeError, e:
00113             print e
00114 
00115     def serv_train(self, req):
00116         if req.expected== 'table':
00117             print 'great'
00118         num_samples = 0
00119         return TrainResponse(num_samples)
00120 
00121 
00122     def serv_success(self, req):
00123         result = "none"
00124         return CheckResponse(result)
00125 
00126 
00127     def calc_hist(self):
00128         self.hist = cv.CreateHist([self.h_bins, self.s_bins], cv.CV_HIST_ARRAY, self.ranges, 1)
00129         hsv = cv.CreateImage(cv.GetSize(self.background_noise[0]), 8, 3)
00130         h_plane = cv.CreateMat(self.background_noise[0].height, self.background_noise[0].width, cv.CV_8UC1)
00131         s_plane = cv.CreateMat(self.background_noise[0].height, self.background_noise[0].width, cv.CV_8UC1)
00132         for i in xrange(len(self.background_noise)):
00133             cv.CvtColor(self.background_noise[i], hsv, cv.CV_BGR2HSV)
00134             cv.Split(hsv, h_plane, s_plane, None, None)            
00135             planes = [h_plane, s_plane]#, s_plane, v_plane]
00136             cv.CalcHist([cv.GetImage(i) for i in planes], self.hist, True, self.mask)            
00137         #cv.NormalizeHist(self.hist, 10000.0)
00138 
00139     def check_for_hist(self):
00140         if self.hist == None:
00141             print "YOU CAN'T CALCULATE NOISE WITHOUT HIST MODEL OF TABLETOP"
00142             exit
00143 
00144     def calc_stats(self):
00145         cv.NamedWindow("noise", cv.CV_WINDOW_AUTOSIZE)
00146         cv.NamedWindow("img1_back", cv.CV_WINDOW_AUTOSIZE)
00147         cv.NamedWindow("img2_back", cv.CV_WINDOW_AUTOSIZE)
00148         self.check_for_hist()
00149         self.avg_noise = cv.CreateImage(cv.GetSize(self.background_noise[0]), 8, 1)
00150         cv.Zero(self.avg_noise)
00151 
00152         for i in xrange(len(self.background_noise)-1):
00153             cv.ShowImage("noise", self.avg_noise)
00154             back_proj_img1, hist1 = self.back_project_hs(self.background_noise[i])
00155             back_proj_img2, hist2 = self.back_project_hs(self.background_noise[i+1])
00156 
00157             self.accumulateBackground(back_proj_img1)
00158 
00159             cv.ShowImage("img1_back", back_proj_img1)
00160             cv.ShowImage("img2_back", back_proj_img2)
00161             scratch = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
00162             scratch2 = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
00163             
00164             # do something clever with ands ors and diffs 
00165             cv.Zero(scratch)
00166             cv.Zero(scratch2)
00167             cv.Sub(back_proj_img2, back_proj_img1, scratch2) #noise, but includes object if failed, 
00168 
00169             #cv.Sub(scratch2, self.avg_noise, scratch)            
00170             #cv.Or(self.avg_noise, scratch2, self.avg_noise)
00171 
00172             cv.Or(self.avg_noise, scratch2, self.avg_noise)
00173 
00174             cv.ShowImage("diff_back", scratch2)
00175             cv.ShowImage("diff_noise_scratch", scratch)
00176 
00177             cv.WaitKey(-1)
00178         self.createModelsfromStats()
00179         print self.Icount
00180         cv.NamedWindow("Ilow", cv.CV_WINDOW_AUTOSIZE)
00181         cv.NamedWindow("Ihi", cv.CV_WINDOW_AUTOSIZE)
00182         cv.NamedWindow("IavgF", cv.CV_WINDOW_AUTOSIZE)
00183 
00184         cv.ShowImage("Ihi", self.IhiF)
00185         cv.ShowImage("Ilow", self.IlowF)
00186         cv.ShowImage("IavgF", self.IavgF)
00187 
00188     def compare_imgs(self, img1, img2):
00189         back_proj_img, hist1 = self.back_project_hs(img1)
00190         back_proj_img2, hist2 = self.back_project_hs(img2)
00191 
00192         scratch = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
00193         scratch2 = cv.CreateImage(cv.GetSize(back_proj_img2), 8, 1)
00194         cv.Zero(scratch)
00195         cv.Zero(scratch2)
00196 
00197         #cv.Sub(back_proj_img, back_proj_img2, scratch2) #opposite noise, but excludes object 
00198         cv.Sub(back_proj_img2, back_proj_img, scratch2) #noise, but includes object if failed, 
00199         cv.Sub(scratch2, ha.avg_noise, scratch)
00200         
00201         return scratch
00202         
00203 
00204     def back_project_hs(self, img):
00205         self.check_for_hist()
00206         hsv = cv.CreateImage(cv.GetSize(img), 8, 3)
00207         scratch = cv.CreateImage(cv.GetSize(img), 8, 1)
00208         back_proj_img = cv.CreateImage(cv.GetSize(img), 8, 1)
00209         cv.CvtColor(img, hsv, cv.CV_BGR2HSV)
00210         h_plane_img = cv.CreateImage(cv.GetSize(img), 8, 1)
00211         s_plane_img = cv.CreateImage(cv.GetSize(img), 8, 1)
00212         cv.Split(hsv, h_plane_img, s_plane_img, None, None)            
00213         cv.CalcBackProject([h_plane_img, s_plane_img], back_proj_img, self.hist)
00214         #cv.Threshold(back_proj_img, back_proj_img, 200, 255, cv.CV_THRESH_BINARY)
00215         #cv.MorphologyEx(back_proj_img, back_proj_img, None, None, cv.CV_MOP_OPEN, 1)
00216         #cv.MorphologyEx(back_proj_img, back_proj_img, None, None, cv.CV_MOP_CLOSE, 2)    
00217         return back_proj_img, self.hist
00218 
00219 
00220 if __name__ == '__main__':
00221 
00222     import optparse
00223     p = optparse.OptionParser()
00224 
00225     p.add_option('--batch', action='store', dest='batch_folder', type="string",
00226                  default=None, help='check for success or failure in batch mode with already stored data')
00227     p.add_option('--online', action='store', dest='topic', type="string", 
00228                  default=None, help='tries to run success or failure detection online using histograms and backprojection')
00229 
00230     opt, args = p.parse_args()
00231 
00232 
00233     if opt.batch_folder != None:
00234         folder = opt.batch_folder+'/background_noise/'
00235         background_noise = deque() #[]
00236 
00237         cv.NamedWindow("Source", cv.CV_WINDOW_AUTOSIZE)
00238         cv.NamedWindow("final", cv.CV_WINDOW_AUTOSIZE)
00239 
00240         for i in xrange(130):
00241             try:
00242                 background_noise.append(cv.LoadImage(folder+'file'+str(i).zfill(3)+'.png'))
00243             except:
00244                 print "no file # ", i
00245         mask = cv.LoadImage(folder+'mask.png', 0)
00246 
00247         ha = HistAnalyzer(background_noise, mask)
00248         ha.calc_hist()
00249         ha.calc_stats()
00250 
00251         cv.ShowImage("Source", ha.avg_noise)
00252         cv.WaitKey(-1)
00253 
00254         back_sum_ls = deque() #[]
00255 
00256         for i in xrange(130):
00257             try:
00258                 img = cv.LoadImage(folder+'file'+str(i).zfill(3)+'.png')
00259                 result = ha.compare_imgs(img, ha.background_noise[0])
00260                 back_sum_ls.append(float(cv.Sum(result)[0]))
00261             except:
00262                 print "no training file # ", i
00263 
00264         avg = np.mean(back_sum_ls)
00265         std = np.std(back_sum_ls)
00266 
00267         print "avg and std are :", avg, std
00268 
00269         n = 0
00270         sum_val = 0
00271 
00272         for i in xrange(9):
00273             file_h = open(opt.batch_folder+'/object'+str(i).zfill(3)+'.pkl', 'r')
00274             res_dict = cPickle.load(file_h)
00275             for j in xrange(len(res_dict['success'])):
00276                 try:
00277                     #check for object before starting ...##########################
00278                     img = cv.LoadImageM(opt.batch_folder+'/object'+str(i).zfill(3)+'_try'+str(j).zfill(3)+'_before_pr2.png')
00279                     result = ha.compare_imgs(img, ha.background_noise[-1])
00280 
00281                     Imask = cv.CreateImage(cv.GetSize(ha.background_noise[0]), cv.IPL_DEPTH_8U, 1)
00282                     cv.Zero(Imask)
00283                     cv.Zero(ha.Imaskt)
00284                     back_img, hist = ha.back_project_hs(img)
00285                     Imask = ha.backgroundDiff(back_img, Imask)
00286 
00287                     cv.MorphologyEx(Imask, Imask, None, None, cv.CV_MOP_OPEN, 1)
00288                     cv.MorphologyEx(Imask, Imask, None, None, cv.CV_MOP_CLOSE, 2)    
00289 
00290                     cv.ShowImage("final", Imask)
00291                     cv.WaitKey(-1)
00292 
00293                     is_object = False
00294                     loc_sum = float(cv.Sum(result)[0])
00295                     if loc_sum < avg+5*std:
00296                         res_dict['success'][j] = None
00297                         print "no object to start with!?"
00298                     else:
00299                         is_object = True
00300                         print "there's an object let's check for success ..."
00301 
00302 
00303                     #check for success if object was in workspace to start with
00304                     if is_object == True:
00305                         img = cv.LoadImageM(opt.batch_folder+'/object'+str(i).zfill(3)+'_try'+str(j).zfill(3)+'_after_pr2.png')
00306                         result2 = ha.compare_imgs(img, ha.background_noise[-1])
00307                         n = n+1
00308                         sum_val = sum_val + float(cv.Sum(result2)[0])
00309 
00310                         #print "here's the sum :", cv.Sum(result2)[0]
00311                         cv.ShowImage("Source", img)
00312                         cv.ShowImage("final", result2)
00313                         Imask = cv.CreateImage(cv.GetSize(ha.background_noise[0]), cv.IPL_DEPTH_8U, 1)
00314                         cv.Zero(Imask)
00315                         cv.Zero(ha.Imaskt)
00316                         Imask = ha.backgroundDiff(ha.back_project_hs(img), Imask)
00317 
00318                         cv.ShowImage("final", Imask)
00319                         cv.WaitKey(-1)
00320 
00321                         loc_sum = float(cv.Sum(result2)[0])
00322                         if loc_sum < avg+5*std:
00323                             res_dict['success'][j] = True
00324                             print "success ! \t:", loc_sum, "\t compared to \t", avg, 5*std
00325                             #ha.background_noise.popleft()
00326                             #ha.background_noise.append(img)
00327                         else:
00328                             res_dict['success'][j] = False
00329                             print "epic fail ! \t:", loc_sum, "\t compared to \t", avg, 5*std
00330 
00331                 except:
00332                     print "problem in try statement"
00333             file_h.close()
00334             try:
00335                 os.system('rm '+opt.batch_folder+'/object'+str(i).zfill(3)+'.pkl')
00336             except:
00337                 print "PROBLEM DELETING OLD FILE..., it didn't exist"
00338                 exit
00339             file_h2 =  open(opt.batch_folder+'/object'+str(i).zfill(3)+'.pkl', 'w')
00340             cPickle.dump(res_dict, file_h2)
00341             file_h2.close()
00342             print "recalculating hist and noise..."
00343             #ha.calc_hist()
00344             #ha.calc_stats()
00345             print "done!"
00346 
00347         print "average error for objects present :", sum_val/n
00348 
00349     elif opt.topic != None:
00350         pass
00351