def check_that_nr_fit_runs(): from jds_image_proc.clouds import voxel_downsample #from brett2.ros_utils import RvizWrapper #import lfd.registration as lr ##import lfd.warping as lw #if rospy.get_name() == "/unnamed": #rospy.init_node("test_rigidity", disable_signals=True) #from time import sleep #sleep(1) #rviz = RvizWrapper.create() pts0 = np.loadtxt("../test/rope_control_points.txt") pts1 = np.loadtxt("../test/bad_rope.txt") pts_rigid = voxel_downsample(pts0[:10], .02) #lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) lin_ag, trans_g, w_eg, x_ea = tps.tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton",plotting=1) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) #lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps.tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea)
def check_that_nr_fit_runs(): from jds_image_proc.clouds import voxel_downsample #from brett2.ros_utils import RvizWrapper #import lfd.registration as lr ##import lfd.warping as lw #if rospy.get_name() == "/unnamed": #rospy.init_node("test_rigidity", disable_signals=True) #from time import sleep #sleep(1) #rviz = RvizWrapper.create() pts0 = np.loadtxt("../test/rope_control_points.txt") pts1 = np.loadtxt("../test/bad_rope.txt") pts_rigid = voxel_downsample(pts0[:10], .02) #lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) lin_ag, trans_g, w_eg, x_ea = tps.tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton", plotting=0) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) #lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps.tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea)
def jacobian_of_tps(): import numdifftools as ndt D = 3 pts0 = np.random.randn(100,D) pts_eval = np.random.randn(20,D) lin_ag, trans_g, w_ng, x_na = np.random.randn(D,D), np.random.randn(D), np.random.randn(len(pts0), D), pts0 def eval_partial(x_ma_flat): x_ma = x_ma_flat.reshape(-1,D) return tps.tps_eval(x_ma, lin_ag, trans_g, w_ng, pts0) for i in xrange(len(pts_eval)): rots = ndt.Jacobian(eval_partial)(pts_eval[i]) rots1 = tps.tps_grad(pts_eval[i:i+1], lin_ag, trans_g, w_ng, pts0) assert np.allclose(rots1, rots) D = 2 pts0 = np.random.randn(100,D) pts_eval = np.random.randn(20,D) lin_ag, trans_g, w_ng, x_na = np.random.randn(D,D), np.random.randn(D), np.random.randn(len(pts0), D), pts0 for i in xrange(len(pts_eval)): rots = ndt.Jacobian(eval_partial)(pts_eval[i]) rots1 = tps.tps_grad(pts_eval[i:i+1], lin_ag, trans_g, w_ng, pts0) assert np.allclose(rots1, rots)
def jacobian_of_tps(): import numdifftools as ndt D = 3 pts0 = np.random.randn(100, 3) pts_eval = np.random.randn(20, D) lin_ag, trans_g, w_ng, x_na = np.random.randn( D, D), np.random.randn(D), np.random.randn(len(pts0), D), pts0 def eval_partial(x_ma_flat): x_ma = x_ma_flat.reshape(-1, 3) return tps.tps_eval(x_ma, lin_ag, trans_g, w_ng, pts0) for i in xrange(len(pts_eval)): rots = ndt.Jacobian(eval_partial)(pts_eval[i]) rots1 = tps.tps_grad(pts_eval[i:i + 1], lin_ag, trans_g, w_ng, pts0) assert np.allclose(rots1, rots)
def compute_jacobian(self, x_ma): grad_mga = tps.tps_grad(x_ma, self.lin_ag, self.trans_g, self.w_ng, self.x_na) return grad_mga