Ejemplo n.º 1
0
def on_draw():  
  if not 'rgbtex' in globals():
    create_texture()

  xyz, uv = projpts
  if xyz is None: return

  if not rgb is None:
    rgb_ = (rgb.astype(np.float32) * 4 + 70).clip(0,255).astype(np.uint8)
    glBindTexture(TEXTURE_TARGET, rgbtex)
    glTexSubImage2D(TEXTURE_TARGET, 0, 0, 0, 640, 480, GL_RGB, GL_UNSIGNED_BYTE, rgb_);

  glClearColor(*clearcolor)
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
  glEnable(GL_DEPTH_TEST)

  # flush that stack in case it's broken from earlier
  glPushMatrix()

  glMatrixMode(GL_PROJECTION)
  glLoadIdentity()
  gluPerspective(60, 4/3., 0.3, 200)

  glMatrixMode(GL_MODELVIEW)
  glLoadIdentity()

  def mouse_rotate(xAngle, yAngle, zAngle):
    glRotatef(xAngle, 1.0, 0.0, 0.0);
    glRotatef(yAngle, 0.0, 1.0, 0.0);
    glRotatef(zAngle, 0.0, 0.0, 1.0);
  glScale(zoomdist,zoomdist,1)
  glTranslate(0, 0,-3.5)
  mouse_rotate(rotangles[0], rotangles[1], 0);
  glTranslate(0,0,1.5)
  #glTranslate(0, 0,-1)

  # Draw some axes
  if 0:
    glBegin(GL_LINES)
    glColor3f(1,0,0); glVertex3f(0,0,0); glVertex3f(1,0,0)
    glColor3f(0,1,0); glVertex3f(0,0,0); glVertex3f(0,1,0)
    glColor3f(0,0,1); glVertex3f(0,0,0); glVertex3f(0,0,1)
    glEnd()

  # We can either project the points ourselves, or embed it in the opengl matrix
  if 0:
    dec = 4
    v,u = mgrid[:480,:640].astype(np.uint16)
    points = np.vstack((u[::dec,::dec].flatten(),
                        v[::dec,::dec].flatten(),
                        depth[::dec,::dec].flatten())).transpose()
    points = points[points[:,2]<2047,:]
    
    glMatrixMode(GL_TEXTURE)
    glLoadIdentity()
    glMultMatrixf(calibkinect.uv_matrix().transpose())
    glMultMatrixf(calibkinect.xyz_matrix().transpose())
    glTexCoordPointers(np.array(points))
    
    glMatrixMode(GL_MODELVIEW)
    glPushMatrix()
    glMultMatrixf(calibkinect.xyz_matrix().transpose())
    glVertexPointers(np.array(points))
  else:
    glMatrixMode(GL_TEXTURE)
    glLoadIdentity()
    glMatrixMode(GL_MODELVIEW)
    glPushMatrix()
    glVertexPointerf(xyz)
    glTexCoordPointerf(uv)

  # Draw the points
  glPointSize(2)
  glEnableClientState(GL_VERTEX_ARRAY)
  glEnableClientState(GL_TEXTURE_COORD_ARRAY)
  glEnable(TEXTURE_TARGET)
  glColor3f(1,1,1)
  glDrawElementsui(GL_POINTS, np.array(range(xyz.shape[0])))
  glDisableClientState(GL_VERTEX_ARRAY)
  glDisableClientState(GL_TEXTURE_COORD_ARRAY)
  glDisable(TEXTURE_TARGET)
  glPopMatrix()

  #
  if 0:
      inds = np.nonzero(xyz[:,2]>-0.55)
      glPointSize(10)
      glColor3f(0,1,1)
      glEnableClientState(GL_VERTEX_ARRAY)
      glDrawElementsui(GL_POINTS, np.array(inds))
      glDisableClientState(GL_VERTEX_ARRAY)

  if 0:
      # Draw only the points in the near plane
      glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
      glEnable(GL_BLEND)
      glColor(0.9,0.9,1.0,0.8)
      glPushMatrix()
      glTranslate(0,0,-0.55)
      glScale(0.6,0.6,1)
      glBegin(GL_QUADS)
      glVertex3f(-1,-1,0); glVertex3f( 1,-1,0);
      glVertex3f( 1, 1,0); glVertex3f(-1, 1,0);
      glEnd()
      glPopMatrix()
      glDisable(GL_BLEND)

  glPopMatrix()
Ejemplo n.º 2
0
def CrossPts(xyz_pt, uv_pt, h_pt, vh_pt, sn4_ref_pt):
    #print "return scene4_cross"
    #4 projection line
    Id3 = np.identity(3)

    #Watch out the projection line origin is the head_virtual
    #Unit vector from sn4_ref to head_virual

    #unit_sn_vh = (sn4_ref_pt[0]-vh_pt)
    unit_sn_vh = (sn4_ref_pt - vh_pt)
    for i in range(4):
        unit_sn_vh[i, :] = unit_sn_vh[i, :] / np.linalg.norm(unit_sn_vh[i, :])
        #print "unit_sn_vh[i,:]= ",unit_sn_vh[i,:]

    scene4_cross = np.ndarray([4, 2])  # cross rgb image
    scene4 = np.ndarray([4, 3])  # cross depth map

    Realtime_mode = 0
    Precise_mode = 1
    Mode = Realtime_mode  # Precise_mode #

    if Mode:
        len, _ = xyz_pt.shape
        #xyz_pt = xyz_pt[:,:,:]
        #why = xyz_pt[:,2].argmin(0)
        #print "closest pt= ", xyz_pt[why,:]
        #len,_ = xyz_pt.shape
        dis_xyz = np.matrix(np.ndarray([len, 12]))
        #print "1= ",dis_xyz.shape

        #P.11/60 Point to line distance
        #unit_sn_vh = unit_sn_vh/np.linalg.norm(unit_sn_vh)
        #dis_xyz = (np.dot((Id3 - unit_sn_vh*unit_sn_vh.T),((xyz_pt[:,:]-vh_pt).T))).T

        proj_line = (xyz_pt[:, :] - vh_pt).T
        #print "4= ",proj_line.shape
        print "proj_line= ", proj_line

        #algorithm to find the closest point with minimized distance
        for i in range(4):
            u = (np.asmatrix(unit_sn_vh[i, :])).T
            uuT = np.dot(u, u.T)
            #print "u*u.T= ", uuT
            #print dis_xyz[:,3*(i):3*(i+1)].shape
            #print (np.dot((Id3 - uuT), proj_line)).shape
            dis_xyz[:, 3 * (i):3 * (i + 1)] = (np.dot((Id3 - uuT),
                                                      proj_line)).T

        #Get the real distance (error) by using norm (Euclidean, sum of square root)
        dis_err = np.matrix(np.ndarray([len, 4]))
        for j in range(len):
            dis_err[j, 0] = np.linalg.norm(dis_xyz[j, 0:3])
            dis_err[j, 1] = np.linalg.norm(dis_xyz[j, 3:6])
            dis_err[j, 2] = np.linalg.norm(dis_xyz[j, 6:9])
            dis_err[j, 3] = np.linalg.norm(dis_xyz[j, 9:12])

        # for each column: the row index of the minimum value
        ind = dis_err.argmin(0)
        #scene4_cross = np.ndarray([4,2])    # cross rgb image
        #scene4 = np.ndarray([4,3])          # cross depth map

        for i in range(4):
            #print xyz_pt[ind[0,i],:]
            scene4[i, :] = xyz_pt[ind[0, i], :]
            #print uv[ind[0,i],:]
            scene4_cross[i, :] = uv_pt[ind[0, i], :]
            #print scene4_cross[i,:]

        #**************************************************
        #Check if scene point direction match proj_line
        #If not match, proj_line must be out of image range (show "warning message")
        #Use other point's depth(Z) for calculating XY on the proj_line
        #But XYZ not on depth map, it will use calibkinect.uv_matrix()*"XYZ1" to map back to uv
        unit_scene4_vh = (scene4 - vh_pt)
        diff_angle = np.ndarray([4, 1])

        for i in range(4):
            unit_scene4_vh[i, :] = unit_scene4_vh[i, :] / np.linalg.norm(
                unit_scene4_vh[i, :])
            diff_angle[i, 0] = np.dot(unit_scene4_vh[i, :], unit_sn_vh[i, :])
            diff_angle[i, 0] = (np.arccos(diff_angle[i, 0])) * 180 / np.pi
            print "Point", i, "out of proj_line angle= ", diff_angle[i, 0]
            #Parameter for detection angle difference(not radius)
            if (diff_angle[i, 0] > 1):
                print "Line ", i, ": your virtual mirror image is out of camera range!!!"

        for i in range(4):
            if (diff_angle[i, 0] > 1):
                good_ref = diff_angle.argmin(0)
                #print "good_ref= ",good_ref
                good_proj = scene4[good_ref, :] - vh_pt
                ratio = good_proj[0, 2] / unit_sn_vh[i, 2]
                good_scene_pt = (unit_sn_vh[i, :]) * ratio + vh_pt
                '''
                #Verification****************
                a = good_scene_pt - vh_pt
                a = a/np.linalg.norm(a)
                b = np.dot(a,unit_sn_vh[i,:])
                b = (np.arccos(b))*180/np.pi
                print "b should be very small or 0", b
                #********************************
                '''
                super_xyz = np.vstack((good_scene_pt.T, 0))
                #print super_xyz
                U, V, W = np.dot(calibkinect.uv_matrix(), super_xyz)
                U, V = U / W, V / W
                #print "Old_scene4_cross[i,:]= ",scene4_cross[i,:]
                scene4_cross[i, :] = np.vstack((U, V)).transpose()
                #print "New_scene4_cross[i,:]= ",scene4_cross[i,:]

        #**************************************************
    else:
        print "Go into real-time mode, head_position= ", h_pt

        for i in range(4):
            #print "*************************"
            #good_ref = diff_angle.argmin(0)
            #print "good_ref= ",good_ref
            good_proj = h_pt - vh_pt
            ratio = good_proj[0, 2] / unit_sn_vh[i, 2]
            good_scene_pt = (unit_sn_vh[i, :]) * ratio + vh_pt
            scene4[i, :] = good_scene_pt
            #print "scene4[i,:]= ",scene4[i,:]
            '''
            #Verification****************
            a = good_scene_pt - vh_pt
            a = a/np.linalg.norm(a)
            b = np.dot(a,unit_sn_vh[i,:])
            b = (np.arccos(b))*180/np.pi
            print "b should be very small or 0", b
            #********************************
            '''
            super_xyz = np.vstack((good_scene_pt.T, 0))
            #print super_xyz
            U, V, W = np.dot(calibkinect.uv_matrix(), super_xyz)
            U, V = U / W, V / W
            #print "Old_scene4_cross[i,:]= ",scene4_cross[i,:]
            scene4_cross[i, :] = np.vstack((U, V)).transpose()
            #print "New_scene4_cross[i,:]= ",scene4_cross[i,:]
            #print "*************************"

    #print "scene4_cross= ",scene4_cross
    #print "============================="
    return scene4_cross
Ejemplo n.º 3
0
def on_draw():
    if not 'rgbtex' in globals():
        create_texture()

    xyz, uv = projpts
    if xyz is None: return

    if not rgb is None:
        rgb_ = (rgb.astype(np.float32) * 4 + 70).clip(0, 255).astype(np.uint8)
        glBindTexture(TEXTURE_TARGET, rgbtex)
        glTexSubImage2D(TEXTURE_TARGET, 0, 0, 0, 640, 480, GL_RGB,
                        GL_UNSIGNED_BYTE, rgb_)

    glClearColor(*clearcolor)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glEnable(GL_DEPTH_TEST)

    # flush that stack in case it's broken from earlier
    glPushMatrix()

    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(60, 4 / 3., 0.3, 200)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    def mouse_rotate(xAngle, yAngle, zAngle):
        glRotatef(xAngle, 1.0, 0.0, 0.0)
        glRotatef(yAngle, 0.0, 1.0, 0.0)
        glRotatef(zAngle, 0.0, 0.0, 1.0)

    glScale(zoomdist, zoomdist, 1)
    glTranslate(0, 0, -3.5)
    mouse_rotate(rotangles[0], rotangles[1], 0)
    glTranslate(0, 0, 1.5)
    #glTranslate(0, 0,-1)

    # Draw some axes
    if 0:
        glBegin(GL_LINES)
        glColor3f(1, 0, 0)
        glVertex3f(0, 0, 0)
        glVertex3f(1, 0, 0)
        glColor3f(0, 1, 0)
        glVertex3f(0, 0, 0)
        glVertex3f(0, 1, 0)
        glColor3f(0, 0, 1)
        glVertex3f(0, 0, 0)
        glVertex3f(0, 0, 1)
        glEnd()

    # We can either project the points ourselves, or embed it in the opengl matrix
    if 0:
        dec = 4
        v, u = mgrid[:480, :640].astype(np.uint16)
        points = np.vstack(
            (u[::dec, ::dec].flatten(), v[::dec, ::dec].flatten(),
             depth[::dec, ::dec].flatten())).transpose()
        points = points[points[:, 2] < 2047, :]

        glMatrixMode(GL_TEXTURE)
        glLoadIdentity()
        glMultMatrixf(calibkinect.uv_matrix().transpose())
        glMultMatrixf(calibkinect.xyz_matrix().transpose())
        glTexCoordPointers(np.array(points))

        glMatrixMode(GL_MODELVIEW)
        glPushMatrix()
        glMultMatrixf(calibkinect.xyz_matrix().transpose())
        glVertexPointers(np.array(points))
    else:
        glMatrixMode(GL_TEXTURE)
        glLoadIdentity()
        glMatrixMode(GL_MODELVIEW)
        glPushMatrix()
        glVertexPointerf(xyz)
        glTexCoordPointerf(uv)

    # Draw the points
    glPointSize(2)
    glEnableClientState(GL_VERTEX_ARRAY)
    glEnableClientState(GL_TEXTURE_COORD_ARRAY)
    glEnable(TEXTURE_TARGET)
    glColor3f(1, 1, 1)
    glDrawElementsui(GL_POINTS, np.array(range(xyz.shape[0])))
    glDisableClientState(GL_VERTEX_ARRAY)
    glDisableClientState(GL_TEXTURE_COORD_ARRAY)
    glDisable(TEXTURE_TARGET)
    glPopMatrix()

    #
    if 0:
        inds = np.nonzero(xyz[:, 2] > -0.55)
        glPointSize(10)
        glColor3f(0, 1, 1)
        glEnableClientState(GL_VERTEX_ARRAY)
        glDrawElementsui(GL_POINTS, np.array(inds))
        glDisableClientState(GL_VERTEX_ARRAY)

    if 0:
        # Draw only the points in the near plane
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glEnable(GL_BLEND)
        glColor(0.9, 0.9, 1.0, 0.8)
        glPushMatrix()
        glTranslate(0, 0, -0.55)
        glScale(0.6, 0.6, 1)
        glBegin(GL_QUADS)
        glVertex3f(-1, -1, 0)
        glVertex3f(1, -1, 0)
        glVertex3f(1, 1, 0)
        glVertex3f(-1, 1, 0)
        glEnd()
        glPopMatrix()
        glDisable(GL_BLEND)

    glPopMatrix()
Ejemplo n.º 4
0
    def on_draw(self):
        if not '_initOK' in dir(self): return

        clearcolor = [0, 0, 0, 0]
        glClearColor(*clearcolor)
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        glEnable(GL_DEPTH_TEST)

        # flush that stack in case it's broken from earlier
        try:
            while glPopMatrix():
                pass
        except:
            pass

        glMatrixMode(GL_PROJECTION)
        glLoadIdentity()
        gluPerspective(60, 4 / 3., 0.3, 200)
        #glScale(-1,1,1)
        #gluOrtho2D(-10,10,-10,10)

        glMatrixMode(GL_MODELVIEW)
        glLoadIdentity()

        def mouse_rotate(xAngle, yAngle, zAngle):
            glRotatef(xAngle, 1.0, 0.0, 0.0)
            glRotatef(yAngle, 0.0, 1.0, 0.0)
            glRotatef(zAngle, 0.0, 0.0, 1.0)

        glScale(self.zoomdist, self.zoomdist, 1)
        glTranslate(0, 0, -1.5)
        mouse_rotate(self.rotangles[0], self.rotangles[1], 0)
        glTranslate(0, 0, 1.5)
        #glTranslate(0, 0,-1)

        glBindBufferARB(GL_ARRAY_BUFFER_ARB, self.xyzbuf)
        glVertexPointers(None)
        glTexCoordPointer(3, GL_SHORT, 0, None)

        glMatrixMode(GL_TEXTURE)
        glLoadIdentity()
        glMultMatrixf(calibkinect.uv_matrix().transpose())
        glMultMatrixf(calibkinect.xyz_matrix().transpose())

        glMatrixMode(GL_MODELVIEW)
        glPushMatrix()
        glMultMatrixf(calibkinect.xyz_matrix().transpose())

        # Draw the points
        glPointSize(2)
        glEnableClientState(GL_VERTEX_ARRAY)
        glEnableClientState(GL_TEXTURE_COORD_ARRAY)

        #if not rgb is None:
        glEnable(TEXTURE_TARGET)

        #if not color is None:
        # glEnableClientState(GL_COLOR_ARRAY)
        # glColorPointerf(color)
        # glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
        # glEnable(GL_BLEND)

        glColor3f(1, 1, 1)
        glDrawElementsui(GL_POINTS, np.mgrid[:640 * 480])
        glDisableClientState(GL_COLOR_ARRAY)
        glDisableClientState(GL_VERTEX_ARRAY)
        glDisableClientState(GL_TEXTURE_COORD_ARRAY)
        glDisable(TEXTURE_TARGET)
        glDisable(GL_BLEND)
        glPopMatrix()

        self._wrap('on_draw_axes')
Ejemplo n.º 5
0
  def on_draw(self):  
    if not '_initOK' in dir(self): return
    
    clearcolor = [0,0,0,0]
    glClearColor(*clearcolor)
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    glEnable(GL_DEPTH_TEST)

    # flush that stack in case it's broken from earlier
    try:
      while glPopMatrix(): pass
    except:
      pass
      
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(60, 4/3., 0.3, 200)
    #glScale(-1,1,1)
    #gluOrtho2D(-10,10,-10,10)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    def mouse_rotate(xAngle, yAngle, zAngle):
      glRotatef(xAngle, 1.0, 0.0, 0.0);
      glRotatef(yAngle, 0.0, 1.0, 0.0);
      glRotatef(zAngle, 0.0, 0.0, 1.0);
    glScale(self.zoomdist,self.zoomdist,1)
    glTranslate(0, 0,-1.5)
    mouse_rotate(self.rotangles[0], self.rotangles[1], 0);
    glTranslate(0,0,1.5)
    #glTranslate(0, 0,-1)



    glBindBufferARB(GL_ARRAY_BUFFER_ARB, self.xyzbuf)
    glVertexPointers(None)
    glTexCoordPointer(3, GL_SHORT, 0, None)

    glMatrixMode(GL_TEXTURE)
    glLoadIdentity()
    glMultMatrixf(calibkinect.uv_matrix().transpose())
    glMultMatrixf(calibkinect.xyz_matrix().transpose())

    glMatrixMode(GL_MODELVIEW)
    glPushMatrix()
    glMultMatrixf(calibkinect.xyz_matrix().transpose())

    # Draw the points
    glPointSize(2)
    glEnableClientState(GL_VERTEX_ARRAY)
    glEnableClientState(GL_TEXTURE_COORD_ARRAY)
  
    #if not rgb is None:
    glEnable(TEXTURE_TARGET)

    #if not color is None:
      # glEnableClientState(GL_COLOR_ARRAY)
      # glColorPointerf(color)
      # glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
      # glEnable(GL_BLEND)
    
    glColor3f(1,1,1)
    glDrawElementsui(GL_POINTS, np.mgrid[:640*480])
    glDisableClientState(GL_COLOR_ARRAY)
    glDisableClientState(GL_VERTEX_ARRAY)
    glDisableClientState(GL_TEXTURE_COORD_ARRAY)
    glDisable(TEXTURE_TARGET)
    glDisable(GL_BLEND)
    glPopMatrix()
    
    self._wrap('on_draw_axes')

    
Ejemplo n.º 6
0
def CrossPts(xyz_pt, uv_pt, h_pt,vh_pt,sn4_ref_pt):
    #print "return scene4_cross"
    #4 projection line
    Id3 = np.identity(3) 

    #Watch out the projection line origin is the head_virtual
    #Unit vector from sn4_ref to head_virual
    
    #unit_sn_vh = (sn4_ref_pt[0]-vh_pt)
    unit_sn_vh = (sn4_ref_pt - vh_pt)
    for i in range(4):
        unit_sn_vh[i,:] = unit_sn_vh[i,:]/np.linalg.norm(unit_sn_vh[i,:])
        #print "unit_sn_vh[i,:]= ",unit_sn_vh[i,:]

    scene4_cross = np.ndarray([4,2])    # cross rgb image
    scene4 = np.ndarray([4,3])          # cross depth map 
    
    Realtime_mode = 0
    Precise_mode = 1
    Mode = Realtime_mode # Precise_mode #
      
    if Mode:
        len,_ = xyz_pt.shape
        #xyz_pt = xyz_pt[:,:,:]
        #why = xyz_pt[:,2].argmin(0)
        #print "closest pt= ", xyz_pt[why,:]
        #len,_ = xyz_pt.shape
        dis_xyz = np.matrix(np.ndarray([len,12]))
        #print "1= ",dis_xyz.shape
        
        #P.11/60 Point to line distance 
        #unit_sn_vh = unit_sn_vh/np.linalg.norm(unit_sn_vh)
        #dis_xyz = (np.dot((Id3 - unit_sn_vh*unit_sn_vh.T),((xyz_pt[:,:]-vh_pt).T))).T
        
        proj_line = (xyz_pt[:,:] - vh_pt).T    
        #print "4= ",proj_line.shape
        print "proj_line= ",proj_line
        
        
        #algorithm to find the closest point with minimized distance
        for i in range(4):
            u = (np.asmatrix(unit_sn_vh[i,:])).T
            uuT = np.dot(u,u.T) 
            #print "u*u.T= ", uuT
            #print dis_xyz[:,3*(i):3*(i+1)].shape
            #print (np.dot((Id3 - uuT), proj_line)).shape
            dis_xyz[:,3*(i):3*(i+1)] = (np.dot((Id3 - uuT), proj_line)).T
        
        #Get the real distance (error) by using norm (Euclidean, sum of square root)
        dis_err = np.matrix(np.ndarray([len,4]))
        for j in range(len):
            dis_err[j,0]=np.linalg.norm(dis_xyz[j,0:3])
            dis_err[j,1]=np.linalg.norm(dis_xyz[j,3:6])
            dis_err[j,2]=np.linalg.norm(dis_xyz[j,6:9])
            dis_err[j,3]=np.linalg.norm(dis_xyz[j,9:12])
    
        # for each column: the row index of the minimum value
        ind = dis_err.argmin(0) 
        #scene4_cross = np.ndarray([4,2])    # cross rgb image
        #scene4 = np.ndarray([4,3])          # cross depth map 
        
        for i in range(4):        
            #print xyz_pt[ind[0,i],:]
            scene4[i,:] = xyz_pt[ind[0,i],:]
            #print uv[ind[0,i],:]
            scene4_cross[i,:] = uv_pt[ind[0,i],:]
            #print scene4_cross[i,:]
        
        #**************************************************
        #Check if scene point direction match proj_line
        #If not match, proj_line must be out of image range (show "warning message")
        #Use other point's depth(Z) for calculating XY on the proj_line
        #But XYZ not on depth map, it will use calibkinect.uv_matrix()*"XYZ1" to map back to uv      
        unit_scene4_vh = (scene4 - vh_pt)
        diff_angle = np.ndarray([4,1])
    
        for i in range(4):
            unit_scene4_vh[i,:] = unit_scene4_vh[i,:]/np.linalg.norm(unit_scene4_vh[i,:])
            diff_angle[i,0] = np.dot(unit_scene4_vh[i,:],unit_sn_vh[i,:])
            diff_angle[i,0] = (np.arccos(diff_angle[i,0]))*180/np.pi
            print "Point",i,"out of proj_line angle= ",diff_angle[i,0]
            #Parameter for detection angle difference(not radius)
            if (diff_angle[i,0] > 1): 
                print "Line ",i,": your virtual mirror image is out of camera range!!!"
            
        for i in range (4):
            if (diff_angle[i,0] > 1): 
                good_ref = diff_angle.argmin(0)
                #print "good_ref= ",good_ref
                good_proj = scene4[good_ref,:] - vh_pt
                ratio = good_proj[0,2]/unit_sn_vh[i,2]
                good_scene_pt = (unit_sn_vh[i,:])*ratio + vh_pt
                '''
                #Verification****************
                a = good_scene_pt - vh_pt
                a = a/np.linalg.norm(a)
                b = np.dot(a,unit_sn_vh[i,:])
                b = (np.arccos(b))*180/np.pi
                print "b should be very small or 0", b
                #********************************
                '''
                super_xyz = np.vstack((good_scene_pt.T,0))
                #print super_xyz
                U,V,W = np.dot(calibkinect.uv_matrix(), super_xyz)
                U,V = U/W, V/W
                #print "Old_scene4_cross[i,:]= ",scene4_cross[i,:]
                scene4_cross[i,:] = np.vstack((U,V)).transpose()
                #print "New_scene4_cross[i,:]= ",scene4_cross[i,:]     
            
            
        #**************************************************
    else:
        print "Go into real-time mode, head_position= ", h_pt

        for i in range (4):
            #print "*************************"
            #good_ref = diff_angle.argmin(0)
            #print "good_ref= ",good_ref
            good_proj = h_pt - vh_pt
            ratio = good_proj[0,2]/unit_sn_vh[i,2]
            good_scene_pt = (unit_sn_vh[i,:])*ratio + vh_pt
            scene4[i,:] = good_scene_pt
            #print "scene4[i,:]= ",scene4[i,:]
            '''
            #Verification****************
            a = good_scene_pt - vh_pt
            a = a/np.linalg.norm(a)
            b = np.dot(a,unit_sn_vh[i,:])
            b = (np.arccos(b))*180/np.pi
            print "b should be very small or 0", b
            #********************************
            '''
            super_xyz = np.vstack((good_scene_pt.T,0))
            #print super_xyz
            U,V,W = np.dot(calibkinect.uv_matrix(), super_xyz)
            U,V = U/W, V/W
            #print "Old_scene4_cross[i,:]= ",scene4_cross[i,:]
            scene4_cross[i,:] = np.vstack((U,V)).transpose()
            #print "New_scene4_cross[i,:]= ",scene4_cross[i,:]     
            #print "*************************"
                    
    #print "scene4_cross= ",scene4_cross
    #print "============================="
    return scene4_cross