def animate_random(max_iters=1000, mod=100):
global pnew, points_range
# Apply a perturb to points_p
obj.RT = np.eye(4, dtype='f')
obj.RT[:3,3] = -obj.vertices[:,:3].mean(0)
obj.RT[:3,3] += [0,0,-3.0]
RT = obj.RT
prev_rimg = obj.range_render(camera.kinect_camera())
window.canvas.SetCurrent()
pnew = prev_rimg.point_model(True)
points_range = pnew
if 0:
obj.RT = np.dot(RT, M)
rimg = obj.range_render(camera.kinect_camera())
window.canvas.SetCurrent()
pm = rimg.point_model(True)
points_range = pm
for iters in range(max_iters):
pnew, err, npairs, uv = fasticp.fast_icp(rimg, pnew, 1000, dist=0.005)
if iters % mod == 0:
# print '%d iterations, [%d] RMS: %.3f' % (iters, npairs, np.sqrt(err))
window.Refresh()
pylab.waitforbuttonpress(0.02)
pnew = pm
window.Refresh()
pylab.waitforbuttonpress(0.02)
def sample_solve():
global prev_cam
global range_image, rimg, points
if not 'prev_cam' in globals(): prev_cam = windowcam
# Render the new image from the current camera point of view
# but set the camera matrix to the estimate (previous frame)
rimg = obj.range_render(windowcam)
rimg.camera = prev_cam
rimg.compute_points()
rimg.point_model()
# Predict a frame by racasting the volume
predict_img = vol.raycast_cuda(prev_cam)
pts = predict_img.point_model()
# Compute the fast ICP and update the volume
pnew = pts
for i in range(100):
pnew, err, npairs, uv = fasticp.fast_icp(rimg, pnew, 0.1, dist=0.05)
print err, npairs
RT = rimg.camera.RT
RT = np.dot(RT, np.linalg.inv(pnew.RT))
RT = np.dot(RT, (pts.RT))
prev_cam = rimg.camera = camera.Camera(rimg.camera.KK, RT)
points = pnew.point_model(True)
vol.distance_transform_cuda(rimg)
vol.raycast_cuda(windowcam)
raycast()
window.Refresh()
