def translated_rotated(R_correct, occ):
# Apply a 90 degree rotation
angle = np.arctan2(R_correct[0,2], R_correct[0,0])
angle_90 = np.round(angle/(np.pi/2))
# Apply a translation
bx,_,bz = np.round(np.linalg.inv(R_correct)[:3,3]/config.LW).astype('i')
return hashalign.apply_correction(occ, bx, 0, bz, -angle_90)
def translated_rotated(R_correct, occ):
# Apply a 90 degree rotation
angle = np.arctan2(R_correct[0, 2], R_correct[0, 0])
angle_90 = np.round(angle / (np.pi / 2))
# Apply a translation
bx, _, bz = np.round(np.linalg.inv(R_correct)[:3, 3] /
config.LW).astype('i')
return hashalign.apply_correction(occ, bx, 0, bz, -angle_90)
def centered_rotated(R_correct, occ):
"""
Args:
R_correct: model matrix
occ: 3D voxel grid
Returns:
3D voxel grid
"""
# Apply a 90 degree rotation
angle = np.arctan2(R_correct[0,2], R_correct[0,0])
angle_90 = np.round(angle/(np.pi/2))
return hashalign.apply_correction(occ, 0, 0, 0, -angle_90)
def centered_rotated(R_correct, occ):
"""
Args:
R_correct: model matrix
occ: 3D voxel grid
Returns:
3D voxel grid
"""
# Apply a 90 degree rotation
angle = np.arctan2(R_correct[0, 2], R_correct[0, 0])
angle_90 = np.round(angle / (np.pi / 2))
return hashalign.apply_correction(occ, 0, 0, 0, -angle_90)
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)
