def registration(xys, fxys):
    if args.to3d:
        xys = np.c_[xys, np.zeros((len(xys)))]
        fxys = np.c_[fxys, np.zeros((len(fxys)))]

    scaled_xys, src_params = reg.unit_boxify(xys)
    scaled_fxys, targ_params = reg.unit_boxify(fxys)

    downsample = voxel_downsample if args.to3d else pixel_downsample
    scaled_ds_xys = downsample(scaled_xys, .03)
    scaled_ds_fxys = downsample(scaled_fxys, .03)

    print "downsampled to %i and %i pts" % (len(scaled_ds_xys),
                                            len(scaled_ds_fxys))
    tstart = time()

    # fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
    fest_scaled, _ = reg.tps_rpm_bij(scaled_ds_xys,
                                     scaled_ds_fxys,
                                     n_iter=10,
                                     reg_init=10,
                                     reg_final=.01)

    print "time: %.4f" % (time() - tstart)
    fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
    fxys_est = fest.transform_points(xys)
    if len(fxys_est) == len(fxys):
        print "error:", np.abs(fxys_est - fxys).mean()

    if args.plotting:
        import matplotlib.pyplot as plt
        plt.clf()
        # plt.plot(xys[:,1], xys[:,0],'r.')
        # plt.plot(fxys[:,1], fxys[:,0],'b.')
        # plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')

        scaled_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
        plt.plot(scaled_ds_xys[:, 1], scaled_ds_xys[:, 0], 'r.')
        plt.plot(scaled_ds_fxys[:, 1], scaled_ds_fxys[:, 0], 'b.')
        plt.plot(scaled_ds_fxys_est[:, 1], scaled_ds_fxys_est[:, 0], 'g.')

        def to2d(f):
            def f2d(x):
                return f(np.c_[x, np.zeros((len(x), 1))])[:, :2]

            return f2d

        transform_func = to2d(fest_scaled.transform_points
                              ) if args.to3d else fest_scaled.transform_points
        plot_warped_grid_2d(transform_func, [-.5, -.5], [.5, .5])
        plt.draw()
        plt.ginput()
def registration(xys, fxys):
    if args.to3d:
        xys = np.c_[xys, np.zeros((len(xys)))]
        fxys = np.c_[fxys, np.zeros((len(fxys)))]

    
    scaled_xys, src_params = reg.unit_boxify(xys)
    scaled_fxys, targ_params = reg.unit_boxify(fxys)
    
    downsample = voxel_downsample if args.to3d else pixel_downsample
    scaled_ds_xys = downsample(scaled_xys, .03)
    scaled_ds_fxys = downsample(scaled_fxys, .03)
    
    print "downsampled to %i and %i pts"%(len(scaled_ds_xys),len(scaled_ds_fxys))
    tstart = time()
    
    # fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
    fest_scaled,_ = reg.tps_rpm_bij(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
    
    
    
    print "time: %.4f"%(time()-tstart)
    fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
    fxys_est = fest.transform_points(xys)
    if len(fxys_est) == len(fxys): print "error:", np.abs(fxys_est - fxys).mean()
    
    if args.plotting:
        import matplotlib.pyplot as plt
        plt.clf()
        # plt.plot(xys[:,1], xys[:,0],'r.')
        # plt.plot(fxys[:,1], fxys[:,0],'b.')
        # plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')

        scaled_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
        plt.plot(scaled_ds_xys[:,1], scaled_ds_xys[:,0],'r.')
        plt.plot(scaled_ds_fxys[:,1], scaled_ds_fxys[:,0],'b.')
        plt.plot(scaled_ds_fxys_est[:,1], scaled_ds_fxys_est[:,0],'g.')


        def to2d(f):
            def f2d(x):
                return f(np.c_[x, np.zeros((len(x),1))])[:,:2]
            return f2d
        transform_func = to2d(fest_scaled.transform_points) if args.to3d else fest_scaled.transform_points
        plot_warped_grid_2d(transform_func, [-.5,-.5], [.5,.5])
        plt.draw()
        plt.ginput()
def plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f):
    import matplotlib.pyplot as plt
    plt.clf()
    # plt.plot(xys[:,1], xys[:,0],'r.')
    # plt.plot(fxys[:,1], fxys[:,0],'b.')
    # plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')

    fx_nd = f.transform_points(x_nd)
    plt.plot(x_nd[:,1], x_nd[:,0],'r.')
    plt.plot(fx_nd[:,1], fx_nd[:,0],'g.')
    plt.plot(y_md[:,1], y_md[:,0],'b.')

    def to2d(f):
        def f2d(x):
            return f(np.c_[x, np.zeros((len(x),1))])[:,:2]
        return f2d
    transform_func = to2d(f.transform_points) if args.to3d else f.transform_points
    plot_warped_grid_2d(transform_func, [-.5,-.5], [.5,.5])
    plt.draw()
    plt.ginput()    
Esempio n. 4
0
            print "%.3f elapsed" % (time() - tstart)

            cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(
                gest_scaled)
            print "cost: %.3f" % cost

            import matplotlib.pyplot as plt
            plt.clf()
            # plt.plot(xys[:,1], xys[:,0],'r.')
            # plt.plot(fxys[:,1], fxys[:,0],'b.')
            # plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')

            scaled_fcloud = fest_scaled.transform_points(scaled_cloud0)
            plt.plot(scaled_cloud0[:, 1], scaled_cloud0[:, 0], 'r.')
            plt.plot(scaled_cloud1[:, 1], scaled_cloud1[:, 0], 'b.')
            plt.plot(scaled_fcloud[:, 1], scaled_fcloud[:, 0], 'g.')

            if dim == 2:
                from rapprentice.plotting_plt import plot_warped_grid_2d
                plot_warped_grid_2d(fest_scaled.transform_points, [-.5, -.5],
                                    [.5, .5])

            elif dim == 3:
                env = openravepy.RaveGetEnvironment(1)
                if env is None: env = openravepy.Environment()
                from rapprentice.plotting_openrave import draw_grid
                draw_grid(fest_scaled, [-.5, -.5, -.5], [.5, .5, .5])

            plt.draw()
            raw_input()
            print "%.3f elapsed"%(time() - tstart)
            
            cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(gest_scaled)
            print "cost: %.3f"%cost

            import matplotlib.pyplot as plt
            plt.clf()
            # plt.plot(xys[:,1], xys[:,0],'r.')
            # plt.plot(fxys[:,1], fxys[:,0],'b.')
            # plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')

            scaled_fcloud = fest_scaled.transform_points(scaled_cloud0)
            plt.plot(scaled_cloud0[:,1], scaled_cloud0[:,0],'r.')
            plt.plot(scaled_cloud1[:,1], scaled_cloud1[:,0],'b.')
            plt.plot(scaled_fcloud[:,1], scaled_fcloud[:,0],'g.')
            
            
            if dim == 2: 
                from rapprentice.plotting_plt import plot_warped_grid_2d
                plot_warped_grid_2d(fest_scaled.transform_points, [-.5,-.5], [.5,.5])
            
            
            elif dim == 3:
                env= openravepy.RaveGetEnvironment(1)
                if env is None: env = openravepy.Environment()
                from rapprentice.plotting_openrave import draw_grid
                draw_grid(fest_scaled, [-.5,-.5,-.5], [.5,.5,.5])
            
            plt.draw()
            raw_input()