def tpsrpm_plot_cb(sim_env, x_nd, y_md, targ_Nd, corr_nm, wt_n, f):
ypred_nd = f.transform_points(x_nd)
handles = []
handles.append(sim_env.env.plot3(ypred_nd, 3, (0,1,0,1)))
handles.extend(plotting_openrave.draw_grid(sim_env.env, f.transform_points, x_nd.min(axis=0), x_nd.max(axis=0), xres = .1, yres = .1, zres = .04))
if sim_env.viewer:
sim_env.viewer.Step()
def registration_plot_cb(sim, x_nd, y_md, f):
if sim.viewer:
handles = []
handles.append(sim.env.plot3(x_nd, 5, (1,0,0)))
handles.append(sim.env.plot3(y_md, 5, (0,0,1)))
xwarped_nd = f.transform_points(x_nd)
handles.append(sim.env.plot3(xwarped_nd, 5, (0,1,0)))
handles.extend(plotting_openrave.draw_grid(sim.env, f.transform_points, x_nd.min(axis=0), x_nd.max(axis=0), xres = .1, yres = .1, zres = .04))
sim.viewer.Step()
def draw_grid(sim_env, old_xyz, f, color=(1, 1, 0, 1)):
grid_means = .5 * (old_xyz.max(axis=0) + old_xyz.min(axis=0))
grid_mins = grid_means - (old_xyz.max(axis=0) - old_xyz.min(axis=0))
grid_maxs = grid_means + (old_xyz.max(axis=0) - old_xyz.min(axis=0))
return plotting_openrave.draw_grid(sim_env.env,
f.transform_points,
grid_mins,
grid_maxs,
xres=.1,
yres=.1,
zres=.04,
color=color)
def draw_grid(sim_env, old_xyz, f, color = (1,1,0,1)):
grid_means = .5 * (old_xyz.max(axis=0) + old_xyz.min(axis=0))
grid_mins = grid_means - (old_xyz.max(axis=0) - old_xyz.min(axis=0))
grid_maxs = grid_means + (old_xyz.max(axis=0) - old_xyz.min(axis=0))
return plotting_openrave.draw_grid(sim_env.env, f.transform_points, grid_mins, grid_maxs,
xres = .1, yres = .1, zres = .04, color = color)
