Guest User

pure ctypes - opencv and libfreenect - multithreaded

a guest
May 8th, 2011
474
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 3.27 KB | None | 0 0
  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')
Add Comment
Please, Sign In to add comment