├── .gitignore ├── README.markdown ├── calibkinect.py ├── demo_freenect.py ├── demo_pclview.py └── pykinectwindow.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | build 3 | _pyxbld 4 | *.DS_Store -------------------------------------------------------------------------------- /README.markdown: -------------------------------------------------------------------------------- 1 | This is a grand unifying demo of the python scientific computing environments. It lets you 2 | tinker with kinect data in OpenCV, OpenGL, and Matplotlib, all at the same time! 3 | 4 | Installation 5 | ------------ 6 | 1. You need to have installed: IPython, Matplotlib, OpenCV 2.1, PyOpengl, wxPython 7 | 2. Build the latest version of libfreenect. 8 | https://github.com/openkinect/libfreenect 9 | 10 | 3. Build and install the python wrappers for libfreenect 11 | 12 | cd libfreenect/wrappers/python 13 | python setup.py install 14 | 15 | 4. Download the latest version of this project 16 | 17 | git clone https://github.com/amiller/libfreenect-goodies.git 18 | cd pykinect 19 | 20 | 5. Test that python can find libfreenect by running: 21 | 22 | python demo_freenect.py 23 | 24 | 25 | Usage instructions 26 | ------------------ 27 | 28 | 0. Please run this script using: 29 | 30 | ipython -pylab -wthread demo_pykinect.py 31 | 32 | 1. You should see an opengl window pop up with a preview of a point cloud. You can pan and 33 | zoom with the mouse. Run the following commands: 34 | 35 | update() # Grabs one frame from kinect and updates the point cloud 36 | update_on() # Grabs frames from kinect on a thread - 3d Video! (might be slow!) 37 | update_off() # Stops the update thread 38 | 39 | 2. You can also use opencv: 40 | 41 | loopcv() # Grab frames and display them as a cv image 42 | (ctrl+c to break) 43 | 44 | 3. You can also use matplotlib: 45 | 46 | imshow(depth) 47 | 48 | 4. Most importantly, you can reload any of the code without pausing or destroying your 49 | python instance: 50 | 51 | %run -i demo_pykinect.py 52 | 53 | Try changing some of the code, like the downsampling factor (search: downsampling) 54 | or the point size (search: `GL_POINT_SIZE`) and update the code without quitting python. 55 | 56 | This is an ideal environment for developing 3D point cloud algorithms and visualizations. 57 | All of your tools are right at hand. 58 | 59 | **Note to MATLAB users:** 60 | Yes, MATLAB already does most of this... there are plenty of reasons to prefer python, 61 | one of which is access to OpenGL drawing commands - scatter3 isn't adequate! 62 | -------------------------------------------------------------------------------- /calibkinect.py: -------------------------------------------------------------------------------- 1 | """ 2 | These are some functions to help work with kinect camera calibration and projective 3 | geometry. 4 | 5 | Tasks: 6 | - Convert the kinect depth image to a metric 3D point cloud 7 | - Convert the 3D point cloud to texture coordinates in the RGB image 8 | 9 | Notes about the coordinate systems: 10 | There are three coordinate systems to worry about. 11 | 1. Kinect depth image: 12 | u,v,depth 13 | u and v are image coordinates, (0,0) is the top left corner of the image 14 | (640,480) is the bottom right corner of the image 15 | depth is the raw 11-bit image from the kinect, where 0 is infinitely far away 16 | and larger numbers are closer to the camera 17 | (2047 indicates an error pixel) 18 | 19 | 2. Kinect rgb image: 20 | u,v 21 | u and v are image coordinates (0,0) is the top left corner 22 | (640,480) is the bottom right corner 23 | 24 | 3. XYZ world coordinates: 25 | x,y,z 26 | The 3D world coordinates, in meters, relative to the depth camera. 27 | (0,0,0) is the camera center. 28 | Negative Z values are in front of the camera, and the positive Z direction points 29 | towards the camera. 30 | The X axis points to the right, and the Y axis points up. This is the standard 31 | right-handed coordinate system used by OpenGL. 32 | 33 | 34 | """ 35 | import numpy as np 36 | 37 | 38 | def depth2xyzuv(depth, u=None, v=None): 39 | """ 40 | Return a point cloud, an Nx3 array, made by projecting the kinect depth map 41 | through intrinsic / extrinsic calibration matrices 42 | Parameters: 43 | depth - comes directly from the kinect 44 | u,v - are image coordinates, same size as depth (default is the original image) 45 | Returns: 46 | xyz - 3D world coordinates in meters (Nx3) 47 | uv - image coordinates for the RGB image (Nx3) 48 | 49 | You can provide only a portion of the depth image, or a downsampled version of 50 | the depth image if you want; just make sure to provide the correct coordinates 51 | in the u,v arguments. 52 | 53 | Example: 54 | # This downsamples the depth image by 2 and then projects to metric point cloud 55 | u,v = mgrid[:480:2,:640:2] 56 | xyz,uv = depth2xyzuv(freenect.sync_get_depth()[::2,::2], u, v) 57 | 58 | # This projects only a small region of interest in the upper corner of the depth image 59 | u,v = mgrid[10:120,50:80] 60 | xyz,uv = depth2xyzuv(freenect.sync_get_depth()[v,u], u, v) 61 | """ 62 | if u is None or v is None: 63 | u,v = np.mgrid[:480,:640] 64 | 65 | # Build a 3xN matrix of the d,u,v data 66 | C = np.vstack((u.flatten(), v.flatten(), depth.flatten(), 0*u.flatten()+1)) 67 | 68 | # Project the duv matrix into xyz using xyz_matrix() 69 | X,Y,Z,W = np.dot(xyz_matrix(),C) 70 | X,Y,Z = X/W, Y/W, Z/W 71 | xyz = np.vstack((X,Y,Z)).transpose() 72 | xyz = xyz[Z<0,:] 73 | 74 | # Project the duv matrix into U,V rgb coordinates using rgb_matrix() and xyz_matrix() 75 | U,V,_,W = np.dot(np.dot(uv_matrix(), xyz_matrix()),C) 76 | U,V = U/W, V/W 77 | uv = np.vstack((U,V)).transpose() 78 | uv = uv[Z<0,:] 79 | 80 | # Return both the XYZ coordinates and the UV coordinates 81 | return xyz, uv 82 | 83 | 84 | 85 | def uv_matrix(): 86 | """ 87 | Returns a matrix you can use to project XYZ coordinates (in meters) into 88 | U,V coordinates in the kinect RGB image 89 | """ 90 | rot = np.array([[ 9.99846e-01, -1.26353e-03, 1.74872e-02], 91 | [-1.4779096e-03, -9.999238e-01, 1.225138e-02], 92 | [1.747042e-02, -1.227534e-02, -9.99772e-01]]) 93 | trans = np.array([[1.9985e-02, -7.44237e-04,-1.0916736e-02]]) 94 | m = np.hstack((rot, -trans.transpose())) 95 | m = np.vstack((m, np.array([[0,0,0,1]]))) 96 | KK = np.array([[529.2, 0, 329, 0], 97 | [0, 525.6, 267.5, 0], 98 | [0, 0, 0, 1], 99 | [0, 0, 1, 0]]) 100 | m = np.dot(KK, (m)) 101 | return m 102 | 103 | def xyz_matrix(): 104 | fx = 594.21 105 | fy = 591.04 106 | a = -0.0030711 107 | b = 3.3309495 108 | cx = 339.5 109 | cy = 242.7 110 | mat = np.array([[1/fx, 0, 0, -cx/fx], 111 | [0, -1/fy, 0, cy/fy], 112 | [0, 0, 0, -1], 113 | [0, 0, a, b]]) 114 | return mat -------------------------------------------------------------------------------- /demo_freenect.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from freenect import sync_get_depth as get_depth, sync_get_video as get_video 3 | import cv 4 | import numpy as np 5 | 6 | def doloop(): 7 | global depth, rgb 8 | while True: 9 | # Get a fresh frame 10 | (depth,_), (rgb,_) = get_depth(), get_video() 11 | 12 | # Build a two panel color image 13 | d3 = np.dstack((depth,depth,depth)).astype(np.uint8) 14 | da = np.hstack((d3,rgb)) 15 | 16 | # Simple Downsample 17 | cv.ShowImage('both',np.array(da[::2,::2,::-1])) 18 | cv.WaitKey(5) 19 | 20 | doloop() 21 | 22 | """ 23 | IPython usage: 24 | ipython 25 | [1]: run -i demo_freenect 26 | # (to interrupt the loop) 27 | [2]: %timeit -n100 get_depth(), get_rgb() # profile the kinect capture 28 | 29 | """ 30 | 31 | -------------------------------------------------------------------------------- /demo_pclview.py: -------------------------------------------------------------------------------- 1 | import pykinectwindow as wxwindow 2 | import numpy as np 3 | import pylab 4 | from OpenGL.GL import * 5 | from OpenGL.GLU import * 6 | import time 7 | import freenect 8 | import calibkinect 9 | 10 | 11 | # I probably need more help with these! 12 | try: 13 | TEXTURE_TARGET = GL_TEXTURE_RECTANGLE 14 | except: 15 | TEXTURE_TARGET = GL_TEXTURE_RECTANGLE_ARB 16 | 17 | 18 | if not 'win' in globals(): win = wxwindow.Window(size=(640,480)) 19 | 20 | def refresh(): win.Refresh() 21 | 22 | if not 'rotangles' in globals(): rotangles = [0,0] 23 | if not 'zoomdist' in globals(): zoomdist = 1 24 | if not 'projpts' in globals(): projpts = (None, None) 25 | if not 'rgb' in globals(): rgb = None 26 | 27 | def create_texture(): 28 | global rgbtex 29 | rgbtex = glGenTextures(1) 30 | glBindTexture(TEXTURE_TARGET, rgbtex) 31 | glTexImage2D(TEXTURE_TARGET,0,GL_RGB,640,480,0,GL_RGB,GL_UNSIGNED_BYTE,None) 32 | 33 | 34 | if not '_mpos' in globals(): _mpos = None 35 | @win.eventx 36 | def EVT_LEFT_DOWN(event): 37 | global _mpos 38 | _mpos = event.Position 39 | 40 | @win.eventx 41 | def EVT_LEFT_UP(event): 42 | global _mpos 43 | _mpos = None 44 | 45 | @win.eventx 46 | def EVT_MOTION(event): 47 | global _mpos 48 | if event.LeftIsDown(): 49 | if _mpos: 50 | (x,y),(mx,my) = event.Position,_mpos 51 | rotangles[0] += y-my 52 | rotangles[1] += x-mx 53 | refresh() 54 | _mpos = event.Position 55 | 56 | 57 | @win.eventx 58 | def EVT_MOUSEWHEEL(event): 59 | global zoomdist 60 | dy = event.WheelRotation 61 | zoomdist *= np.power(0.95, -dy) 62 | refresh() 63 | 64 | 65 | clearcolor = [0,0,0,0] 66 | @win.event 67 | def on_draw(): 68 | if not 'rgbtex' in globals(): 69 | create_texture() 70 | 71 | xyz, uv = projpts 72 | if xyz is None: return 73 | 74 | if not rgb is None: 75 | rgb_ = (rgb.astype(np.float32) * 4 + 70).clip(0,255).astype(np.uint8) 76 | glBindTexture(TEXTURE_TARGET, rgbtex) 77 | glTexSubImage2D(TEXTURE_TARGET, 0, 0, 0, 640, 480, GL_RGB, GL_UNSIGNED_BYTE, rgb_); 78 | 79 | glClearColor(*clearcolor) 80 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) 81 | glEnable(GL_DEPTH_TEST) 82 | 83 | # flush that stack in case it's broken from earlier 84 | glPushMatrix() 85 | 86 | glMatrixMode(GL_PROJECTION) 87 | glLoadIdentity() 88 | gluPerspective(60, 4/3., 0.3, 200) 89 | 90 | glMatrixMode(GL_MODELVIEW) 91 | glLoadIdentity() 92 | 93 | def mouse_rotate(xAngle, yAngle, zAngle): 94 | glRotatef(xAngle, 1.0, 0.0, 0.0); 95 | glRotatef(yAngle, 0.0, 1.0, 0.0); 96 | glRotatef(zAngle, 0.0, 0.0, 1.0); 97 | glScale(zoomdist,zoomdist,1) 98 | glTranslate(0, 0,-3.5) 99 | mouse_rotate(rotangles[0], rotangles[1], 0); 100 | glTranslate(0,0,1.5) 101 | #glTranslate(0, 0,-1) 102 | 103 | # Draw some axes 104 | if 0: 105 | glBegin(GL_LINES) 106 | glColor3f(1,0,0); glVertex3f(0,0,0); glVertex3f(1,0,0) 107 | glColor3f(0,1,0); glVertex3f(0,0,0); glVertex3f(0,1,0) 108 | glColor3f(0,0,1); glVertex3f(0,0,0); glVertex3f(0,0,1) 109 | glEnd() 110 | 111 | # We can either project the points ourselves, or embed it in the opengl matrix 112 | if 0: 113 | dec = 4 114 | v,u = mgrid[:480,:640].astype(np.uint16) 115 | points = np.vstack((u[::dec,::dec].flatten(), 116 | v[::dec,::dec].flatten(), 117 | depth[::dec,::dec].flatten())).transpose() 118 | points = points[points[:,2]<2047,:] 119 | 120 | glMatrixMode(GL_TEXTURE) 121 | glLoadIdentity() 122 | glMultMatrixf(calibkinect.uv_matrix().transpose()) 123 | glMultMatrixf(calibkinect.xyz_matrix().transpose()) 124 | glTexCoordPointers(np.array(points)) 125 | 126 | glMatrixMode(GL_MODELVIEW) 127 | glPushMatrix() 128 | glMultMatrixf(calibkinect.xyz_matrix().transpose()) 129 | glVertexPointers(np.array(points)) 130 | else: 131 | glMatrixMode(GL_TEXTURE) 132 | glLoadIdentity() 133 | glMatrixMode(GL_MODELVIEW) 134 | glPushMatrix() 135 | glVertexPointerf(xyz) 136 | glTexCoordPointerf(uv) 137 | 138 | # Draw the points 139 | glPointSize(2) 140 | glEnableClientState(GL_VERTEX_ARRAY) 141 | glEnableClientState(GL_TEXTURE_COORD_ARRAY) 142 | glEnable(TEXTURE_TARGET) 143 | glColor3f(1,1,1) 144 | glDrawElementsui(GL_POINTS, np.array(range(xyz.shape[0]))) 145 | glDisableClientState(GL_VERTEX_ARRAY) 146 | glDisableClientState(GL_TEXTURE_COORD_ARRAY) 147 | glDisable(TEXTURE_TARGET) 148 | glPopMatrix() 149 | 150 | # 151 | if 0: 152 | inds = np.nonzero(xyz[:,2]>-0.55) 153 | glPointSize(10) 154 | glColor3f(0,1,1) 155 | glEnableClientState(GL_VERTEX_ARRAY) 156 | glDrawElementsui(GL_POINTS, np.array(inds)) 157 | glDisableClientState(GL_VERTEX_ARRAY) 158 | 159 | if 0: 160 | # Draw only the points in the near plane 161 | glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) 162 | glEnable(GL_BLEND) 163 | glColor(0.9,0.9,1.0,0.8) 164 | glPushMatrix() 165 | glTranslate(0,0,-0.55) 166 | glScale(0.6,0.6,1) 167 | glBegin(GL_QUADS) 168 | glVertex3f(-1,-1,0); glVertex3f( 1,-1,0); 169 | glVertex3f( 1, 1,0); glVertex3f(-1, 1,0); 170 | glEnd() 171 | glPopMatrix() 172 | glDisable(GL_BLEND) 173 | 174 | glPopMatrix() 175 | 176 | 177 | # A silly loop that shows you can busy the ipython thread while opengl runs 178 | def playcolors(): 179 | while 1: 180 | global clearcolor 181 | clearcolor = [np.random.random(),0,0,0] 182 | time.sleep(0.1) 183 | refresh() 184 | 185 | # Update the point cloud from the shell or from a background thread! 186 | 187 | def update(dt=0): 188 | global projpts, rgb, depth 189 | depth,_ = freenect.sync_get_depth() 190 | rgb,_ = freenect.sync_get_video() 191 | q = depth 192 | X,Y = np.meshgrid(range(640),range(480)) 193 | # YOU CAN CHANGE THIS AND RERUN THE PROGRAM! 194 | # Point cloud downsampling 195 | d = 4 196 | projpts = calibkinect.depth2xyzuv(q[::d,::d],X[::d,::d],Y[::d,::d]) 197 | refresh() 198 | 199 | def update_join(): 200 | update_on() 201 | try: 202 | _thread.join() 203 | except: 204 | update_off() 205 | 206 | def update_on(): 207 | global _updating 208 | if not '_updating' in globals(): _updating = False 209 | if _updating: return 210 | 211 | _updating = True 212 | from threading import Thread 213 | global _thread 214 | def _run(): 215 | while _updating: 216 | update() 217 | _thread = Thread(target=_run) 218 | _thread.start() 219 | 220 | def update_off(): 221 | global _updating 222 | _updating = False 223 | 224 | 225 | # Get frames in a loop and display with opencv 226 | def loopcv(): 227 | import cv 228 | while 1: 229 | cv.ShowImage('hi',get_depth().astype(np.uint8)) 230 | cv.WaitKey(10) 231 | 232 | update() 233 | #update_on() 234 | 235 | 236 | 237 | 238 | -------------------------------------------------------------------------------- /pykinectwindow.py: -------------------------------------------------------------------------------- 1 | # This module requires IPython to work! It is meant to be used from an IPython environment with: 2 | # ipython -wthread and -pylab 3 | # See demo_pykinect.py for an example 4 | 5 | import wx 6 | from wx import glcanvas 7 | from OpenGL.GL import * 8 | 9 | # Get the app ourselves so we can attach it to each window 10 | if not '__myapp' in wx.__dict__: 11 | wx.__myapp = wx.PySimpleApp() 12 | app = wx.__myapp 13 | 14 | class Window(wx.Frame): 15 | 16 | # wx events can be put in directly 17 | def eventx(self, target): 18 | def wrapper(*args, **kwargs): 19 | target(*args, **kwargs) 20 | self.canvas.Bind(wx.__dict__[target.__name__], wrapper) 21 | 22 | # Events special to this class, just add them this way 23 | def event(self, target): 24 | def wrapper(*args, **kwargs): 25 | target(*args, **kwargs) 26 | self.__dict__[target.__name__] = wrapper 27 | 28 | def _wrap(self, name, *args, **kwargs): 29 | try: 30 | self.__getattribute__(name) 31 | except AttributeError: 32 | pass 33 | else: 34 | self.__getattribute__(name)(*args, **kwargs) 35 | 36 | 37 | def __init__(self, title='WxWindow', id=-1, pos=wx.DefaultPosition, 38 | size=wx.DefaultSize, style=wx.DEFAULT_FRAME_STYLE, 39 | name='frame'): 40 | 41 | style = wx.DEFAULT_FRAME_STYLE | wx.NO_FULL_REPAINT_ON_RESIZE 42 | 43 | super(Window,self).__init__(None, id, title, pos, size, style, name) 44 | 45 | attribList = (glcanvas.WX_GL_RGBA, # RGBA 46 | glcanvas.WX_GL_DOUBLEBUFFER, # Double Buffered 47 | glcanvas.WX_GL_DEPTH_SIZE, 24) # 24 bit 48 | 49 | self.canvas = glcanvas.GLCanvas(self, attribList=attribList) 50 | 51 | self.canvas.Bind(wx.EVT_ERASE_BACKGROUND, self.processEraseBackgroundEvent) 52 | self.canvas.Bind(wx.EVT_SIZE, self.processSizeEvent) 53 | self.canvas.Bind(wx.EVT_PAINT, self.processPaintEvent) 54 | self.Show() 55 | 56 | def processEraseBackgroundEvent(self, event): 57 | """Process the erase background event.""" 58 | pass # Do nothing, to avoid flashing on MSWin 59 | 60 | def processSizeEvent(self, event): 61 | """Process the resize event.""" 62 | if self.canvas.GetContext(): 63 | # Make sure the frame is shown before calling SetCurrent. 64 | #self.Show() 65 | self.canvas.SetCurrent() 66 | size = self.GetClientSize() 67 | self.OnReshape(size.width, size.height) 68 | #self.canvas.Refresh(False) 69 | event.Skip() 70 | 71 | def processPaintEvent(self, event=None): 72 | """Process the drawing event.""" 73 | self.canvas.SetCurrent() 74 | self._wrap('on_draw') 75 | self.canvas.SwapBuffers() 76 | if event: event.Skip() 77 | 78 | def OnReshape(self, width, height): 79 | """Reshape the OpenGL viewport based on the dimensions of the window.""" 80 | glViewport(0, 0, width, height) --------------------------------------------------------------------------------