def fit_tps(x_gt, x, plot=True): """ Fits a thin-plate spline model to x (source points) and x_gt (ground-truth target points). This transform can be used to correct the state-dependent hydra errors. X, X_GT are of the shape : Nx3 """ bend_coeff = 0.1 ## increase this to make the tps-interpolation more smooth, decrease to fit better. f_tps = registration.fit_ThinPlateSpline(x, x_gt, bend_coef = bend_coeff, rot_coef = 0.01*bend_coeff) if plot: plot_reqs = plot_warping(f_tps.transform_points, x, x_gt, fine=False, draw_plinks=True) plotter = PlotterInit() for req in plot_reqs: plotter.request(req) return f_tps
def traj_cost(traj1, traj2, f, g): """ Downsampled traj to have n points from start to end. """ fo = registration.fit_ThinPlateSpline(traj1, traj2, traj_bend_c, traj_rot_c) go = registration.fit_ThinPlateSpline(traj2, traj1, traj_bend_c, traj_rot_c) cost = (registration.tps_reg_cost(fo)+registration.tps_reg_cost(go))/2 if f is not None and g is not None: traj1_f = f.transform_points(traj1) traj2_g = g.transform_points(traj2) fn1 = registration.fit_ThinPlateSpline(traj1_f, traj2, traj_bend_c, traj_rot_c) gn1 = registration.fit_ThinPlateSpline(traj2, traj1_f, traj_bend_c, traj_rot_c) fn2 = registration.fit_ThinPlateSpline(traj1, traj2_g, traj_bend_c, traj_rot_c) gn2 = registration.fit_ThinPlateSpline(traj2_g, traj1, traj_bend_c, traj_rot_c) cost_fg = (registration.tps_reg_cost(fn1)+registration.tps_reg_cost(gn1)+ registration.tps_reg_cost(fn2) +registration.tps_reg_cost(gn2))/4 return cost, cost_fg return cost