SHARE
TWEET

pure ctypes - opencv and libfreenect - multithreaded

a guest May 8th, 2011 456 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/python
  2. # updated may8th 2011
  3. import os, sys, ctypes, thread
  4. if '..' not in sys.path: sys.path.append( '..' )
  5. import rpythonic
  6. rpythonic.set_cache('../cache')
  7. freenect = rpythonic.module( 'libfreenect_sync' )
  8. assert freenect
  9. gui = rpythonic.module( 'highgui' )
  10. assert gui
  11. cv = rpythonic.module( 'cv', secondary=gui )
  12. assert cv
  13. import time
  14.  
  15. IPL_DEPTH_16S = 2147483664
  16. IPL_DEPTH_8S = 2147483656
  17.  
  18. lock = thread.allocate_lock()
  19.  
  20. class Threaded(object):
  21.         def start(self):
  22.                 self.active = True
  23.                 thread.start_new_thread( self.loop, () )
  24.  
  25.  
  26. class Kinect( Threaded ):
  27.         def __init__(self, ocv):
  28.                 self.active = False
  29.                 self.ocv = ocv
  30.                 self._lenpix = 480 * 640 * 2
  31.                 self.buffer_type = ctypes.c_void_p      #(ctypes.c_char * lenpix)
  32.                 self.buffer = self.buffer_type() #_ref = ctypes.cast(buff, ctypes.c_void_p)
  33.                 self.pointer = ctypes.pointer( self.buffer )
  34.                 print( 'setting leds' )
  35.                 freenect.sync_set_led( freenect.LED_YELLOW, 0 )
  36.  
  37.         def loop(self):
  38.                 print( 'starting kinect thread' )
  39.                 while self.active:
  40.                         status = freenect.sync_get_depth( ctypes.byref(self.pointer), 0, 0,
  41.                                 freenect.FREENECT_DEPTH_11BIT
  42.                         )
  43.                         lock.acquire()          # seems safe even without locking?
  44.                         cv.SetData( self.ocv.depth16raw, self.pointer, 640*2 )
  45.                         lock.release()
  46.                 freenect.sync_set_led( freenect.LED_OFF, 0 )
  47.                 freenect.sync_stop()
  48.                 print( 'exit kinect thread' )
  49.  
  50. class OpenCV( Threaded ):
  51.         def __init__(self):
  52.                 self.active = False
  53.                 self.depth16raw = cv.CreateImage((640,480), IPL_DEPTH_16S, 1)
  54.                 self.depth16 = cv.CreateImage((640,480), IPL_DEPTH_16S, 1)
  55.                 self.depth8 = cv.cvCreateImage((640,480), 8, 1)
  56.                 self.sweep_thresh = [ cv.cvCreateImage((640,480), 8, 1) for i in range(12) ]                   
  57.                 self.contours_image = cv.cvCreateImage((640,480), 8, 3)
  58.                 self.storage = cv.CreateMemStorage(0)
  59.                 cv.NamedWindow('depth', 1)
  60.                 cv.NamedWindow('contours', 1)
  61.                 cv.SetZero( self.depth16raw )
  62.  
  63.         def loop(self):
  64.                 print( 'starting opencv thread' )
  65.                 self.active = True
  66.                 while self.active:
  67.                         lock.acquire()
  68.                         cv.ConvertScale( self.depth16raw, self.depth8, 0.18, 0 )
  69.                         lock.release()
  70.                         ## thread free ##
  71.                         #cv.Smooth( self.depth16raw, self.depth16, cv.CV_BLUR, 7, 7, 0.1, 0.1 )
  72.                         cv.SetZero( self.contours_image )
  73.                         thresh = 100
  74.                         for i, img in enumerate(self.sweep_thresh):
  75.                                 cv.Threshold( self.depth8, img, thresh, 256, cv.CV_THRESH_BINARY_INV )
  76.                                 thresh += 4
  77.                                 seq = cv.CvSeq()
  78.                                 contours = ctypes.pointer( ctypes.pointer( seq ) )
  79.                                 cv.FindContours(img, self.storage, contours,
  80.                                         ctypes.sizeof( cv.Contour ), cv.CV_RETR_TREE, cv.CV_CHAIN_APPROX_SIMPLE, (0,0)
  81.                                 )
  82.                                 cv.DrawContours(
  83.                                         self.contours_image,
  84.                                         contours.contents,
  85.                                         (128+thresh,0,128-thresh),      # external color
  86.                                         (255,255,0),    # hole color
  87.                                         1, # max levels
  88.                                         2, # thickness
  89.                                         8, # linetype
  90.                                         (0, 0)
  91.                                 )
  92.  
  93.                         cv.ShowImage( 'depth', self.depth8 )
  94.                         cv.ShowImage( 'contours', self.contours_image )
  95.                         cv.WaitKey(1)
  96.  
  97.                 print( 'exit opencv thread' )
  98.  
  99. o = OpenCV()
  100. k = Kinect( o )
  101. k.start()               # thread 1
  102. time.sleep(1)
  103. o.start()               # thread 2
  104.  
  105.  
  106. while True:     # main thread
  107.         try:
  108.                 print( 'waiting' )
  109.                 time.sleep(10.0)
  110.         except:
  111.                 print( 'user exit' )
  112.                 o.active = False
  113.                 k.active = False
  114.                 break
  115. time.sleep(1)
  116.  
  117. print('threaded kinect test done')
RAW Paste Data
Top