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()