kinect

from OpenGL.GL import *
from pygame import *
from opencv import highgui
import Image, pygame, opencv, math, sys

class Camera(object):
    def __init__(self, draw_rectangle=False):
        self.camera = highgui.cvCreateCameraCapture(0)
        self.template = highgui.cvLoadImage("template.png", 0)
        self.image = None; self.location = None
        self.rectangle = draw_rectangle

    def update_image(self):
        im = highgui.cvQueryFrame(self.camera)
        self.image = opencv.cvGetMat(im)

    def get_image(self): return opencv.adaptors.Ipl2PIL(self.image)
    def get_position(self):
        if not self.location: return 0,0
        return self.location.x, self.location.y

    def track(self):
        result = opencv.cvCreateImage(opencv.cvSize(self.image.width-self.template.width+1,
                                                    self.image.height-self.template.height+1),
                                      opencv.IPL_DEPTH_32F, 1)
        image = opencv.cvCreateImage(opencv.cvSize(self.image.width,self.image.height), opencv.IPL_DEPTH_8U, 1)
        opencv.cvCvtColor(self.image,image,opencv.CV_RGB2GRAY)
        opencv.cvZero(result)
        opencv.cvMatchTemplate(image, self.template, result, opencv.CV_TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = opencv.cvMinMaxLoc(result)
        if max_val < .25: return
        else: self.location = max_loc

    def draw_rectangle(self):
        if not self.location: return
        opencv.cvRectangle(self.image, self.location,
                           opencv.cvPoint(self.location.x+self.template.width,
                                          self.location.y+self.template.height),
                           opencv.cvScalar(0), 1)

    def update(self):
        self.update_image()
        self.track()
        if self.rectangle: self.draw_rectangle()

class Tracking(object):
    def __init__(self, position, motions, callback=None):
        self.position = position
        self.change = [0,0]
        self.path = []
        self.threshold = 32
        self.diff_threshold = 20
        self.angle_step = 45
        self.motions = motions
        self.callback = callback
        self.motion = None

    def update_position(self, position):
        self.change = [self.position[x]-position[x] for x in range(len(position))]
        self.position = position

    def motion_diff(self, path, motion):
        diffs = [abs(motion[x]-path[x]) for x in range(min([len(path),len(motion)]))]
        diff = sum(diffs)/float(len(diffs))
        return diff

    def get_motion(self):
        return self.motion

    def update(self):
        self.motion = None
        if math.sqrt(self.change[0]**2+self.change[1]**2) > self.threshold:
            self.path.append(self.change)
        else:
            if len(self.path) > 1:
                angles = []
                for angle in [math.degrees(math.atan2(y,x)) for x,y in self.path]:
                    if not angles: angles.append(angle)
                    else:
                        if abs(abs(angles[-1])-abs(angle)) > self.angle_step:
                            angles.append(angle)
                diffs = {}
                for motion in self.motions:
                    diff = self.motion_diff(angles,motion)
                    if diff < self.diff_threshold: diffs[diff] = motion
                if len(diffs.keys()): self.motion = diffs[min(diffs.keys())]
                if self.callback and self.motion: self.callback(self.motion)
            self.path = []


class Application(object):
    def __init__(self):
        pygame.init()
        self.screen_size = (640,480)
        self.motions = [[0],[90],[-180],[180],[-90]]
        self.window = pygame.display.set_mode(self.screen_size, OPENGL|DOUBLEBUF)
        self.initgl()
        self.camera = Camera(True)
        self.camera.update()
        self.tracking = Tracking(self.camera.get_position(), self.motions, self.motion_callback)
        self.display_list = glGenLists(1)
        self.clock = pygame.time.Clock()
        self.background = None

    def start(self):
        self.update()
        self.background = opencv.cvCloneImage(self.camera.image)
        self.render()
        self.loop()

    def initgl(self):
        glViewport(0,0,self.screen_size[0],self.screen_size[1])
        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        glOrtho(0,self.screen_size[0],self.screen_size[1],0,0,10)
        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()
        glClearColor(1,1,1,1)
        glEnable(GL_TEXTURE_2D)
        glGenTextures(1)

    def set_texture(self):
        glBindTexture(GL_TEXTURE_2D, 1)
        image = self.camera.get_image()
        data = image.tostring()
        glTexImage2D(GL_TEXTURE_2D, 0, 3, image.size[0], image.size[1], 0,
                     GL_RGB, GL_UNSIGNED_BYTE, data)
        glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR)
        glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR)

    def render(self):
        glNewList(self.display_list, GL_COMPILE)
        glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT)
        glPushMatrix()
        glBegin(GL_QUADS)
        glTexCoord2i(0,0); glVertex2i(0,0)
        glTexCoord2i(1,0); glVertex2i(self.screen_size[0],0)
        glTexCoord2i(1,1); glVertex2i(self.screen_size[0],self.screen_size[1])
        glTexCoord2i(0,1); glVertex2i(0,self.screen_size[1])
        glEnd()
        glPopMatrix()
        glEndList()

    def motion_callback(self, motion):
        print motion

    def end(self):
        print self.clock.get_fps()
        pygame.quit()
        sys.exit(0)

    def handle_events(self, events):
        for event in events:
            if event.type == QUIT:
                self.end()

    def paint(self):
        self.set_texture()
        glCallList(self.display_list)
        pygame.display.flip()

    def sync(self):
        glFinish()

    def update(self):
        self.camera.update()
        self.tracking.update_position(self.camera.get_position())
        self.tracking.update()

    def loop(self):
        while True:
            self.clock.tick()
            self.handle_events(pygame.event.get())
            self.update()
            self.paint()
            self.sync()

if __name__ == '__main__':
    app = Application()
    app.start()