Esempio n. 1
0
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)
Esempio n. 2
0
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)
Esempio n. 3
0
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)
Esempio n. 4
0
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)
Esempio n. 5
0
 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
Esempio n. 6
0
 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