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()
#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()
