def get_scene_dist(demo_clouds, exp_clouds): assert len(demo_clouds) == len(exp_clouds) total_cost = 0 for demo_cloud, exp_cloud in zip(demo_clouds, exp_clouds): warp_func = registration.tps_rpm_zrot(demo_cloud, exp_cloud, reg_init=2, reg_final=.5, n_iter=10, verbose=False) total_cost += warp_func.cost return total_cost
def test_pcs(fname_src,fname_targ): import rospy if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) src = np.loadtxt(fname_src) targ = np.loadtxt(fname_targ) good_inds = targ[:,0].argsort()[:-3] targ = targ[good_inds,:] f = registration.tps_rpm_zrot(src,targ, plotting=1,reg_init=2,reg_final=.5,n_iter=8, verbose=False, dist2_per_pt_per_radian=1e-6)
def test_zrot2(): #def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_utils.math_utils import normr, norms if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) xyz1 = np.random.randn(300,3)*.2 xyz2 = np.random.randn(300,3)*.2 xyz1 = xyz1[norms(xyz1,1) < 1]*.2 xyz2 = xyz2[norms(xyz1,1) < 1]*.2 from numpy import sin, cos, pi f = registration.tps_rpm_zrot(xyz1, xyz2, plotting=1,reg_init=2,reg_final=.05,n_iter=8, verbose=False, cost_per_radian = 2)
def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_image_proc.pcd_io import load_xyzrgb import h5py if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/python/lfd/data" f = h5py.File(osp.join(data_dir, "images/pickup-plate0.seg.h5"),"r") xyz1, rgb1 = np.asarray(f["plate"]["xyz"]), np.asarray(f["plate"]["rgb"]) xyz1 = voxel_downsample(xyz1, .02) from numpy import sin, cos, pi xyz_target = xyz1.mean(axis=0)[None,:] + 1.5*np.dot(xyz1 - xyz1.mean(axis=0)[None,:], np.array([[cos(pi/3), sin(pi/3), 0], [-sin(pi/3), cos(pi/3), 0], [0, 0, 1]])) f = registration.tps_rpm_zrot(xyz1, xyz_target, plotting=200,reg_init=2,reg_final=.5,n_iter=8, verbose=False)
def get_warping_transform(from_cloud, to_cloud, transform_type="tps"): if transform_type == "tps": return registration.tps_rpm(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False) elif transform_type == "tps_zrot": return registration.tps_rpm_zrot(from_cloud, to_cloud, plotting=2, reg_init=2, reg_final=.5, n_iter=10, verbose=False) elif transform_type == "translation2d": warp = registration.Translation2d() warp.fit(from_cloud, to_cloud) return warp elif transform_type == "rigid2d": warp = registration.Rigid2d() warp.fit(from_cloud, to_cloud) return warp else: raise Exception("transform type %s is not yet implemented" % transform_type)
def test_zrot1(): #def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_image_proc.pcd_io import load_xyzrgb import h5py if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) data_dir = "/home/joschu/python/lfd/data" f = h5py.File(osp.join(data_dir, "images/pickup-plate0.seg.h5"),"r") xyz1, rgb1 = np.asarray(f["plate"]["xyz"]), np.asarray(f["plate"]["rgb"]) xyz1 = voxel_downsample(xyz1, .02) from numpy import sin, cos, pi print "true angle", pi/3 xyz_target = xyz1.mean(axis=0)[None,:] + 1.5*np.dot(xyz1 - xyz1.mean(axis=0)[None,:], np.array([[cos(pi/3), sin(pi/3), 0], [-sin(pi/3), cos(pi/3), 0], [0, 0, 1]])) f = registration.tps_rpm_zrot(xyz1, xyz_target, plotting=1,reg_init=2,reg_final=.5,n_iter=8, verbose=False, cost_per_radian=2)
#return from mayavi import mlab fignum=0 fignum = mlab.figure(0) mlab.clf(figure=fignum) x,y,z = x_nd.T mlab.points3d(x,y,z, color=(1,0,0),scale_factor=.01,figure=fignum) x,y,z, = f.transform_points(x_nd).T mlab.points3d(x,y,z, color=(0,1,0),scale_factor=.01,figure=fignum) x,y,z = y_md.T mlab.points3d(x,y,z, color=(0,0,1),scale_factor=.01,figure=fignum) #raw_input() for other in group["others"]: xyz_other = np.loadtxt(osp.join("../test/test_pcs", other["file"])) f = registration.tps_rpm_zrot(xyz_base, xyz_other,reg_init=2,reg_final=.5,n_iter=9, verbose=True, rot_param = (0.01,0.01,0.0025),scale_param=.01, plot_cb = plot_cb, plotting=1) T_calc = np.c_[ f.lin_ag.T, f.trans_g.reshape(3,1) ] # (.01,.01,.005) print "result" print T_calc print "axis-angle:", print axisanglepart(T_calc[:3,:3]) if "transform" in other: T_other_base = np.array(other["transform"]).reshape(4,4) print "actual" print T_other_base import transform_gui transformer = transform_gui.CloudAffineTransformer(xyz_base, xyz_other, T_calc) transformer.configure_traits()