def normals_numpy(depth, rect=((0,0),(640,480)), win=7, mat=None):
assert depth.dtype == np.float32
from scipy.ndimage.filters import uniform_filter
(l,t),(r,b) = rect
v,u = np.mgrid[t:b,l:r]
depth = depth[v,u]
depth[depth==0] = -1e8 # 2047
depth = calibkinect.recip_depth_openni(depth)
depth = uniform_filter(depth, win)
global duniform
duniform = depth
dx = (np.roll(depth,-1,1) - np.roll(depth,1,1))/2
dy = (np.roll(depth,-1,0) - np.roll(depth,1,0))/2
#dx,dy = np.array(depth),np.array(depth)
#speedup.gradient(depth.ctypes.data, dx.ctypes.data,
# dy.ctypes.data, depth.shape[0], depth.shape[1])
X,Y,Z,W = -dx, -dy, 0*dy+1, -(-dx*u + -dy*v + depth).astype(np.float32)
mat = calibkinect.projection().astype('f').transpose()
mat = np.ascontiguousarray(mat)
x = X*mat[0,0] + Y*mat[0,1] + Z*mat[0,2] + W*mat[0,3]
y = X*mat[1,0] + Y*mat[1,1] + Z*mat[1,2] + W*mat[1,3]
z = X*mat[2,0] + Y*mat[2,1] + Z*mat[2,2] + W*mat[2,3]
w = np.sqrt(x*x + y*y + z*z)
w[z<0] *= -1
weights = z*0+1
weights[depth<-1000] = 0
weights[(z/w)<.1] = 0
#return x/w, y/w, z/w
return np.dstack((x/w,y/w,z/w)), weights
def setup_kernel(mats=None):
if mats is None:
mats = (np.ascontiguousarray(np.linalg.inv(calibkinect.projection())),
np.eye(4))
KK, RT = mats
kernel_normals = kernel_normals_template % (normal_maker(
'ONE',
np.linalg.inv(KK).transpose(), np.dot(RT, KK),
np.linalg.inv(RT).transpose()), )
global program
program = cl.Program(context, kernel_normals).build("-cl-mad-enable")
# I have no explanation for this workaround. Presumably it's fixed in
# another version of pyopencl. Wtf. Getting the kernel like this
# makes it go much faster when we __call__ it.
def workaround(self):
return self
cl.Kernel.workaround = workaround
program.flatrot_compute = program.flatrot_compute.workaround()
program.normal_compute_ONE = program.normal_compute_ONE.workaround()
program.lattice2_compute = program.lattice2_compute.workaround()
program.float4_sum = program.float4_sum.workaround()
program.gridinds_compute = program.gridinds_compute.workaround()
def setup_kernel(mats=None):
if mats is None:
mats = (np.ascontiguousarray(np.linalg.inv(calibkinect.projection())),
np.eye(4))
KK, RT = mats
kernel_normals = kernel_normals_template % (
normal_maker('ONE',
np.linalg.inv(KK).transpose(),
np.dot(RT, KK),
np.linalg.inv(RT).transpose()),
)
global program
program = cl.Program(context, kernel_normals).build("-cl-mad-enable")
# I have no explanation for this workaround. Presumably it's fixed in
# another version of pyopencl. Wtf. Getting the kernel like this
# makes it go much faster when we __call__ it.
def workaround(self):
return self
cl.Kernel.workaround = workaround
program.flatrot_compute = program.flatrot_compute.workaround()
program.normal_compute_ONE = program.normal_compute_ONE.workaround()
program.lattice2_compute = program.lattice2_compute.workaround()
program.float4_sum = program.float4_sum.workaround()
program.gridinds_compute = program.gridinds_compute.workaround()
def normals_c(depth, rect=((0,0),(640,480)), win=7):
assert depth.dtype == np.uint16
from scipy.ndimage.filters import uniform_filter
(l,t),(r,b) = rect
v,u = np.mgrid[t:b,l:r]
depth = depth[v,u]
depth = calibkinect.recip_depth_openni(depth)
output_ = np.empty(depth.shape, 'f')
uniform_filter(depth, win, output=output_)
depth = output_
x,y,z = [np.empty_like(depth) for i in range(3)]
mat = calibkinect.projection().astype('f').transpose()
mat = np.ascontiguousarray(mat)
speedup.normals(depth.astype('f'), u.astype('f'), v.astype('f'), x, y, z,
mat, depth.shape[0], depth.shape[1])
weights = z*0+1
weights[depth<-1000] = 0
weights[z<.1] = 0
return np.dstack((x,y,z)), weights
def find_plane(depth, boundpts):
from visuals.camerawindow import CameraWindow
global window
if not 'window' in globals():
window = CameraWindow()
# Build a mask of the image inside the convex points clicked
u,v = uv = np.mgrid[:480,:640][::-1]
mask = np.ones((480,640),bool)
for (x,y),(dx,dy) in zip(boundpts, boundpts - np.roll(boundpts,1,0)):
mask &= ((uv[0]-x)*dy - (uv[1]-y)*dx)<0
# Borrow the initialization from calibkinect
KK = np.linalg.inv(calibkinect.projection()).astype('f')
KK = np.ascontiguousarray(KK)
# Find the average plane going through here
global n,w
n,w = normals.normals_c(depth)
maskw = mask & (w>0)
abc = n[maskw].mean(0)
abc /= np.sqrt(np.dot(abc,abc))
a,b,c = abc
x,y,z = [_[maskw].mean() for _ in calibkinect.convertOpenNI2Real_numpy(depth)]
d = -(a*x+b*y+c*z)
tableplane = np.array([a,b,c,d])
#tablemean = np.array([x,y,z])
# Backproject the table plane into the image using inverse transpose
global tb0
tb0 = np.dot(KK.transpose(), tableplane)
tb0[2] = tb0[2]
# Build a matrix projecting sensor points to an system with
# the origin on the table, and Y pointing up from the table
# NOTE: the default orientation is offset by 45 degrees from the camera's
# natural direction.
v1 = np.array([a,b,c]); v1 /= np.sqrt(np.dot(v1,v1))
v0 = np.cross(v1, [-1,0,1]); v0 /= np.sqrt(np.dot(v0,v0))
v2 = np.cross(v0,v1);
Ktable = np.eye(4)
Ktable[:3,:3] = np.vstack((v0,v1,v2)).transpose()
Ktable[:3,3] = [x,y,z]
Ktable = np.linalg.inv(Ktable).astype('f')
KtableKK = np.dot(Ktable, KK).astype('f')
global boundptsM
boundptsM = []
for (up,vp) in boundpts:
# First project the image points (u,v) onto to the (imaged) tableplane
dp = -(tb0[0]*up + tb0[1]*vp + tb0[3])/tb0[2]
# Then project them into metric space
xp, yp, zp, wp = np.dot(KtableKK, [up,vp,dp,1])
xp /= wp ; yp /= wp ; zp /= wp;
boundptsM += [[xp,yp,zp]]
# Use OpenGL and framebuffers to draw the table and the walls
fbo = glGenFramebuffers(1)
glBindFramebuffer(GL_FRAMEBUFFER, fbo);
rb,rbc = glGenRenderbuffers(2)
glBindRenderbuffer(GL_RENDERBUFFER, rb);
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, 640, 480)
glFramebufferRenderbuffer(GL_FRAMEBUFFER,
GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER, rb);
glEnable(GL_DEPTH_TEST)
glClear(GL_DEPTH_BUFFER_BIT)
glViewport(0, 0, 640, 480)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0,640,0,480,0,-10)
glMultMatrixf(np.linalg.inv(KtableKK).transpose())
def draw():
glClear(GL_DEPTH_BUFFER_BIT|GL_COLOR_BUFFER_BIT)
glEnable(GL_CULL_FACE)
glBegin(GL_QUADS)
for x,y,z in boundptsM:
glVertex(x,y,z,1)
for (x,y,z),(x_,y_,z_) in zip(boundptsM,np.roll(boundptsM,1,0)):
glVertex(x ,y ,z ,1)
glVertex(x_,y_,z_,1)
glVertex(0,1,0,0)
glVertex(0,1,0,0)
glEnd()
glDisable(GL_CULL_FACE)
glFinish()
gf = glGetIntegerv(GL_FRONT_FACE)
glFrontFace(GL_CCW)
draw()
openglbgHi = glReadPixels(0, 0, 640, 480,
GL_DEPTH_COMPONENT, GL_FLOAT).reshape(480,640);
glFrontFace(GL_CW)
draw()
openglbgLo = glReadPixels(0, 0, 640, 480,
GL_DEPTH_COMPONENT, GL_FLOAT).reshape(480,640);
glFrontFace(gf)
global hi,lo
openglbgHi = 1000./(openglbgHi*10)
openglbgLo = 1000./(openglbgLo*10)
lo = np.array(openglbgLo)
hi = np.array(openglbgHi)
#openglbgLo[openglbgLo>=2047] = 0
#openglbgHi[np.isnan(openglbgHi)] = 0
openglbgLo[openglbgHi==openglbgLo] = 0
glBindFramebuffer(GL_FRAMEBUFFER, 0);
glDeleteRenderbuffers(2, [rb,rbc]);
glDeleteFramebuffers(1, [fbo]);
background = np.array(depth)
background[~mask] = 0
background = np.maximum(background,openglbgHi)
#backgroundM = normals.project(background)
openglbgLo = openglbgLo.astype(np.uint16)
background = background.astype(np.uint16)
background[background>=5] -= 5
#openglbgLo += 5
return dict(
bgLo=openglbgLo,
bgHi=background,
boundpts=boundpts,
boundptsM=boundptsM,
KK=KK,
Ktable=Ktable)
def find_plane(depth, boundpts):
from visuals.camerawindow import CameraWindow
global window
if not 'window' in globals():
window = CameraWindow()
# Build a mask of the image inside the convex points clicked
u, v = uv = np.mgrid[:480, :640][::-1]
mask = np.ones((480, 640), bool)
for (x, y), (dx, dy) in zip(boundpts, boundpts - np.roll(boundpts, 1, 0)):
mask &= ((uv[0] - x) * dy - (uv[1] - y) * dx) < 0
# Borrow the initialization from calibkinect
KK = np.linalg.inv(calibkinect.projection()).astype('f')
KK = np.ascontiguousarray(KK)
# Find the average plane going through here
global n, w
n, w = normals.normals_c(depth)
maskw = mask & (w > 0)
abc = n[maskw].mean(0)
abc /= np.sqrt(np.dot(abc, abc))
a, b, c = abc
x, y, z = [
_[maskw].mean() for _ in calibkinect.convertOpenNI2Real_numpy(depth)
]
d = -(a * x + b * y + c * z)
tableplane = np.array([a, b, c, d])
#tablemean = np.array([x,y,z])
# Backproject the table plane into the image using inverse transpose
global tb0
tb0 = np.dot(KK.transpose(), tableplane)
tb0[2] = tb0[2]
# Build a matrix projecting sensor points to an system with
# the origin on the table, and Y pointing up from the table
# NOTE: the default orientation is offset by 45 degrees from the camera's
# natural direction.
v1 = np.array([a, b, c])
v1 /= np.sqrt(np.dot(v1, v1))
v0 = np.cross(v1, [-1, 0, 1])
v0 /= np.sqrt(np.dot(v0, v0))
v2 = np.cross(v0, v1)
Ktable = np.eye(4)
Ktable[:3, :3] = np.vstack((v0, v1, v2)).transpose()
Ktable[:3, 3] = [x, y, z]
Ktable = np.linalg.inv(Ktable).astype('f')
KtableKK = np.dot(Ktable, KK).astype('f')
global boundptsM
boundptsM = []
for (up, vp) in boundpts:
# First project the image points (u,v) onto to the (imaged) tableplane
dp = -(tb0[0] * up + tb0[1] * vp + tb0[3]) / tb0[2]
# Then project them into metric space
xp, yp, zp, wp = np.dot(KtableKK, [up, vp, dp, 1])
xp /= wp
yp /= wp
zp /= wp
boundptsM += [[xp, yp, zp]]
# Use OpenGL and framebuffers to draw the table and the walls
fbo = glGenFramebuffers(1)
glBindFramebuffer(GL_FRAMEBUFFER, fbo)
rb, rbc = glGenRenderbuffers(2)
glBindRenderbuffer(GL_RENDERBUFFER, rb)
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, 640, 480)
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER, rb)
glEnable(GL_DEPTH_TEST)
glClear(GL_DEPTH_BUFFER_BIT)
glViewport(0, 0, 640, 480)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0, 640, 0, 480, 0, -10)
glMultMatrixf(np.linalg.inv(KtableKK).transpose())
def draw():
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT)
glEnable(GL_CULL_FACE)
glBegin(GL_QUADS)
for x, y, z in boundptsM:
glVertex(x, y, z, 1)
for (x, y, z), (x_, y_, z_) in zip(boundptsM, np.roll(boundptsM, 1,
0)):
glVertex(x, y, z, 1)
glVertex(x_, y_, z_, 1)
glVertex(0, 1, 0, 0)
glVertex(0, 1, 0, 0)
glEnd()
glDisable(GL_CULL_FACE)
glFinish()
gf = glGetIntegerv(GL_FRONT_FACE)
glFrontFace(GL_CCW)
draw()
openglbgHi = glReadPixels(0, 0, 640, 480, GL_DEPTH_COMPONENT,
GL_FLOAT).reshape(480, 640)
glFrontFace(GL_CW)
draw()
openglbgLo = glReadPixels(0, 0, 640, 480, GL_DEPTH_COMPONENT,
GL_FLOAT).reshape(480, 640)
glFrontFace(gf)
global hi, lo
openglbgHi = 1000. / (openglbgHi * 10)
openglbgLo = 1000. / (openglbgLo * 10)
lo = np.array(openglbgLo)
hi = np.array(openglbgHi)
#openglbgLo[openglbgLo>=2047] = 0
#openglbgHi[np.isnan(openglbgHi)] = 0
openglbgLo[openglbgHi == openglbgLo] = 0
glBindFramebuffer(GL_FRAMEBUFFER, 0)
glDeleteRenderbuffers(2, [rb, rbc])
glDeleteFramebuffers(1, [fbo])
background = np.array(depth)
background[~mask] = 0
background = np.maximum(background, openglbgHi)
#backgroundM = normals.project(background)
openglbgLo = openglbgLo.astype(np.uint16)
background = background.astype(np.uint16)
background[background >= 5] -= 5
#openglbgLo += 5
return dict(bgLo=openglbgLo,
bgHi=background,
boundpts=boundpts,
boundptsM=boundptsM,
KK=KK,
Ktable=Ktable)
def kinect_camera():
return Camera(calibkinect.projection())
def find_plane(depth, boundpts):
from wxpy3d.camerawindow import CameraWindow
global window
if not 'window' in globals():
window = CameraWindow()
# Build a mask of the image inside the convex points clicked
mask = make_mask(boundpts)
# Borrow the initialization from calibkinect
KK = np.linalg.inv(calibkinect.projection()).astype('f')
KK = np.ascontiguousarray(KK)
# Find the average plane going through here
global n,w
n,w = normals.normals_c(depth)
maskw = mask & (w>0)
abc = n[maskw].mean(0)
abc /= np.sqrt(np.dot(abc,abc))
a,b,c = abc
x,y,z = [_[maskw].mean() for _ in calibkinect.convertOpenNI2Real_numpy(depth)]
d = -(a*x+b*y+c*z)
tableplane = np.array([a,b,c,d])
#tablemean = np.array([x,y,z])
# Backproject the table plane into the image using inverse transpose
global tb0
tb0 = np.dot(KK.T, tableplane)
tb0[2] = tb0[2]
# Build a matrix projecting sensor points to an system with
# the origin on the table, and Y pointing up from the table
# NOTE: the default orientation is offset by 45 degrees from the camera's
# natural direction.
v1 = np.array([a,b,c]); v1 /= np.sqrt(np.dot(v1,v1))
v0 = np.cross(v1, [-1,0,1]); v0 /= np.sqrt(np.dot(v0,v0))
v2 = np.cross(v0,v1);
Ktable = np.eye(4)
Ktable[:3,:3] = np.vstack((v0,v1,v2)).T
Ktable[:3,3] = [x,y,z]
Ktable = np.linalg.inv(Ktable).astype('f')
KtableKK = np.dot(Ktable, KK).astype('f')
global boundptsM
boundptsM = []
for (up,vp) in boundpts:
# First project the image points (u,v) onto to the (imaged) tableplane
dp = -(tb0[0]*up + tb0[1]*vp + tb0[3])/tb0[2]
# Then project them into metric space
xp, yp, zp, wp = np.dot(KtableKK, [up,vp,dp,1])
xp /= wp ; yp /= wp ; zp /= wp;
boundptsM += [[xp,yp,zp]]
# Use OpenGL and framebuffers to draw the table and the walls
fbo = glGenFramebuffers(1)
glBindFramebuffer(GL_FRAMEBUFFER, fbo);
rb,rbc = glGenRenderbuffers(2)
glBindRenderbuffer(GL_RENDERBUFFER, rb);
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, 640, 480)
glFramebufferRenderbuffer(GL_FRAMEBUFFER,
GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER, rb);
glEnable(GL_DEPTH_TEST)
glClear(GL_DEPTH_BUFFER_BIT)
glViewport(0, 0, 640, 480)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0,640,0,480,0,-10)
glMultMatrixf(np.linalg.inv(KtableKK).T)
def draw():
glClear(GL_DEPTH_BUFFER_BIT|GL_COLOR_BUFFER_BIT)
glEnable(GL_CULL_FACE)
glBegin(GL_QUADS)
for x,y,z in boundptsM:
glVertex(x,y,z,1)
for (x,y,z),(x_,y_,z_) in zip(boundptsM,np.roll(boundptsM,1,0)):
glVertex(x ,y ,z ,1)
glVertex(x_,y_,z_,1)
glVertex(0,1,0,0)
glVertex(0,1,0,0)
glEnd()
glDisable(GL_CULL_FACE)
glFinish()
# Rendering the outside faces gives us the near walls
gf = glGetIntegerv(GL_FRONT_FACE)
glFrontFace(GL_CCW)
draw()
openglbgHi = glReadPixels(0, 0, 640, 480,
GL_DEPTH_COMPONENT, GL_FLOAT).reshape(480,640)
# Rendering the interior faces gives us the table plane and the far walls
glFrontFace(GL_CW)
draw()
openglbgLo = glReadPixels(0, 0, 640, 480,
GL_DEPTH_COMPONENT, GL_FLOAT).reshape(480,640)
glFrontFace(gf)
# We need to invert the depth measurements to obtain kinect-style range
# images.
# initially, openglbgHi/Lo are in units of 1/m.
# afterwards, they are in units of mm.
global hi,lo
openglbgHi = 1000./(openglbgHi*10)
openglbgLo = 1000./(openglbgLo*10)
lo = np.array(openglbgLo)
hi = np.array(openglbgHi)
openglbgLo[openglbgHi==openglbgLo] = 0
glBindFramebuffer(GL_FRAMEBUFFER, 0);
glDeleteRenderbuffers(2, [rb,rbc]);
glDeleteFramebuffers(1, [fbo]);
# Some of the observed image points are nearer than the fitted plane.
# We want fewer false positives in this case, so take the maximum
# of either the fitted plane or the observed depth
background = np.array(depth)
background[~mask] = 0
background = np.maximum(background,openglbgHi)
openglbgLo = openglbgLo.astype(np.uint16)
background = background.astype(np.uint16)
background[background>=5] -= 5 # Reduce false positives even more.
return dict(
bgLo=openglbgLo,
bgHi=background,
boundpts=boundpts,
boundptsM=boundptsM,
KK=KK,
Ktable=Ktable)
def find_plane(depth, boundpts):
from wxpy3d.camerawindow import CameraWindow
global window
if not 'window' in globals():
window = CameraWindow()
# Build a mask of the image inside the convex points clicked
mask = make_mask(boundpts)
# Borrow the initialization from calibkinect
KK = np.linalg.inv(calibkinect.projection()).astype('f')
KK = np.ascontiguousarray(KK)
# Find the average plane going through here
global n, w
n, w = normals.normals_c(depth)
maskw = mask & (w > 0)
abc = n[maskw].mean(0)
abc /= np.sqrt(np.dot(abc, abc))
a, b, c = abc
x, y, z = [
_[maskw].mean() for _ in calibkinect.convertOpenNI2Real_numpy(depth)
]
d = -(a * x + b * y + c * z)
tableplane = np.array([a, b, c, d])
#tablemean = np.array([x,y,z])
# Backproject the table plane into the image using inverse transpose
global tb0
tb0 = np.dot(KK.T, tableplane)
tb0[2] = tb0[2]
# Build a matrix projecting sensor points to an system with
# the origin on the table, and Y pointing up from the table
# NOTE: the default orientation is offset by 45 degrees from the camera's
# natural direction.
v1 = np.array([a, b, c])
v1 /= np.sqrt(np.dot(v1, v1))
v0 = np.cross(v1, [-1, 0, 1])
v0 /= np.sqrt(np.dot(v0, v0))
v2 = np.cross(v0, v1)
Ktable = np.eye(4)
Ktable[:3, :3] = np.vstack((v0, v1, v2)).T
Ktable[:3, 3] = [x, y, z]
Ktable = np.linalg.inv(Ktable).astype('f')
KtableKK = np.dot(Ktable, KK).astype('f')
global boundptsM
boundptsM = []
for (up, vp) in boundpts:
# First project the image points (u,v) onto to the (imaged) tableplane
dp = -(tb0[0] * up + tb0[1] * vp + tb0[3]) / tb0[2]
# Then project them into metric space
xp, yp, zp, wp = np.dot(KtableKK, [up, vp, dp, 1])
xp /= wp
yp /= wp
zp /= wp
boundptsM += [[xp, yp, zp]]
# Use OpenGL and framebuffers to draw the table and the walls
fbo = glGenFramebuffers(1)
glBindFramebuffer(GL_FRAMEBUFFER, fbo)
rb, rbc = glGenRenderbuffers(2)
glBindRenderbuffer(GL_RENDERBUFFER, rb)
glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, 640, 480)
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT,
GL_RENDERBUFFER, rb)
glEnable(GL_DEPTH_TEST)
glClear(GL_DEPTH_BUFFER_BIT)
glViewport(0, 0, 640, 480)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glOrtho(0, 640, 0, 480, 0, -10)
glMultMatrixf(np.linalg.inv(KtableKK).T)
def draw():
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT)
glEnable(GL_CULL_FACE)
glBegin(GL_QUADS)
for x, y, z in boundptsM:
glVertex(x, y, z, 1)
for (x, y, z), (x_, y_, z_) in zip(boundptsM, np.roll(boundptsM, 1,
0)):
glVertex(x, y, z, 1)
glVertex(x_, y_, z_, 1)
glVertex(0, 1, 0, 0)
glVertex(0, 1, 0, 0)
glEnd()
glDisable(GL_CULL_FACE)
glFinish()
# Rendering the outside faces gives us the near walls
gf = glGetIntegerv(GL_FRONT_FACE)
glFrontFace(GL_CCW)
draw()
openglbgHi = glReadPixels(0, 0, 640, 480, GL_DEPTH_COMPONENT,
GL_FLOAT).reshape(480, 640)
# Rendering the interior faces gives us the table plane and the far walls
glFrontFace(GL_CW)
draw()
openglbgLo = glReadPixels(0, 0, 640, 480, GL_DEPTH_COMPONENT,
GL_FLOAT).reshape(480, 640)
glFrontFace(gf)
# We need to invert the depth measurements to obtain kinect-style range
# images.
# initially, openglbgHi/Lo are in units of 1/m.
# afterwards, they are in units of mm.
global hi, lo
openglbgHi = 1000. / (openglbgHi * 10)
openglbgLo = 1000. / (openglbgLo * 10)
lo = np.array(openglbgLo)
hi = np.array(openglbgHi)
openglbgLo[openglbgHi == openglbgLo] = 0
glBindFramebuffer(GL_FRAMEBUFFER, 0)
glDeleteRenderbuffers(2, [rb, rbc])
glDeleteFramebuffers(1, [fbo])
# Some of the observed image points are nearer than the fitted plane.
# We want fewer false positives in this case, so take the maximum
# of either the fitted plane or the observed depth
background = np.array(depth)
background[~mask] = 0
background = np.maximum(background, openglbgHi)
openglbgLo = openglbgLo.astype(np.uint16)
background = background.astype(np.uint16)
background[background >= 5] -= 5 # Reduce false positives even more.
return dict(bgLo=openglbgLo,
bgHi=background,
boundpts=boundpts,
boundptsM=boundptsM,
KK=KK,
Ktable=Ktable)
