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()
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()
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
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
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
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
