GatherDetectionResults.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright (C) 2009 Rosen Diankov
00004 # 
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #     http://www.apache.org/licenses/LICENSE-2.0
00009 # 
00010 # Unless required by applicable law or agreed to in writing, software
00011 # distributed under the License is distributed on an "AS IS" BASIS,
00012 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 # See the License for the specific language governing permissions and
00014 # limitations under the License. 
00015 from __future__ import with_statement # for python 2.5
00016 PKG = 'posedetectiondb' # this package name
00017 NAME = 'GatherDetectionResults'
00018 
00019 import roslib; roslib.load_manifest(PKG) 
00020 import os, sys, time, string, threading
00021 from optparse import OptionParser
00022 import numpy # nice to be able to explicitly call some functions
00023 from numpy import *
00024 
00025 import rospy
00026 import posedetection_msgs.msg
00027 
00028 from openravepy import *
00029 
00030 class VisibilityModel(metaclass.AutoReloader):
00031     def __init__(self,measurements=None,filename=None,symmetricplane=None,kinbodyfile=None):
00032         if measurements is not None:
00033             self.rawmeasurements = measurements
00034             
00035         if symmetricplane is not None:
00036             dists = dot(symmetricplane[0:3],transpose(self.rawmeasurements))+symmetricplane[3]
00037             self.rawmeasurements = r_[self.rawmeasurements,self.rawmeasurements-dot(reshape(2.0*dists,(dists.shape[0],1)),reshape(symmetricplane[0:3],(1,3)))]
00038 
00039         self.trimesh = None
00040         if kinbodyfile is not None:
00041             self.env = Environment()
00042             self.orobj = self.env.ReadKinBodyXMLFile(kinbodyfile)
00043             if self.orobj is None:
00044                 raise ValueError('failed to open %s openrave file'%kinbodyfile)
00045             self.env.AddKinBody(self.orobj)
00046             self.trimesh = self.env.Triangulate(self.orobj)
00047         self.measurements = self.rawmeasurements
00048     def CreateReducedModel(self,bandwidth=0.04,bandthresh=0.01,neighthresh=0.01,showdata=False,savefile=None):
00049         self.measurements,indices = self.Prune(self.rawmeasurements,100,neighthresh**2,1)
00050         uniformpoints,dists,pointscale = self.UniformlySampleSpace(bandwidth,bandthresh)
00051         if showdata:
00052             from enthought.mayavi import mlab
00053             mlab.figure(1,fgcolor=(0,0,0), bgcolor=(1,1,1))
00054             src = mlab.pipeline.scalar_field(dists)
00055             mlab.pipeline.iso_surface(src,contours=[0.01],opacity=0.1)
00056             mlab.pipeline.volume(mlab.pipeline.scalar_field(dists*500))
00057             v = pointscale[0]*self.trimesh.vertices+pointscale[1]
00058             mlab.triangular_mesh(v[:,0],v[:,1],v[:,2],self.trimesh.indices,color=(0,0,0.5))
00059         if savefile is None:
00060             savefile = self.getfilename()
00061         print 'saving measurements to %s'%savefile
00062         mkdir_recursive(os.path.split(savefile)[0])
00063         savetxt(savefile,uniformpoints,'%f')
00064     def getfilename(self):
00065         return os.path.join(self.env.GetHomeDirectory(),'kinbody.'+self.orobj.GetKinematicsGeometryHash(),'visibility.txt')
00066 
00067     def UniformlySampleSpace(self,bandwidth,bandthresh,delta=0.02):
00068         maxradius = sqrt(max(sum(self.measurements**2,1)))
00069         nsteps = floor(maxradius/delta)
00070         X,Y,Z = mgrid[-nsteps:nsteps,-nsteps:nsteps,-nsteps:nsteps]
00071         allpoints = c_[X.flat,Y.flat,Z.flat]*delta/bandwidth
00072         sampleinds = flatnonzero(sum(allpoints**2,1)<(maxradius/bandwidth)**2)
00073         samplepoints = allpoints[sampleinds,:]
00074         kdtree = pyANN.KDTree(self.measurements/bandwidth)
00075         sampledists = zeros(samplepoints.shape[0])
00076         goodpoints = []
00077         for i in xrange(samplepoints.shape[0]):
00078             neighs,dists,kball = kdtree.kFRSearchArray(samplepoints[i:(i+1),:],5.0**2,32,0.0001)
00079             sampledists[i] = sum(exp(-dists[neighs>=0]))
00080         uniformpoints = samplepoints[sampledists>bandthresh,:]*bandwidth
00081         alldists = zeros(prod(X.shape))
00082         alldists[sampleinds] = sampledists
00083         return uniformpoints,reshape(alldists,X.shape),array((1.0/delta,nsteps))
00084 
00085     def Prune(self,rawposes, nsize, thresh2, neighsize,giveupiters=100):
00086         """rawposes is Nx7"""
00087         iter = 1
00088         poses = array(rawposes)
00089         indices = range(poses.shape[0])
00090         N = poses.shape[0]
00091         nochange=0
00092         while N > nsize:
00093             ind = numpy.random.randint(N)
00094             g = poses[ind,:]
00095             # check g's neighbors
00096             d = sum((poses[0:N,:] - tile(g, (N,1)))**2,1)
00097             neigh = sum(d < thresh2)
00098             if neigh > neighsize:
00099                 # move to the last pose and resize
00100                 poses[ind,:] = poses[N-1,:]
00101                 indices[ind] = indices[N-1]
00102                 nochange=0
00103                 N -= 1
00104             nochange += 1
00105             iter += 1
00106             if iter > 5000 or nochange > giveupiters:
00107                 break
00108         return poses[0:N,:],indices[0:N]
00109 
00110 class OpenRAVEVisualizer(metaclass.AutoReloader):
00111     def __init__(self,kinbodyfile,automaticadd=True,measurementsfilename=None):
00112         self.automaticadd = automaticadd
00113         self.orenv = Environment()
00114         self.orenv.SetViewer('qtcoin')
00115         self.orobj = self.orenv.ReadKinBodyXMLFile(kinbodyfile)
00116         if self.orobj is None:
00117             raise ValueError('failed to open %s openrave file'%kinbodyfile)
00118         self.orenv.AddKinBody(self.orobj)
00119         self.objab = self.orobj.ComputeAABB()
00120         self.Tcamera = None
00121         self.lck = threading.Lock()
00122         self.camerahandle = None
00123         self.measurementhandles = []
00124         self.measurements = []
00125         if measurementsfilename is not None:
00126             f = open(measurementsfilename,'r')
00127             for l in f.readlines():
00128                 m = array([string.atof(s) for s in string.split(l)])
00129                 self.drawmeasurement(m)
00130                 self.measurements.append(m)
00131             f.close()
00132 
00133         self.sub_objdet = rospy.Subscriber("ObjectDetection", posedetection_msgs.msg.ObjectDetection,self.objdetcb, queue_size=1)
00134         rospy.init_node(NAME, anonymous=True)#,disable_signals=False)
00135 
00136     def __del__(self):
00137         self.sub_objdet.unregister()
00138         self.orenv.Destroy()
00139 
00140     def objdetcb(self,msg):
00141         newcamerahandle = None
00142         if len(msg.objects) > 0:
00143             q = msg.objects[0].pose.orientation
00144             t = msg.objects[0].pose.position
00145             with self.lck:
00146                 self.Tcamera = linalg.inv(matrixFromPose([q.w,q.x,q.y,q.z,t.x,t.y,t.z]))
00147 
00148             # draw in openrave environment
00149             sx = 0.02
00150             sy = 0.02
00151             camlocalpts = array(((sx,sy,0.05),(-sx,sy,0.05),(-sx,-sy,0.05),(sx,-sy,0.05),(sx,sy,0.05),(0,0,0),(-sx,sy,0.05),(0,0,0),(-sx,-sy,0.05),(0,0,0),(sx,-sy,0.05)))
00152             campts = dot(self.Tcamera,r_[transpose(camlocalpts),ones((1,camlocalpts.shape[0]))])
00153             self.camerahandle = self.orenv.drawlinestrip(transpose(campts[0:3,:]),2,array([0,0,1]))
00154         if self.automaticadd:
00155             self.addmeasurement()
00156 
00157     def addmeasurement(self):
00158         self.lck.acquire()
00159         Tcamera = self.Tcamera
00160         self.lck.release()
00161         dist = dot(Tcamera[0:3,2],self.objab.pos()-Tcamera[0:3,3:4])
00162         m = Tcamera[0:3,2]*dist
00163         self.drawmeasurement(m)
00164         self.measurements.append(m)
00165         print 'num measurements %d'%len(self.measurements)
00166 
00167     def drawmeasurement(self,m):
00168         dist = sqrt(sum(m**2))
00169         dir = m/dist
00170         p = self.objab.pos()-dist*dir
00171         self.measurementhandles.append(self.orenv.drawlinestrip(transpose(c_[p-0.02*dir,p+0.02*dir]),4,array((1,min(1,dist/1.0),0))))
00172 
00173     def savemeasurements(self,filename):
00174         self.lck.acquire()
00175         print 'saving measurements to %s'%filename
00176         savetxt(filename,self.measurements,'%f')
00177         self.lck.release()
00178 
00179 if __name__=='__main__':
00180     parser = OptionParser(description='Gather object detection transformations and filter and display them.')
00181     parser.add_option('--kinbodyfile',
00182                       action="store",type='string',dest='kinbodyfile',
00183                       help='OpenRAVE object file that represents the incoming object pose. Updates are show in the openrave window')
00184     parser.add_option('-s','--single',
00185                       action="store_true",dest='single',default=False,
00186                       help='If set, will wait for user input in order to add a measurement')
00187     parser.add_option('-f','--savefile',
00188                       action="store",dest="filename",
00189                       help='If specified, will save all recorded measurements to this file at exit time')
00190     parser.add_option('-m','--measurements',
00191                       action="store",dest="measurements",default=None,
00192                       help='If specified, will start with the current measurements file')
00193     (options, args) = parser.parse_args()
00194     if not options.kinbodyfile:
00195         print('Error: Need to specify an openrave kinbody file')
00196         sys.exit(1)
00197 
00198     visualizer = OpenRAVEVisualizer(options.kinbodyfile,measurementsfilename=options.measurements,automaticadd=not options.single)
00199     while True:
00200         cmd = raw_input('Enter command (q-quit and save,c-capture): ');
00201         if cmd == 'q':
00202             break
00203         elif cmd == 'c' and options.single:
00204             print 'adding measurement'
00205             visualizer.addmeasurement()
00206         else:
00207             print 'bad command',cmd
00208     if options.filename:
00209         visualizer.savemeasurements(options.filename)
00210 
00211 def test():
00212     "rosrun posedetectiondb GatherDetectionResults.py --kinbodyfile=scenes/cereal_frootloops.kinbody.xml -f test.txt ObjectDetection:=/CerealDetection"
00213     import GatherDetectionResults
00214     self = GatherDetectionResults.VisibilityModel(measurements=loadtxt('uroncha_raw.txt'),symmetricplane=array([1.0,0,0,0]),kinbodyfile='scenes/uroncha.kinbody.xml')
00215     self = GatherDetectionResults.VisibilityModel(measurements=loadtxt('peachjuice_raw.txt'),kinbodyfile='scenes/peachjuice.kinbody.xml')
00216     self.CreateReducedModel(bandwidth=0.03,bandthresh=0.02)


posedetectiondb
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Tue Jan 27 2015 11:53:11