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 lin_rigid_tps_transform(tps_transform, lin_point):
lin_ag, trans_g, w_ng, x_na = tps_transform.lin_ag, tps_transform.trans_g, tps_transform.w_ng, tps_transform.x_na
# orientation part is gradient(f)
orien = tps.tps_grad(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0]
# translation part is f(a) - gradient(f)*a
trans = tps.tps_eval(np.array([lin_point]), lin_ag, trans_g, w_ng, x_na)[0] - np.dot(orien, lin_point)
linearized_transform = juc.trans_rot_to_hmat(trans, juc.mat2quat(orthogonalize3_cross(np.array([orien]))))
return linearized_transform
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)
