Ejemplo n.º 1
0
Archivo: batchtps.py Proyecto: rll/lfd
def batch_tps_rpm_bij(
    src_ctx,
    tgt_ctx,
    T_init=1e-1,
    T_final=5e-3,
    outlierfrac=1e-2,
    outlierprior=1e-1,
    outliercutoff=1e-2,
    em_iter=EM_ITER_CHEAP,
    component_cost=False,
):
    """
    computes tps rpm for the clouds in src and tgt in batch
    TODO: Fill out comment cleanly
    """
    ##TODO: add check to ensure that src_ctx and tgt_ctx are formatted properly
    n_iter = len(src_ctx.bend_coefs)
    T_vals = loglinspace(T_init, T_final, n_iter)

    src_ctx.reset_tps_params()
    tgt_ctx.reset_tps_params()
    for i, b in enumerate(src_ctx.bend_coefs):
        T = T_vals[i]
        for _ in range(em_iter):
            src_ctx.transform_points()
            tgt_ctx.transform_points()
            src_ctx.get_target_points(tgt_ctx, outlierprior, outlierfrac, outliercutoff, T)
            src_ctx.update_transform(b)
            # check_update(src_ctx, b)
            tgt_ctx.update_transform(b)
    return src_ctx.bidir_tps_cost(tgt_ctx, return_components=component_cost)
Ejemplo n.º 2
0
Archivo: batchtps.py Proyecto: rll/lfd
    def __init__(self, bend_coefs=None):
        if bend_coefs is None:
            lambda_init, lambda_final = DEFAULT_LAMBDA
            bend_coefs = np.around(loglinspace(lambda_init, lambda_final, N_ITER_CHEAP), BEND_COEF_DIGITS)
        self.bend_coefs = bend_coefs
        self.ptrs_valid = False
        self.N = 0

        self.tps_params = []
        self.tps_param_ptrs = None
        self.trans_d = []
        self.trans_d_ptrs = None
        self.lin_dd = []
        self.lin_dd_ptrs = None
        self.w_nd = []
        self.w_nd_ptrs = None
        """
        TPS PARAM FORMAT
        [      np.zeros(DATA_DIM)      ]   [  trans_d  ]   [1 x d]
        [       np.eye(DATA_DIM)       ] = [  lin_dd   ] = [d x d]
        [np.zeros((np.zeros, DATA_DIM))]   [   w_nd    ]   [n x d]
        """
        self.default_tps_params = gpuarray.zeros((DATA_DIM + 1 + MAX_CLD_SIZE, DATA_DIM), np.float32)
        self.default_tps_params[1 : DATA_DIM + 1, :].set(np.eye(DATA_DIM, dtype=np.float32))

        self.proj_mats = dict([(b, []) for b in bend_coefs])
        self.proj_mat_ptrs = dict([(b, None) for b in bend_coefs])
        self.offset_mats = dict([(b, []) for b in bend_coefs])
        self.offset_mat_ptrs = dict([(b, None) for b in bend_coefs])

        self.pts = []
        self.pt_ptrs = None
        self.kernels = []
        self.kernel_ptrs = None
        self.pts_w = []
        self.pt_w_ptrs = None
        self.pts_t = []
        self.pt_t_ptrs = None
        self.dims = []
        self.dims_gpu = None
        self.scale_params = []

        self.warp_err = None
        self.bend_res = []
        self.bend_res_ptrs = None

        self.corr_cm = []
        self.corr_cm_ptrs = None
        self.corr_rm = []
        self.corr_rm_ptrs = None
        self.r_coefs = []
        self.r_coef_ptrs = None
        self.c_coefs_rn = []
        self.c_coef_rn_ptrs = None
        self.c_coefs_cn = []
        self.c_coef_cn_ptrs = None

        self.seg_names = []
        self.names2inds = {}
Ejemplo n.º 3
0
Archivo: batchtps.py Proyecto: rll/lfd
def test_batch_tps_rpm_bij(
    src_ctx,
    tgt_ctx,
    T_init=1e-1,
    T_final=5e-3,
    outlierfrac=1e-2,
    outlierprior=1e-1,
    outliercutoff=0.5,
    em_iter=EM_ITER_CHEAP,
    test_ind=0,
):
    from lfd.tpsopt.transformations import ThinPlateSpline, set_ThinPlateSpline

    n_iter = len(src_ctx.bend_coefs)
    T_vals = loglinspace(T_init, T_final, n_iter)

    x_nd = src_ctx.pts[test_ind].get()[: src_ctx.dims[test_ind]]
    y_md = tgt_ctx.pts[0].get()[: tgt_ctx.dims[0]]
    (n, d) = x_nd.shape
    (m, _) = y_md.shape

    f = ThinPlateSpline(d)
    g = ThinPlateSpline(d)

    src_ctx.reset_tps_params()
    tgt_ctx.reset_tps_params()
    for i, b in enumerate(src_ctx.bend_coefs):
        T = T_vals[i]
        for _ in range(em_iter):
            src_ctx.transform_points()
            tgt_ctx.transform_points()

            xwarped_nd = f.transform_points(x_nd)
            ywarped_md = g.transform_points(y_md)
            gpu_xw = src_ctx.pts_w[test_ind].get()[:n, :]
            gpu_yw = tgt_ctx.pts_w[test_ind].get()[:m, :]
            assert np.allclose(xwarped_nd, gpu_xw, atol=1e-5)
            assert np.allclose(ywarped_md, gpu_yw, atol=1e-5)

            xwarped_nd = gpu_xw
            ywarped_md = gpu_yw

            src_ctx.get_target_points(tgt_ctx, outlierprior, outlierfrac, outliercutoff, T)

            fwddist_nm = ssd.cdist(xwarped_nd, y_md, "euclidean")
            invdist_nm = ssd.cdist(x_nd, ywarped_md, "euclidean")
            prob_nm = outlierprior * np.ones((n + 1, m + 1), np.float32)
            prob_nm[:n, :m] = np.exp(-(fwddist_nm + invdist_nm) / float(2 * T))
            prob_nm[n, m] = outlierfrac * np.sqrt(n * m)

            gpu_corr = src_ctx.corr_rm[test_ind].get()
            gpu_corr = gpu_corr.flatten()
            gpu_corr = gpu_corr[: (n + 1) * (m + 1)].reshape(n + 1, m + 1).astype(np.float32)

            assert np.allclose(prob_nm[:n, :m], gpu_corr[:n, :m], atol=1e-5)
            save_prob_nm = np.array(prob_nm)
            save_gpu_corr = np.array(gpu_corr)

            prob_nm[:n, :m] = gpu_corr[:n, :m]

            r_coefs = np.ones(n + 1, np.float32)
            c_coefs = np.ones(m + 1, np.float32)
            a_N = np.ones((n + 1), dtype=np.float32)
            a_N[n] = m * outlierfrac
            b_M = np.ones((m + 1), dtype=np.float32)
            b_M[m] = n * outlierfrac

            for norm_iter_i in range(DEFAULT_NORM_ITERS):
                r_coefs = a_N / prob_nm.dot(c_coefs)
                rn_c_coefs = c_coefs
                c_coefs = b_M / r_coefs.dot(prob_nm)

            gpu_r_coefs = src_ctx.r_coefs[test_ind].get()[: n + 1].reshape(n + 1)
            gpu_c_coefs_cn = src_ctx.c_coefs_cn[test_ind].get()[: m + 1].reshape(m + 1)
            gpu_c_coefs_rn = src_ctx.c_coefs_rn[test_ind].get()[: m + 1].reshape(m + 1)

            r_diff = np.abs(r_coefs - gpu_r_coefs)
            rn_diff = np.abs(rn_c_coefs - gpu_c_coefs_rn)
            cn_diff = np.abs(c_coefs - gpu_c_coefs_cn)

            assert np.allclose(r_coefs, gpu_r_coefs, atol=1e-5)
            assert np.allclose(c_coefs, gpu_c_coefs_cn, atol=1e-5)
            assert np.allclose(rn_c_coefs, gpu_c_coefs_rn, atol=1e-5)

            prob_nm = prob_nm[:n, :m]
            prob_nm *= gpu_r_coefs[:n, None]
            rn_p_nm = prob_nm * gpu_c_coefs_rn[None, :m]
            cn_p_nm = prob_nm * gpu_c_coefs_cn[None, :m]

            wt_n = rn_p_nm.sum(axis=1)
            gpu_corr_cm = src_ctx.corr_cm[test_ind].get().flatten()[: (n + 1) * (m + 1)]
            gpu_corr_cm = gpu_corr_cm.reshape(m + 1, n + 1)  ## b/c it is column major
            assert np.allclose(wt_n, gpu_corr_cm[m, :n], atol=1e-4)

            inlier = wt_n > outliercutoff
            xtarg_nd = np.empty((n, DATA_DIM), np.float32)
            xtarg_nd[inlier, :] = rn_p_nm.dot(y_md)[inlier, :]
            xtarg_nd[~inlier, :] = xwarped_nd[~inlier, :]

            wt_m = cn_p_nm.sum(axis=0)
            assert np.allclose(wt_m, gpu_corr[n, :m], atol=1e-4)

            inlier = wt_m > outliercutoff
            ytarg_md = np.empty((m, DATA_DIM), np.float32)
            ytarg_md[inlier, :] = cn_p_nm.T.dot(x_nd)[inlier, :]
            ytarg_md[~inlier, :] = ywarped_md[~inlier, :]

            xt_gpu = src_ctx.pts_t[test_ind].get()[:n, :]
            yt_gpu = tgt_ctx.pts_t[test_ind].get()[:m, :]
            assert np.allclose(xtarg_nd, xt_gpu, atol=1e-4)
            assert np.allclose(ytarg_md, yt_gpu, atol=1e-4)

            src_ctx.update_transform(b)
            tgt_ctx.update_transform(b)

            f_p_mat = src_ctx.proj_mats[b][test_ind].get()[: n + d + 1, :n]
            f_o_mat = src_ctx.offset_mats[b][test_ind].get()[: n + d + 1]
            b_p_mat = tgt_ctx.proj_mats[b][0].get()[: m + d + 1, :m]
            b_o_mat = tgt_ctx.offset_mats[b][0].get()[: m + d + 1]
            f_params = f_p_mat.dot(xtarg_nd) + f_o_mat
            g_params = b_p_mat.dot(ytarg_md) + b_o_mat

            gpu_fparams = src_ctx.tps_params[test_ind].get()[: n + d + 1]
            gpu_gparams = tgt_ctx.tps_params[test_ind].get()[: m + d + 1]
            assert np.allclose(f_params, gpu_fparams, atol=1e-4)
            assert np.allclose(g_params, gpu_gparams, atol=1e-4)

            set_ThinPlateSpline(f, x_nd, gpu_fparams)
            set_ThinPlateSpline(g, y_md, gpu_gparams)

    f._cost = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, f.x_na, xtarg_nd, 1)
    g._cost = tps.tps_cost(g.lin_ag, g.trans_g, g.w_ng, g.x_na, ytarg_md, 1)

    gpu_cost = src_ctx.bidir_tps_cost(tgt_ctx)
    cpu_cost = f._cost + g._cost
    assert np.isclose(gpu_cost[test_ind], cpu_cost, atol=1e-4)
Ejemplo n.º 4
0
def main():
    scikits.cuda.linalg.init()
    args = parse_arguments()

    f = h5py.File(args.datafile, 'r+')

    bend_coefs = np.around(
        loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter),
        BEND_COEF_DIGITS)

    for seg_name, seg_info in f.iteritems():
        if 'inv' in seg_info:
            if args.replace:
                del seg_info['inv']
                inv_group = seg_info.create_group('inv')
            else:
                inv_group = seg_info['inv']
        else:
            inv_group = seg_info.create_group('inv')
        ds_key = 'DS_SIZE_{}'.format(DS_SIZE)
        if ds_key in inv_group:
            scaled_x_na = inv_group[ds_key]['scaled_cloud_xyz'][:]
            K_nn = inv_group[ds_key]['scaled_K_nn'][:]
        else:
            ds_g = inv_group.create_group(ds_key)
            x_na = downsample_cloud(seg_info[args.cloud_name][:, :3])
            scaled_x_na, scale_params = unit_boxify(x_na)
            K_nn = tps_kernel_matrix(scaled_x_na)
            if args.fill_traj:
                r_traj = seg_info['r_gripper_tool_frame']['hmat'][:, :3, 3]
                l_traj = seg_info['l_gripper_tool_frame']['hmat'][:, :3, 3]
                scaled_r_traj = r_traj * scale_params[0] + scale_params[1]
                scaled_l_traj = l_traj * scale_params[0] + scale_params[1]
                scaled_r_traj_K = tps_kernel_matrix2(scaled_r_traj,
                                                     scaled_x_na)
                scaled_l_traj_K = tps_kernel_matrix2(scaled_l_traj,
                                                     scaled_x_na)
                ds_g['scaled_r_traj'] = scaled_r_traj
                ds_g['scaled_l_traj'] = scaled_l_traj
                ds_g['scaled_r_traj_K'] = scaled_r_traj_K
                ds_g['scaled_l_traj_K'] = scaled_l_traj_K
                # Precompute l,r closing indices
                lr2finger_traj = {}
                for lr in 'lr':
                    arm_name = {"l": "leftarm", "r": "rightarm"}[lr]
                    lr2finger_traj[
                        lr] = gripper_joint2gripper_l_finger_joint_values(
                            np.asarray(seg_info['%s_gripper_joint' %
                                                lr]))[:, None]
                    opening_inds, closing_inds = get_opening_closing_inds(
                        lr2finger_traj[lr])
                    if '%s_closing_inds' % lr in seg_info:
                        del seg_info['%s_closing_inds' % lr]
                    if not closing_inds:
                        closing_inds = False
                    seg_info['%s_closing_inds' % lr] = closing_inds


#
            ds_g['cloud_xyz'] = x_na
            ds_g['scaled_cloud_xyz'] = scaled_x_na
            ds_g['scaling'] = scale_params[0]
            ds_g['scaled_translation'] = scale_params[1]
            ds_g['scaled_K_nn'] = K_nn

        for bend_coef in bend_coefs:
            if str(bend_coef) in inv_group:
                continue

            bend_coef_g = inv_group.create_group(str(bend_coef))
            _, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
            for k, v in res.iteritems():
                bend_coef_g[k] = v

        if args.verbose:
            sys.stdout.write(
                '\rprecomputed tps solver for segment {}'.format(seg_name))
            sys.stdout.flush()
    print ""

    if args.test:
        atol = 1e-7
        print 'Running batch get sol params test with atol = ', atol
        test_batch_get_sol_params(f, [bend_coef], atol=atol)
        print 'batch sol params test succeeded'
    print ""

    bend_coefs = np.around(
        loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,
                    args.exact_n_iter), BEND_COEF_DIGITS)
    for seg_name, seg_info in f.iteritems():
        if 'solver' in seg_info:
            if args.replace:
                del seg_info['solver']
                solver_g = seg_info.create_group('solver')
            else:
                solver_g = seg_info['solver']
        else:
            solver_g = seg_info.create_group('solver')
        x_nd = seg_info['inv'][ds_key]['scaled_cloud_xyz'][:]
        K_nn = seg_info['inv'][ds_key]['scaled_K_nn'][:]
        N, QN, NON, NR = get_exact_solver(x_nd, K_nn, bend_coefs)
        solver_g['N'] = N
        solver_g['QN'] = QN
        solver_g['NR'] = NR
        solver_g['x_nd'] = x_nd
        solver_g['K_nn'] = K_nn
        NON_g = solver_g.create_group('NON')
        for b in bend_coefs:
            NON_g[str(b)] = NON[b]
        if args.verbose:
            sys.stdout.write(
                '\rprecomputed exact tps solver for segment {}'.format(
                    seg_name))
            sys.stdout.flush()
    print ""

    f.close()
Ejemplo n.º 5
0
def main():
    scikits.cuda.linalg.init()
    args = parse_arguments()

    f = h5py.File(args.datafile, 'r+')
    
    bend_coefs = np.around(loglinspace(args.bend_coef_init, args.bend_coef_final, args.n_iter), 
                           BEND_COEF_DIGITS)

    for seg_name, seg_info in f.iteritems():
        if 'inv' in seg_info:
            if args.replace:
                del seg_info['inv'] 
                inv_group = seg_info.create_group('inv')
            else:
                inv_group =  seg_info['inv']
        else:
            inv_group = seg_info.create_group('inv')
        ds_key = 'DS_SIZE_{}'.format(DS_SIZE)
        if ds_key in inv_group:
            scaled_x_na = inv_group[ds_key]['scaled_cloud_xyz'][:]
            K_nn = inv_group[ds_key]['scaled_K_nn'][:]
        else:
            ds_g = inv_group.create_group(ds_key)
            x_na = downsample_cloud(seg_info[args.cloud_name][:, :3])
            scaled_x_na, scale_params = unit_boxify(x_na)
            K_nn = tps_kernel_matrix(scaled_x_na)
            if args.fill_traj:
                r_traj          = seg_info['r_gripper_tool_frame']['hmat'][:, :3, 3]
                l_traj          = seg_info['l_gripper_tool_frame']['hmat'][:, :3, 3]
                scaled_r_traj   = r_traj * scale_params[0] + scale_params[1]
                scaled_l_traj   = l_traj * scale_params[0] + scale_params[1]
                scaled_r_traj_K = tps_kernel_matrix2(scaled_r_traj, scaled_x_na)
                scaled_l_traj_K = tps_kernel_matrix2(scaled_l_traj, scaled_x_na)
                ds_g['scaled_r_traj']      = scaled_r_traj
                ds_g['scaled_l_traj']      = scaled_l_traj
                ds_g['scaled_r_traj_K']    = scaled_r_traj_K
                ds_g['scaled_l_traj_K']    = scaled_l_traj_K
                # Precompute l,r closing indices
                lr2finger_traj = {}
                for lr in 'lr':
                    arm_name = {"l":"leftarm", "r":"rightarm"}[lr]
                    lr2finger_traj[lr] = gripper_joint2gripper_l_finger_joint_values(np.asarray(seg_info['%s_gripper_joint'%lr]))[:,None]
                    opening_inds, closing_inds = get_opening_closing_inds(lr2finger_traj[lr])
                    if '%s_closing_inds'%lr in seg_info:
                        del seg_info['%s_closing_inds'%lr]
                    if not closing_inds:
                        closing_inds = False
                    seg_info['%s_closing_inds'%lr] = closing_inds
#
            ds_g['cloud_xyz']          = x_na
            ds_g['scaled_cloud_xyz']   = scaled_x_na
            ds_g['scaling']            = scale_params[0]
            ds_g['scaled_translation'] = scale_params[1]
            ds_g['scaled_K_nn']        = K_nn


        for bend_coef in bend_coefs:
            if str(bend_coef) in inv_group:
                continue
            
            bend_coef_g = inv_group.create_group(str(bend_coef))
            _, res = get_sol_params(scaled_x_na, K_nn, bend_coef)
            for k, v in res.iteritems():
                bend_coef_g[k] = v

        if args.verbose:
            sys.stdout.write('\rprecomputed tps solver for segment {}'.format(seg_name))
            sys.stdout.flush()
    print ""

    if args.test:
        atol = 1e-7
        print 'Running batch get sol params test with atol = ', atol
        test_batch_get_sol_params(f, [bend_coef], atol=atol)
        print 'batch sol params test succeeded'
    print ""

    bend_coefs = np.around(loglinspace(args.exact_bend_coef_init, args.exact_bend_coef_final,
                                       args.exact_n_iter), 
                           BEND_COEF_DIGITS)
    for seg_name, seg_info in f.iteritems():
        if 'solver' in seg_info:
            if args.replace:
                del seg_info['solver']
                solver_g = seg_info.create_group('solver')
            else:
                solver_g = seg_info['solver']
        else:
            solver_g = seg_info.create_group('solver')
        x_nd = seg_info['inv'][ds_key]['scaled_cloud_xyz'][:]
        K_nn = seg_info['inv'][ds_key]['scaled_K_nn'][:]
        N, QN, NON, NR = get_exact_solver(x_nd, K_nn, bend_coefs)
        solver_g['N']    = N
        solver_g['QN']   = QN
        solver_g['NR']   = NR
        solver_g['x_nd'] = x_nd
        solver_g['K_nn'] = K_nn
        NON_g = solver_g.create_group('NON')
        for b in bend_coefs:
            NON_g[str(b)] = NON[b]
        if args.verbose:
            sys.stdout.write('\rprecomputed exact tps solver for segment {}'.format(seg_name))
            sys.stdout.flush()
    print ""


    f.close()