def tests():
global times
px = []
p_np = []
p_c = []
p_cl = []
iters = 15
for pixels in range(10, 480, 10):
px += [pixels * pixels]
print pixels
rect = ((0, 0), (pixels, pixels))
p_np += [
timeit(
lambda: normals.normals_numpy(depth, rect),
number=iters,
)
]
p_c += [
timeit(
lambda: normals.normals_c(depth, rect),
number=iters,
)
]
p_cl += [
timeit(
lambda: normals.normals_opencl(depth, rect),
number=iters,
)
]
times = np.vstack((px, p_np, p_c, p_cl))
times[1:, :] /= iters
def tests():
global times
px = []
p_np = []
p_c = []
p_cl = []
iters=15
for pixels in range(10,480,10):
px += [pixels*pixels]
print pixels
rect = ((0,0),(pixels,pixels))
p_np += [timeit(lambda:normals.normals_numpy(depth,rect), number=iters,)]
p_c += [timeit(lambda:normals.normals_c(depth,rect), number=iters,)]
p_cl += [timeit(lambda:normals.normals_opencl(depth,rect), number=iters,)]
times = np.vstack((px,p_np,p_c,p_cl))
times[1:,:] /= iters
def update_frame(depth, rgb=None):
def from_rect(m,rect):
(l,t),(r,b) = rect
return m[t:b,l:r]
global mask, rect, modelmat
try:
(mask,rect) = preprocess.threshold_and_mask(depth,config.bg)
except IndexError:
grid.initialize()
modelmat = None
return
# Compute the surface normals
normals.normals_opencl(depth, mask, rect)
# Find the lattice orientation and then translation
global R_oriented, R_aligned, R_correct
R_oriented = lattice.orientation_opencl()
R_aligned = lattice.translation_opencl(R_oriented)
# Use occvac to estimate the voxels from just the current frame
occ, vac = occvac.carve_opencl()
# Further carve out the voxels using spacecarve
warn = np.seterr(invalid='ignore')
try:
vac = vac | spacecarve.carve(depth, R_aligned)
except np.linalg.LinAlgError:
return
np.seterr(divide=warn['invalid'])
if grid.has_previous_estimate() and np.any(grid.occ):
try:
c,err = hashalign.find_best_alignment(grid.occ, grid.vac,
occ, vac,
R_aligned,
grid.previous_estimate['R_correct'])
except ValueError:
#print 'could not align previous'
return None
R_correct = hashalign.correction2modelmat(R_aligned, *c)
occ = occvac.occ = hashalign.apply_correction(occ, *c)
vac = occvac.vac = hashalign.apply_correction(vac, *c)
elif np.any(occ):
# If this is the first estimate (bootstrap) then try to center the grid
if np.any(grid.occ):
# Initialize with ground truth
try:
c,err = hashalign.find_best_alignment(grid.occ, grid.vac,
occ, vac, R_aligned)
R_correct = hashalign.correction2modelmat(R_aligned, *c)
occ = occvac.occ = hashalign.apply_correction(occ, *c)
vac = occvac.vac = hashalign.apply_correction(vac, *c)
except ValueError:
#print 'could not align bootstrap'
return None
else:
R_correct, occ, vac = grid.center(R_aligned, occ, vac)
occvac.occ, occvac.vac = occ, vac
else:
#print 'nothing happened'
return
def matrix_slerp(matA, matB, alpha=0.6):
if matA is None:
return matB
import transformations
qA = transformations.quaternion_from_matrix(matA)
qB = transformations.quaternion_from_matrix(matB)
qC =transformations.quaternion_slerp(qA, qB, alpha)
mat = matB.copy()
mat[:3,3] = (alpha)*matA[:3,3] + (1-alpha)*matB[:3,3]
mat[:3,:3] = transformations.quaternion_matrix(qC)[:3,:3]
return mat
global R_display
R_display = matrix_slerp(R_display, R_correct)
occ_stencil, vac_stencil = grid.stencil_carve(depth, rect,
R_correct, occ, vac,
rgb)
if lattice.is_valid_estimate():
# Run stencil carve and merge
color = stencil.RGB if not rgb is None else None
grid.merge_with_previous(occ, vac, occ_stencil, vac_stencil, color)
grid.update_previous_estimate(R_correct)