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 normals_opencl(depth, mask=None, rect=((0,0),(640,480)), win=6):
def from_rect(m,rect):
(l,t),(r,b) = rect
return m[t:b,l:r]
opencl.set_rect(rect)
depth = from_rect(depth,rect)
(l,t),(r,b) = rect
assert depth.dtype == np.uint16
assert depth.shape == (b-t, r-l)
#depth[depth==0] = -1e8 # 2047 this is taken care of by recip_depth
if mask is None:
mask = np.ones((b-t,r-l),'bool')
else:
mask = np.array(from_rect(mask,rect))
global filt
depth = np.ascontiguousarray(depth)
depth = calibkinect.recip_depth_openni(depth)
filt = scipy.ndimage.uniform_filter(depth,win)
opencl.load_filt(filt)
opencl.load_raw(depth)
opencl.load_mask(mask)
return opencl.compute_normals().wait()
def filter(self, win=7):
"""Creates @depth_filtered and @depth_recip"""
depth = from_rect(self.depth, self.rect)
depth = np.ascontiguousarray(depth)
depth = calibkinect.recip_depth_openni(depth)
self.depth_recip = depth
depth = scipy.ndimage.uniform_filter(depth,win)
self.depth_filtered = depth
def compute_points(self):
mat = np.dot(self.camera.RT, self.camera.KK)
# TODO: Make this faster by following the region of interest
# Convert the depth to units of (1./meters)
depth = calibkinect.recip_depth_openni(self.depth)
v,u = np.mgrid[:480,:640].astype('f')
X,Y,Z = u, v, depth
x = X*mat[0,0] + Y*mat[0,1] + Z*mat[0,2] + mat[0,3]
y = X*mat[1,0] + Y*mat[1,1] + Z*mat[1,2] + mat[1,3]
z = X*mat[2,0] + Y*mat[2,1] + Z*mat[2,2] + mat[2,3]
w = X*mat[3,0] + Y*mat[3,1] + Z*mat[3,2] + mat[3,3]
w = 1/w
self.xyz = np.ascontiguousarray(np.dstack((x*w,y*w,z*w)))
def compute_normals(self):
"""Computes the normals, with KK *and* RT applied. The stored points
are in global coordinates.
"""
v,u = from_rect(np.mgrid, self.rect)
if 'depth_filtered' in self.__dict__:
depth = self.depth_filtered
else:
depth = calibkinect.recip_depth_openni(from_rect(self.depth, self.rect))
dx = (np.roll(depth,-1,1) - np.roll(depth,1,1))/2
dy = (np.roll(depth,-1,0) - np.roll(depth,1,0))/2
X,Y,Z,W = -dx, -dy, 0*dy+1, -(-dx*u + -dy*v + depth).astype(np.float32)
mat = np.linalg.inv(self.camera.KK).transpose()
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
x,y,z = (_ / w for _ in (x,y,z))
weights = z*0+1
weights[depth<-1000] = 0
weights[~from_rect(self.mask,self.rect)] = 0
weights[z<=.1] = 0
weights[np.abs(dx)+np.abs(dy) > 10] = 0
mat = self.camera.RT
x_ = x*mat[0,0] + y*mat[0,1] + z*mat[0,2]
y_ = x*mat[1,0] + y*mat[1,1] + z*mat[1,2]
z_ = x*mat[2,0] + y*mat[2,1] + z*mat[2,2]
#self.normals = np.empty(self.depth.shape+(3,), 'f')
#self.normals[t:b,l:r] = np.dstack((x_,y_,z_))
self.normals = np.ascontiguousarray(np.dstack((x_,y_,z_)))
self.weights = weights
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
