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()
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
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()
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')
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')
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