示例#1
0
def update(dt=0):
  global projpts, rgb, depth
  depth,_ = freenect.sync_get_depth()
  rgb,_ = freenect.sync_get_video()
  q = depth
  X,Y = np.meshgrid(range(640),range(480))
  # YOU CAN CHANGE THIS AND RERUN THE PROGRAM!
  # Point cloud downsampling
  d = 4
  projpts = calibkinect.depth2xyzuv(q[::d,::d],X[::d,::d],Y[::d,::d])
  refresh()
示例#2
0
def update(dt=0):
    global projpts, rgb, depth
    depth, _ = freenect.sync_get_depth()
    rgb, _ = freenect.sync_get_video()
    q = depth
    X, Y = np.meshgrid(range(640), range(480))
    # YOU CAN CHANGE THIS AND RERUN THE PROGRAM!
    # Point cloud downsampling
    d = 4
    projpts = calibkinect.depth2xyzuv(q[::d, ::d], X[::d, ::d], Y[::d, ::d])
    refresh()
def calcPosition(dets):
    for k, d in enumerate(dets):
        #print("Detection {}: Left: {} Top: {} Right: {} Bottom: {}".format(
        #    k, d.left(), d.top(), d.right(), d.bottom()))
            
        u,v = np.mgrid[d.left():d.right(), d.top():d.bottom()]
        #print("Height/y-pixel: {}".format(HEIGHT/(d.bottom()-d.top())))
        #print("Width/x-pixel:  {}".format(WIDTH /(d.right()-d.left())))
        #print(" ")
                        
    xyz, uv = calibkinect.depth2xyzuv(depth1[v,u], u, v)
                                    
    if len(xyz) != 0:
                
        #print(depth_dets[k-1].bottom()) #+= 20
        #depth_dets[k-1].set_bottom(depth_dets[k-1].bottom()+20)
        #print(depth_dets[k-1].bottom())
                
        #x_ind = np.argmin(abs(xyz[:,0]))
        #y_ind = len(xyz[:,1])/2 #np.argmin(abs(xyz[:,1]))            
        z_ind = len(xyz[:,2])/2#np.argmin(abs(xyz[:,2]))
                
        x = np.median(xyz[:,0]) #xyz[x_ind,0] #+ 0.03 
        y = np.median(xyz[:,1]) #xyz[y_ind,1] #+ 0.04
        z = np.median(xyz[:,2])#*(1-.270718) #xyz[z_ind,2] #- 0.04
                
        x = x*100 + 3.3 + z*9.01e-1
        y = y*100 + 15 - z*4.87
        z = z*100 - 44.6 - z*21
                
        x = x/100
        y = y/100
        z = z/100
        
        rect = dlib.rectangle( left=np.median(uv[:,0])-5, right=np.median(uv[:,0])+5, 
            top=np.median(uv[:,1])-5, bottom=np.median(uv[:,1])+5 )
                
        print("x [m]: {}".format(x))
        print("y [m]: {}".format(y))            
        print("z [m]: {}".format(z))
        print(" ")  

    return x, y, z, rect
示例#4
0
#!/usr/bin/env python
# Test of mapping depth to video using calibkinect, which doesn't seem to work
import numpy as np
import freenect, calibkinect
import matplotlib.pyplot as plt

depth = freenect.sync_get_depth()[0]
video = freenect.sync_get_video()[0]

depth = np.where(depth == 2047, 0, depth)

_, uv = calibkinect.depth2xyzuv(depth)
mapping = uv.astype(np.dtype('int16')).reshape(480, 640, 2)
mapped = np.zeros((480, 640, 3), np.dtype('uint8'))

for i, row in enumerate(mapping):
    for j, point in enumerate(row):
        mapped[i, j] = video[point[0], point[1]]

plt.figure(1)
plt.imshow(depth, cmap=plt.cm.gray)
plt.figure(2)
plt.imshow(mapped)
plt.figure(3)
plt.imshow(video)
plt.show()
def point_cloud():
    '''
    turn depth into raw data
    '''
    return depth2xyzuv(freenect.sync_get_depth()[0])
示例#6
0
    while True:
        depth, _ = freenect.sync_get_depth()
        rgb, _ = freenect.sync_get_video()
        depth = depth[::d, ::d]
        intdepth = depth.astype(np.int)

        diff = intdepth
        diff[np.fabs(intdepth - bgnd) < 10] = 0
        for i in range(3, 4):
            diff = median_filter(diff, size=(i, i))

        old_diff = diff.copy()
        nonz_idx = diff.nonzero()
        diff[nonz_idx] = prev[nonz_idx] * diff[nonz_idx]

        projpts = calibkinect.depth2xyzuv(diff, X, Y)
        points = projpts[0]
        if points.shape[0] < 15:
            cloud = None
        else:
            cloud = np.mean(points, axis=0)
            print(cloud)
        #cv.imshow('Current', depth_to_rgb(depth))
        cv.imshow('Diff', depth_to_rgb(diff))
        cv.waitKey(5)
        prev = old_diff
        prev[prev.nonzero()] = 1

        #print("Projected points")
        #refresh()
        #print("Drew image")
示例#7
0
 def get_data(self):
     (depth, _) = get_depth()
     (raw_data, _) = depth2xyzuv(depth)
     self.data = np.array(
         [point for point in raw_data[::4] if inBBox(point, self.bounds)])
示例#8
0
 def xyzuv(self):
     q = self.depth
     X, Y = numpy.meshgrid(range(KINECT_FRAME_WIDTH),
                           range(KINECT_FRAME_HEIGHT))
     return calibkinect.depth2xyzuv(q, X, Y)
示例#9
0
def loopcv():
    cv.NamedWindow("Depth_Map", cv.CV_WINDOW_AUTOSIZE)
    cv.SetMouseCallback("Depth_Map", on_mouse, None)
    print "Execute loopcv: click 3 pts on mirror, click 4 pts at screen corner"
    global mr3
    global sn4
    global sn4_ref

    while 1:
        (depth, _) = freenect.sync_get_depth()
        im = array2cv(depth.astype(np.uint8))

        #[warp]add rgb as img

        if pt is not None:
            print "=================="
            (x_d, y_d) = pt
            print "x=", x_d, " ,y=", y_d
            #print depth.shape
            #Watch out the indexing for depth col,row = 480,640
            d_raw = np.array([depth[y_d, x_d]])
            u_d = np.array([x_d])
            v_d = np.array([y_d])

            print "d_raw= ", d_raw
            print "u_d= ", u_d
            print "v_d= ", v_d
            xyz, uv = calibkinect.depth2xyzuv(d_raw, u_d, v_d)
            print "XYZ=", xyz
            print "XYZonRGBplane=", uv
            cv.WaitKey(100)
            cv.Circle(im, (x_d, y_d), 4, (0, 0, 255, 0), -1, 8, 0)
            cv.Circle(im, (int(uv[0, 0]), int(uv[0, 1])), 2,
                      (255, 255, 255, 0), -1, 8, 0)

            if (mr3 is None):
                mr3 = xyz
                #print mr3

            elif (mr3.shape[0] <= 2):  #append "2"+1= 3pts
                mr3 = np.append(mr3, xyz, axis=0)
                #print "append mr3=",mr3
                if (mr3.shape[0] == 3):
                    print "enough for mirror, click on screen"
                    #print "mr3.shape= ",mr3.shape

            elif (mr3.shape[0] == 3):
                if (sn4 is None):
                    sn4 = xyz
                    sn4_ref = MirrorReflection(mr3, xyz[0])

                elif (sn4.shape[0] <= 3):  #append "3"+1= 4pts
                    sn4 = np.append(sn4, xyz, axis=0)
                    sn4_ref = np.append(sn4_ref,
                                        MirrorReflection(mr3, xyz[0]),
                                        axis=0)

                    if (sn4.shape[0] == 4):  #sn4 have 4 pts actually
                        print "Total screen pts before reflection=", sn4
                        #print "sn4 shape= ",sn4.shape
                        print "Total screen pts after reflection=", sn4_ref
                        #print "sn4_ref shape= ",sn4_ref.shape
                        if (mr3.shape[0] == 3 and sn4.shape[0] == 4):
                            print "go into Real Game: Virtual Mirror mode"
                            #print "mr3= ", mr3
                            #print "sn4_ref[0:3,:]= ", sn4_ref[0:3,:]
            else:
                print "..."

        #for (x,y) in feat:
        #print x, y, velx[y,x], vely[y,x]
        cv.ShowImage("Depth_Map", im)
        if cv.WaitKey(10) == 27:
            print "screen size after calibration: "
            print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0] - sn4_ref[1])
            print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1] - sn4_ref[2])
            print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2] - sn4_ref[3])
            print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3] - sn4_ref[0])
            break
    #update()
    if (sn4_ref is not None):
        cv.DestroyWindow("Depth_Map")
        VirtualMirror()
示例#10
0
def VirtualMirror():
    cv.NamedWindow("RGB_remap", cv.CV_WINDOW_NORMAL)
    cv.NamedWindow("Depth_remap", cv.CV_WINDOW_AUTOSIZE)
    cv.NamedWindow('dst', cv.CV_WINDOW_NORMAL)
    cv.SetMouseCallback("Depth_remap", on_mouse, None)
    print "Virtual Mirror"
    print "Calibrated 4 Screen corner= ", sn4_ref
    print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0] - sn4_ref[1])
    print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1] - sn4_ref[2])
    print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2] - sn4_ref[3])
    print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3] - sn4_ref[0])
    global head_pos
    global head_virtual
    global scene4_cross
    head_pos = np.array([-0.2, -0.2, 1.0])  #Head_detect()

    while 1:
        (depth, _) = freenect.sync_get_depth()
        (rgb, _) = freenect.sync_get_video()
        #print type(depth)
        img = array2cv(rgb[:, :, ::-1])
        im = array2cv(depth.astype(np.uint8))
        #modulize this part for update_on() and loopcv()
        #q = depth
        X, Y = np.meshgrid(range(640), range(480))
        d = 2  #downsampling if need
        projpts = calibkinect.depth2xyzuv(depth[::d, ::d], X[::d, ::d],
                                          Y[::d, ::d])
        xyz, uv = projpts

        if tracking == 0:
            #*********************************
            if pt is not None:
                print "=================="
                (x_d, y_d) = pt
                print "x=", x_d, " ,y=", y_d
                #print depth.shape
                #Watch out the indexing for depth col,row = 480,640
                d_raw = np.array([depth[y_d, x_d]])
                u_d = np.array([x_d])
                v_d = np.array([y_d])

                print "d_raw= ", d_raw
                print "u_d= ", u_d
                print "v_d= ", v_d
                head3D, head2D = calibkinect.depth2xyzuv(d_raw, u_d, v_d)
                print "XYZ=", head3D
                print "XYZonRGBplane=", head2D

                head_pos = head3D[0]
                #print "head_pos.shape",head_pos.shape
                print "head_pos= ", head_pos
                cv.WaitKey(100)
                cv.Circle(im, (x_d, y_d), 4, (0, 0, 255, 0), -1, 8, 0)
                cv.Circle(im, (int(head2D[0, 0]), int(head2D[0, 1])), 2,
                          (255, 255, 255, 0), -1, 8, 0)

            #*********************************
        elif tracking == 1:
            #find the nearest point (nose) as reference for right eye position
            print "nose"
            inds = np.nonzero(xyz[:, 2] > 0.5)
            #print xyz.shape
            new_xyz = xyz[inds]
            #print new_xyz.shape
            close_ind = np.argmin(new_xyz[:, 2])
            head_pos = new_xyz[close_ind, :] + (0.03, 0.04, 0.01)
            #print head_pos.shape
            #print head_pos

        elif tracking == 2:
            #find the closest point as eye posiiton
            print "camera"
            inds = np.nonzero(xyz[:, 2] > 0.5)
            #print xyz.shape
            new_xyz = xyz[inds]
            #print new_xyz.shape
            close_ind = np.argmin(new_xyz[:, 2])
            head_pos = new_xyz[close_ind, :]
            #print head_pos.shape
            #print head_pos

        else:
            print "please select a tracking mode"

        head_virtual = MirrorReflection(sn4_ref[0:3, :], head_pos)
        print "head_virtual= ", head_virtual

        rgbK = np.array([[520.97092069697146, 0.0, 318.40565581396697],
                         [0.0, 517.85544366622719, 263.46756370601804],
                         [0.0, 0.0, 1.0]])
        rgbD = np.array([[0.22464481251757576], [-0.47968370787671893], [0.0],
                         [0.0]])
        irK = np.array([[588.51686020601733, 0.0, 320.22664144213843],
                        [0.0, 584.73028132692866, 241.98395817513071],
                        [0.0, 0.0, 1.0]])
        irD = np.array([[-0.1273506872313161], [0.36672476189160591], [0.0],
                        [0.0]])

        mapu = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapv = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)

        cv.InitUndistortMap(rgbK, rgbD, mapu, mapv)
        cv.InitUndistortMap(irK, irD, mapx, mapy)

        if 1:
            rgb_remap = cv.CloneImage(img)
            cv.Remap(img, rgb_remap, mapu, mapv)

            depth_remap = cv.CloneImage(im)
            cv.Remap(im, depth_remap, mapx, mapy)

        scene4_cross = Cross4Pts.CrossPts(xyz, uv, head_pos, head_virtual,
                                          sn4_ref)
        #[warp] Add whole warpping code here
        #[warp] points = Scene4Pts() as warpping 4 pts
        #Flip the dst image!!!!!!!!!
        #ShowImage("rgb_warp", dst)

        #Within/out of the rgb range
        #Mapping Destination (width, height)=(x,y)

        #Warning: the order of pts in clockwise: pt1(L-T),pt2(R-T),pt3(R-B),pt4(L-B)
        #points = [(test[0,0],test[0,1]), (630.,300.), (700.,500.), (400.,470.)]
        points = [(scene4_cross[0, 0], scene4_cross[0, 1]),
                  (scene4_cross[1, 0], scene4_cross[1, 1]),
                  (scene4_cross[2, 0], scene4_cross[2, 1]),
                  (scene4_cross[3, 0], scene4_cross[3, 1])]
        #Warping the image without flipping (camera image)
        #npoints  = [(0.,0.), (640.,0.), (640.,480.), (0.,480.)]
        #Warping the image with flipping (mirror flip image)
        npoints = [(640., 0.), (0., 0.), (0., 480.), (640., 480.)]
        mat = cv.CreateMat(3, 3, cv.CV_32FC1)
        cv.GetPerspectiveTransform(points, npoints, mat)

        #src = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_32F, 3 )
        src = cv.CreateImage(cv.GetSize(rgb_remap), cv.IPL_DEPTH_32F, 3)
        #cv.ConvertScale(img,src,(1/255.00))
        cv.ConvertScale(rgb_remap, src, (1 / 255.00))

        dst = cv.CloneImage(src)
        cv.Zero(dst)
        cv.WarpPerspective(src, dst, mat)
        #************************************************************************

        #Remap the rgb and depth image
        #Warping will use remap rgb image as src

        if 1:
            cv.ShowImage("RGB_remap", rgb_remap)  #rgb[200:440,300:600,::-1]
            cv.ShowImage("Depth_remap", depth_remap)
            cv.ShowImage("dst", dst)  #warp rgb image

        if cv.WaitKey(5) == 27:
            cv.DestroyWindow("RGB_remap")
            cv.DestroyWindow("Depth_remap")
            cv.DestroyWindow("dst")
            break
示例#11
0
 while True:
     depth, _ = freenect.sync_get_depth()
     rgb,_ = freenect.sync_get_video()
     depth = depth[::d, ::d]
     intdepth = depth.astype(np.int)
     
     diff = intdepth
     diff[np.fabs(intdepth - bgnd) < 10] = 0
     for i in range(3,4):
         diff = median_filter(diff, size=(i,i))
     
     old_diff = diff.copy()
     nonz_idx = diff.nonzero()
     diff[nonz_idx] = prev[nonz_idx] * diff[nonz_idx]
     
     projpts = calibkinect.depth2xyzuv(diff, X, Y)
     points = projpts[0]
     if points.shape[0] < 15:
         cloud = None
     else:
         cloud = np.mean(points, axis=0)
         print(cloud)
     #cv.imshow('Current', depth_to_rgb(depth))
     cv.imshow('Diff', depth_to_rgb(diff))
     cv.waitKey(5)
     prev = old_diff
     prev[prev.nonzero()] = 1
     
     #print("Projected points")
     #refresh()
     #print("Drew image")
示例#12
0
 def xyzuv(self):
     q = self.depth
     X,Y = numpy.meshgrid(range(KINECT_FRAME_WIDTH),
                          range(KINECT_FRAME_HEIGHT))
     return calibkinect.depth2xyzuv(q,X,Y)
def VirtualMirror():
    cv.NamedWindow("RGB_remap",cv.CV_WINDOW_NORMAL)
    cv.NamedWindow("Depth_remap",cv.CV_WINDOW_AUTOSIZE)
    cv.NamedWindow('dst', cv.CV_WINDOW_NORMAL)
    cv.SetMouseCallback( "Depth_remap", on_mouse, None)
    print "Virtual Mirror"
    print "Calibrated 4 Screen corner= ", sn4_ref
    print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0]-sn4_ref[1])
    print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1]-sn4_ref[2])
    print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2]-sn4_ref[3])
    print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3]-sn4_ref[0])
    global head_pos
    global head_virtual
    global scene4_cross
    head_pos = np.array([-0.2,-0.2,1.0]) #Head_detect()
    
    while 1:
        (depth,_) = freenect.sync_get_depth()
        (rgb,_)   = freenect.sync_get_video()
        #print type(depth)
        img = array2cv(rgb[:,:,::-1])
        im = array2cv(depth.astype(np.uint8))
        #modulize this part for update_on() and loopcv()
        #q = depth
        X,Y = np.meshgrid(range(640),range(480))
        d = 2 #downsampling if need
        projpts = calibkinect.depth2xyzuv(depth[::d,::d],X[::d,::d],Y[::d,::d])
        xyz,uv = projpts
        
        if tracking == 0:
            #*********************************
            if pt is not None:
                print "=================="
                (x_d,y_d) = pt
                print "x=",x_d, " ,y=",y_d
                #print depth.shape
                #Watch out the indexing for depth col,row = 480,640
                d_raw = np.array([depth[y_d,x_d]])
                u_d = np.array([x_d])
                v_d = np.array([y_d])
            
                print "d_raw= ", d_raw
                print "u_d= ", u_d 
                print "v_d= ", v_d 
                head3D,head2D = calibkinect.depth2xyzuv(d_raw,u_d,v_d)
                print "XYZ=", head3D
                print "XYZonRGBplane=", head2D
             
                head_pos = head3D[0]
                #print "head_pos.shape",head_pos.shape
                print "head_pos= ",head_pos
                cv.WaitKey(100) 
                cv.Circle(im, (x_d,y_d), 4, (0, 0, 255, 0), -1, 8, 0)
                cv.Circle(im, (int(head2D[0,0]),int(head2D[0,1])), 2, (255, 255, 255, 0), -1, 8, 0)
    


            #*********************************
        elif tracking == 1:
            #find the nearest point (nose) as reference for right eye position
            print "nose"
            inds = np.nonzero(xyz[:,2]>0.5)
            #print xyz.shape
            new_xyz = xyz[inds]
            #print new_xyz.shape
            close_ind = np.argmin(new_xyz[:,2])
            head_pos =  new_xyz[close_ind,:]+(0.03,0.04,0.01)
            #print head_pos.shape
            #print head_pos
            
        elif tracking == 2:
            #find the closest point as eye posiiton
            print "camera"
            inds = np.nonzero(xyz[:,2]>0.5)
            #print xyz.shape
            new_xyz = xyz[inds]
            #print new_xyz.shape
            close_ind = np.argmin(new_xyz[:,2])
            head_pos =  new_xyz[close_ind,:]
            #print head_pos.shape
            #print head_pos
               
        else:
            print "please select a tracking mode"
            
        head_virtual = MirrorReflection (sn4_ref[0:3,:],head_pos)
        print "head_virtual= ",head_virtual
        
        rgbK = np.array([[520.97092069697146,0.0,318.40565581396697], [0.0,517.85544366622719,263.46756370601804], [0.0,0.0,1.0]])      
        rgbD = np.array([[0.22464481251757576],[-0.47968370787671893],[0.0],[0.0]])
        irK = np.array([[588.51686020601733,0.0,320.22664144213843], [0.0,584.73028132692866,241.98395817513071], [0.0,0.0,1.0]])
        irD = np.array([[-0.1273506872313161], [0.36672476189160591], [0.0], [0.0]])
        
        mapu = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapv = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
        
        cv.InitUndistortMap(rgbK, rgbD, mapu, mapv)
        cv.InitUndistortMap(irK, irD, mapx, mapy)
    
        if 1:
            rgb_remap = cv.CloneImage(img)
            cv.Remap(img, rgb_remap, mapu, mapv)
            
            depth_remap = cv.CloneImage(im)
            cv.Remap(im, depth_remap, mapx, mapy)
            

         
        scene4_cross = Cross4Pts.CrossPts(xyz, uv, head_pos, head_virtual ,sn4_ref)        
        #[warp] Add whole warpping code here
        #[warp] points = Scene4Pts() as warpping 4 pts
        #Flip the dst image!!!!!!!!!
        #ShowImage("rgb_warp", dst)
       
        #Within/out of the rgb range
        #Mapping Destination (width, height)=(x,y)
        
        
        #Warning: the order of pts in clockwise: pt1(L-T),pt2(R-T),pt3(R-B),pt4(L-B)
        #points = [(test[0,0],test[0,1]), (630.,300.), (700.,500.), (400.,470.)]
        points = [(scene4_cross[0,0],scene4_cross[0,1]), 
                  (scene4_cross[1,0],scene4_cross[1,1]), 
                  (scene4_cross[2,0],scene4_cross[2,1]), 
                  (scene4_cross[3,0],scene4_cross[3,1])] 
        #Warping the image without flipping (camera image)
        #npoints  = [(0.,0.), (640.,0.), (640.,480.), (0.,480.)]            
        #Warping the image with flipping (mirror flip image)
        npoints  = [(640.,0.), 
                    (0.,0.), 
                    (0.,480.), 
                    (640.,480.)]            
        mat = cv.CreateMat(3, 3, cv.CV_32FC1)
        cv.GetPerspectiveTransform( points, npoints, mat);
    
        #src = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_32F, 3 )
        src = cv.CreateImage( cv.GetSize(rgb_remap), cv.IPL_DEPTH_32F, 3 )
        #cv.ConvertScale(img,src,(1/255.00))
        cv.ConvertScale(rgb_remap,src,(1/255.00))
        
        dst = cv.CloneImage( src );
        cv.Zero(dst);     
        cv.WarpPerspective(src, dst, mat);
        #************************************************************************
    
        #Remap the rgb and depth image
        #Warping will use remap rgb image as src
                
        if 1:
            cv.ShowImage("RGB_remap",rgb_remap)  #rgb[200:440,300:600,::-1]
            cv.ShowImage("Depth_remap",depth_remap)
            cv.ShowImage("dst", dst) #warp rgb image
           
        
        if cv.WaitKey(5)==27:
            cv.DestroyWindow("RGB_remap")
            cv.DestroyWindow("Depth_remap")
            cv.DestroyWindow("dst")
            break
def loopcv():
    cv.NamedWindow("Depth_Map", cv.CV_WINDOW_AUTOSIZE)
    cv.SetMouseCallback( "Depth_Map", on_mouse, None)
    print "Execute loopcv: click 3 pts on mirror, click 4 pts at screen corner"
    global mr3
    global sn4
    global sn4_ref 
    
    while 1:
        (depth,_) = freenect.sync_get_depth()
        im = array2cv(depth.astype(np.uint8))
        
        #[warp]add rgb as img
        
        if pt is not None:
            print "=================="
            (x_d,y_d) = pt
            print "x=",x_d, " ,y=",y_d
            #print depth.shape
            #Watch out the indexing for depth col,row = 480,640
            d_raw = np.array([depth[y_d,x_d]])
            u_d = np.array([x_d])
            v_d = np.array([y_d])
        
            print "d_raw= ", d_raw
            print "u_d= ", u_d 
            print "v_d= ", v_d 
            xyz,uv = calibkinect.depth2xyzuv(d_raw,u_d,v_d)
            print "XYZ=", xyz
            print "XYZonRGBplane=", uv
            cv.WaitKey(100)   
            cv.Circle(im, (x_d,y_d), 4, (0, 0, 255, 0), -1, 8, 0)
            cv.Circle(im, (int(uv[0,0]),int(uv[0,1])), 2, (255, 255, 255, 0), -1, 8, 0)
            
            if(mr3 is None):
                mr3=xyz
                #print mr3
            
            elif(mr3.shape[0] <= 2): #append "2"+1= 3pts
                mr3=np.append(mr3,xyz,axis=0)
                #print "append mr3=",mr3 
                if(mr3.shape[0] == 3):
                    print "enough for mirror, click on screen"
                    #print "mr3.shape= ",mr3.shape
                        
            elif(mr3.shape[0] == 3):
                if(sn4 is None):
                    sn4     = xyz
                    sn4_ref = MirrorReflection(mr3,xyz[0])
                    
                elif(sn4.shape[0] <= 3): #append "3"+1= 4pts
                    sn4     = np.append(sn4,     xyz,                   axis=0)
                    sn4_ref = np.append(sn4_ref, MirrorReflection(mr3,xyz[0]), axis=0)
                    
                    
                    if(sn4.shape[0] == 4):#sn4 have 4 pts actually
                        print "Total screen pts before reflection=" ,sn4
                        #print "sn4 shape= ",sn4.shape
                        print "Total screen pts after reflection="  ,sn4_ref
                        #print "sn4_ref shape= ",sn4_ref.shape
                        if(mr3.shape[0]==3 and sn4.shape[0]==4):
                            print "go into Real Game: Virtual Mirror mode"
                            #print "mr3= ", mr3
                            #print "sn4_ref[0:3,:]= ", sn4_ref[0:3,:]
            else:
                print "..."

            
        #for (x,y) in feat:
        #print x, y, velx[y,x], vely[y,x]
        cv.ShowImage("Depth_Map",im)
        if cv.WaitKey(10)==27:
            print "screen size after calibration: "
            print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0]-sn4_ref[1])
            print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1]-sn4_ref[2])
            print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2]-sn4_ref[3])
            print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3]-sn4_ref[0])
            break
    #update()
    if(sn4_ref is not None):
        cv.DestroyWindow("Depth_Map")
        VirtualMirror()
示例#15
0
	def get_data(self):
		(depth,_) = get_depth()
		(raw_data, _) = depth2xyzuv(depth)
		self.data = np.array([point for point in raw_data[::4] if inBBox(point, self.bounds)])