示例#1
0
def bigtest():
    from jds_image_proc.clouds import voxel_downsample
    from jds_image_proc.pcd_io import load_xyz
    import mayavi.mlab as mlab

    pts = load_xyz("/home/joschu/Data/scp/three_objs_ds.pcd")
    #pts = voxel_downsample(xyz, .03, False)

    mlab.clf()
    mlab.points3d(pts[:, 0],
                  pts[:, 1],
                  pts[:, 2],
                  color=(1, 1, 1),
                  scale_factor=.01)

    clus = []

    labels = decompose(pts, .025)
    for i in xrange(labels.max() + 1):
        clu = np.flatnonzero(labels == i)
        clus.append(clu)

    for clu in sorted(clus, key=len, reverse=True):
        if len(clu) < 10: break
        dirs = ss.get_sphere_points(1)
        sup_pd = np.dot(pts[clu, :], dirs.T)
        best_d = sup_pd.max(axis=0)
        print "max deficit", (sup_pd - best_d[None, :]).max(axis=1).min()
        mlab.points3d(pts[clu, 0],
                      pts[clu, 1],
                      pts[clu, 2],
                      color=(rand(), rand(), rand()),
                      scale_factor=.01)
        raw_input()
示例#2
0
def bigtest():
    from jds_image_proc.clouds import voxel_downsample
    from jds_image_proc.pcd_io import load_xyz
    import mayavi.mlab as mlab
    
    pts = load_xyz("/home/joschu/Data/scp/three_objs_ds.pcd")
    #pts = voxel_downsample(xyz, .03, False)


    mlab.clf()
    mlab.points3d(pts[:,0], pts[:,1], pts[:,2], color = (1,1,1), scale_factor=.01)
    
    clus = []
    
    labels = decompose(pts, .025)
    for i in xrange(labels.max()+1):
        clu = np.flatnonzero(labels == i)
        clus.append(clu)

    for clu in sorted(clus, key=len, reverse=True):
        if len(clu) < 10: break
        dirs = ss.get_sphere_points(1)
        sup_pd = np.dot(pts[clu,:], dirs.T)
        best_d = sup_pd.max(axis=0)
        print "max deficit",(sup_pd - best_d[None,:]).max(axis=1).min()
        mlab.points3d(pts[clu,0], pts[clu,1], pts[clu,2], color = (rand(),rand(),rand()), scale_factor=.01)
        raw_input()
示例#3
0
def generate_db(db, manip, xlim, ylim, zlim, xres, yres, zres, subdivisions):
    manip_name = manip.GetName()
    group = db.h5file[manip_name]

    erase_all_data(group)

    pointing_axes = sphere_sampling.get_sphere_points(subdivisions)

    xticks = np.arange(xlim[0], xlim[1], xres)
    yticks = np.arange(ylim[0], ylim[1], yres)
    zticks = np.arange(zlim[0], zlim[1], zres)

    nx, ny, nz = xticks.size, yticks.size, zticks.size

    x0, y0, z0 = manip.GetBase().GetTransform()[:3, 3]

    reachable = np.zeros((nx, ny, nz), dtype='uint8')

    t_start = time()
    count = 0
    n_total_ik = len(pointing_axes) * reachable.size
    for (i_ax, x_ax) in enumerate(pointing_axes):

        for (ix, iy, iz) in np.ndindex(nx, ny, nz):
            if count % 200 == 100:
                t_remains = (n_total_ik - count) / count * (time() - t_start)
                print "%i hours, %i minutes, %i seconds remain" % (
                    t_remains // 3600, t_remains // 60 % 60, t_remains % 60)
            #if iz == 0:
            #print "orientation %i of %i. position %i of %i -----"%(i_ax+1, len(pointing_axes), count+1, reachable.size)
            posemat = np.eye(4)
            posemat[:3,
                    3] = [xticks[ix] + x0, yticks[iy] + y0, zticks[iz] + z0]
            posemat[:3, :3] = make_orth_basis(x_ax)
            try:
                solns = manip.FindIKSolutions(posemat, 2 + 16)
                reachable[ix, iy, iz] = len(solns)
            except Exception:
                print "error when solving for ik"
                reachable[ix, iy, iz] = 0
            count += 1

        ax_str = str(i_ax)
        group[ax_str] = reachable

    group["xticks"] = xticks
    group["yticks"] = yticks
    group["zticks"] = zticks
    group["pointing_axes"] = pointing_axes
示例#4
0
def generate_db(db, manip, xlim, ylim, zlim, xres, yres, zres, subdivisions):
    manip_name = manip.GetName()
    group = db.h5file[manip_name]
            
    erase_all_data(group)
            
    pointing_axes = sphere_sampling.get_sphere_points(subdivisions)
    
    xticks = np.arange(xlim[0], xlim[1], xres)
    yticks = np.arange(ylim[0], ylim[1], yres)
    zticks = np.arange(zlim[0], zlim[1], zres)

    nx, ny, nz = xticks.size, yticks.size, zticks.size
    
    x0, y0, z0 = manip.GetBase().GetTransform()[:3,3]
    
    reachable = np.zeros((nx, ny, nz), dtype='uint8')
    
    t_start = time()
    count = 0        
    n_total_ik = len(pointing_axes) * reachable.size
    for (i_ax, x_ax) in enumerate(pointing_axes):
        
        for (ix, iy, iz) in np.ndindex(nx, ny, nz):
            if count % 200 == 100:
                t_remains = (n_total_ik - count) / count * (time() - t_start)
                print "%i hours, %i minutes, %i seconds remain"%(t_remains//3600, t_remains//60%60, t_remains%60)
            #if iz == 0:
                #print "orientation %i of %i. position %i of %i -----"%(i_ax+1, len(pointing_axes), count+1, reachable.size)
            posemat = np.eye(4)
            posemat[:3,3] = [xticks[ix] + x0, yticks[iy] + y0, zticks[iz] + z0]
            posemat[:3,:3] = make_orth_basis(x_ax)
            try:
                solns = manip.FindIKSolutions(posemat, 2+16)
                reachable[ix,iy,iz] = len(solns)
            except Exception:
                print "error when solving for ik"
                reachable[ix, iy, iz] = 0
            count += 1

        ax_str = str(i_ax)
        group[ax_str] = reachable
        
        
    group["xticks"] = xticks
    group["yticks"] = yticks
    group["zticks"] = zticks
    group["pointing_axes"] = pointing_axes
示例#5
0
def decompose(x, thresh):

    if x.shape[1] == 2:
        n_dirs = 8
        dirs = np.array([(cos(x), sin(x))
                         for x in (2 * pi * np.arange(n_dirs)) / n_dirs])
    elif x.shape[1] == 3:
        dirs = ss.get_sphere_points(1)
        n_dirs = len(dirs)

    #dists = ssd.squareform(ssd.pdist(x))
    #pt2nn = dists.argsort(axis=1)[:,:5]
    #def neighbors(ind):
    #return pt2nn[ind]

    tree = kdtree.KDTree(x)

    def neighbors(ind):
        return tree.query(x[ind], 5)[1]

    print "starting"
    n_pts = x.shape[0]

    pt2supports = x.dot(dirs.T)
    pt2label = np.zeros(n_pts, int)
    UNLABELED = -1
    pt2label.fill(UNLABELED)

    i_seed = 0
    i_label = 0
    while True:

        while True:
            if i_seed == n_pts:
                return pt2label
            if pt2label[i_seed] == UNLABELED:
                break
            i_seed += 1

        logging.debug("starting new region with %i", i_seed)

        pt2dirs = {}

        dir2supinfo = [
            supinfo([i_seed], [pt2supports[i_seed][i_dir]],
                    pt2supports[i_seed][i_dir]) for i_dir in xrange(n_dirs)
        ]
        pt2label[i_seed] = i_label
        pt2dirs[i_seed] = set(range(n_dirs))
        frontier = deque()
        rejected = set([])

        frontier.extend(neighbors(i_seed))
        print "neighbs", neighbors(i_seed)

        while len(frontier) > 0:

            ######
            #clu = pt2dirs.keys()
            #sup_pd = x[clu,:].dot(dirs.T)
            #best_d = sup_pd.max(axis=0)
            #for i_dir in xrange(n_dirs):
            #i_nearext = [clu[i] for i in np.flatnonzero(best_d[i_dir] - sup_pd[:,i_dir] < thresh)]
            #assert( set(dir2supinfo[i_dir].inds) == set(i_nearext) )

            ###

            #print dir2supinfo
            #raw_input()

            logging.debug("frontier: %s", frontier)
            i_cur = frontier.popleft()
            if pt2label[i_cur] != UNLABELED or i_cur in rejected: continue
            sup_cur = pt2supports[i_cur]

            logging.debug("checking: %i", i_cur)

            print i_cur

            reject = False

            pt2decrement = defaultdict(int)
            for i_dir in xrange(n_dirs):
                cursup = sup_cur[i_dir]
                if cursup > dir2supinfo[i_dir].best:
                    for (i_pt, sup) in zip(dir2supinfo[i_dir].inds,
                                           dir2supinfo[i_dir].supports):
                        if cursup - sup > thresh:
                            pt2decrement[i_pt] += 1
            for (i_pt, dec) in pt2decrement.items():
                if dec == len(pt2dirs[i_pt]):
                    reject = True
                    logging.debug("rejected because %i would be interior",
                                  i_pt)
            if reject:
                logging.debug("reject")
                print "reject"
                raw_input()
                rejected.add(i_cur)
            if not reject:
                pt2label[i_cur] = i_label
                pt2dirs[i_cur] = set([
                    i_dir for i_dir in xrange(n_dirs)
                    if dir2supinfo[i_dir].best - sup_cur[i_dir] < thresh
                ])
                for i_dir in xrange(n_dirs):
                    cursup = sup_cur[i_dir]
                    if cursup > dir2supinfo[i_dir].best:
                        filtinds, filtsups = [], []
                        for (i_pt, sup) in zip(dir2supinfo[i_dir].inds,
                                               dir2supinfo[i_dir].supports):
                            if cursup - sup >= thresh:
                                pt2dirs[i_pt].remove(i_dir)
                            else:
                                filtinds.append(i_pt)
                                filtsups.append(sup)
                        filtinds.append(i_cur)
                        filtsups.append(cursup)
                        dir2supinfo[i_dir].inds = filtinds
                        dir2supinfo[i_dir].supports = filtsups
                        dir2supinfo[i_dir].best = cursup
                    elif cursup > dir2supinfo[i_dir].best - thresh:
                        dir2supinfo[i_dir].inds.append(i_cur)
                        dir2supinfo[i_dir].supports.append(cursup)
                logging.debug("adding %i. new clu: %s", i_cur, pt2dirs.keys())

                frontier.extend(neighbors(i_cur))
        i_label += 1
示例#6
0
def decompose(x, thresh):
    
    if x.shape[1] == 2:
        n_dirs = 8
        dirs = np.array([(cos(x), sin(x)) for x in (2*pi*np.arange(n_dirs))/n_dirs])
    elif x.shape[1] == 3:
        dirs = ss.get_sphere_points(1)
        n_dirs = len(dirs)
            
    
        
    
    #dists = ssd.squareform(ssd.pdist(x))
    #pt2nn = dists.argsort(axis=1)[:,:5]
    #def neighbors(ind):
        #return pt2nn[ind]
    
    
    tree = kdtree.KDTree(x)
    def neighbors(ind):
        return tree.query(x[ind], 5)[1]
    
    print "starting"
    n_pts = x.shape[0]

    pt2supports = x.dot(dirs.T)
    pt2label = np.zeros(n_pts, int)
    UNLABELED = -1
    pt2label.fill(UNLABELED)
    
    i_seed = 0
    i_label = 0
    while True:
        
        while True:
            if i_seed == n_pts:
                return pt2label
            if pt2label[i_seed] == UNLABELED:
                break
            i_seed += 1
            
        logging.debug("starting new region with %i", i_seed)
        
        pt2dirs = {}        
        
        dir2supinfo = [supinfo([i_seed], [pt2supports[i_seed][i_dir]], pt2supports[i_seed][i_dir]) for i_dir in xrange(n_dirs)]
        pt2label[i_seed] = i_label
        pt2dirs[i_seed] = set(range(n_dirs))
        frontier = deque()
        rejected = set([])
        
        frontier.extend(neighbors(i_seed))
        print "neighbs", neighbors(i_seed)
        
        while len(frontier) > 0:
            
            
            ######
            #clu = pt2dirs.keys()
            #sup_pd = x[clu,:].dot(dirs.T)
            #best_d = sup_pd.max(axis=0)
            #for i_dir in xrange(n_dirs):
                #i_nearext = [clu[i] for i in np.flatnonzero(best_d[i_dir] - sup_pd[:,i_dir] < thresh)]
                #assert( set(dir2supinfo[i_dir].inds) == set(i_nearext) )
                
                
            
            
            ###            
            
            
            #print dir2supinfo
            #raw_input()

            logging.debug("frontier: %s",frontier)
            i_cur = frontier.popleft()
            if pt2label[i_cur] != UNLABELED or i_cur in rejected: continue
            sup_cur = pt2supports[i_cur]

            logging.debug("checking: %i", i_cur)
            
            print i_cur
            
            reject = False
            
            pt2decrement = defaultdict(int)                        
            for i_dir in xrange(n_dirs):
                cursup = sup_cur[i_dir]
                if cursup > dir2supinfo[i_dir].best:
                    for (i_pt, sup) in zip(dir2supinfo[i_dir].inds, dir2supinfo[i_dir].supports):                        
                        if cursup - sup > thresh:
                            pt2decrement[i_pt] += 1
            for (i_pt, dec) in pt2decrement.items():
                if dec == len(pt2dirs[i_pt]):
                    reject = True
                    logging.debug("rejected because %i would be interior", i_pt)
            if reject: 
                logging.debug("reject")     
                print "reject"
                raw_input()
                rejected.add(i_cur)
            if not reject:
                pt2label[i_cur] = i_label
                pt2dirs[i_cur] = set([i_dir for i_dir in xrange(n_dirs) if dir2supinfo[i_dir].best - sup_cur[i_dir] < thresh])                
                for i_dir in xrange(n_dirs):
                    cursup = sup_cur[i_dir]
                    if cursup > dir2supinfo[i_dir].best:
                        filtinds, filtsups = [],[]
                        for (i_pt, sup) in zip(dir2supinfo[i_dir].inds, dir2supinfo[i_dir].supports):                        
                            if cursup - sup >= thresh:
                                pt2dirs[i_pt].remove(i_dir)
                            else:
                                filtinds.append(i_pt)
                                filtsups.append(sup)
                        filtinds.append(i_cur)
                        filtsups.append(cursup)
                        dir2supinfo[i_dir].inds = filtinds
                        dir2supinfo[i_dir].supports = filtsups
                        dir2supinfo[i_dir].best = cursup
                    elif cursup > dir2supinfo[i_dir].best - thresh:
                        dir2supinfo[i_dir].inds.append(i_cur)
                        dir2supinfo[i_dir].supports.append(cursup)
                logging.debug("adding %i. new clu: %s", i_cur, pt2dirs.keys())
                        
                        
                frontier.extend(neighbors(i_cur))
        i_label += 1