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