def initialize_rope_from_cloud(xyzs, plotting=False):
xyzs = xyzs.reshape(-1,3)
if len(xyzs) > 500: xyzs = xyzs[::len(xyzs)//500]
pdists = ssd.squareform(ssd.pdist(xyzs,'sqeuclidean'))
G = nx.Graph()
for (i_from, row) in enumerate(pdists):
to_inds = np.flatnonzero(row[:i_from] < MAX_DIST**2)
for i_to in to_inds:
G.add_edge(i_from, i_to, weight = pdists[i_from, i_to])
A = nx.floyd_warshall_numpy(G)
A[np.isinf(A)] = 0
(i_from_long, i_to_long) = np.unravel_index(A.argmax(), A.shape)
nodes = G.nodes();
path = nx.shortest_path(G, source=nodes[i_from_long], target=nodes[i_to_long])
xyz_path = xyzs[path,:]
xyzs_unif = curves.unif_resample(xyz_path,N_CTRL_PTS,tol=.005)
labels = np.ones(len(xyzs_unif)-1,'int')
labels[[0,-1]] = 2
if plotting:
import enthought.mayavi.mlab as mlab
mlab.plot3d(*xyzs_unif.T, tube_radius=.001)
mlab.points3d(*xyzs.T, scale_factor=.01)
mlab.show()
return xyzs_unif, labels
def human_get_rope():
point_cloud = rospy.wait_for_message("/camera/depth_registered/points", sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xys = roi.get_polyline(bgr,'draw curve')
uvs = np.int32(xys)[:,::-1]
us,vs = uvs.T
xyzs = xyz[us,vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good"%(len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good,100,tol=.002)
return xyzs_unif
def human_get_rope():
point_cloud = rospy.wait_for_message("/camera/depth_registered/points",
sm.PointCloud2)
xyz, bgr = ros_utils.pc2xyzrgb(point_cloud)
xys = roi.get_polyline(bgr, 'draw curve')
uvs = np.int32(xys)[:, ::-1]
us, vs = uvs.T
xyzs = xyz[us, vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good" % (len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good, 100, tol=.002)
return xyzs_unif
def initialize_rope(xyzs, plotting=False):
pdists = ssd.squareform(ssd.pdist(xyzs))
for (i_from, row) in enumerate(pdists):
to_inds = np.flatnonzero(row[:i_from] < MAX_DIST)
for i_to in to_inds:
G.add_edge(i_from, i_to, weight = pdists[i_from, i_to])
A = nx.floyd_warshall_numpy(G)
A[np.isinf(A)] = 0
(i_from_long, i_to_long) = np.unravel_index(A.argmax(), A.shape)
path = nx.shortest_path(G, source=i_from_long, target=i_to_long)
xyz_path = xyz[path,:]
xyzs_unif = curves.unif_resample(total_path,N_CTRL_PTS,tol=.002)
labels = np.ones(len(xyzs_unif),'int')
labels[[1,-1]] = 2
return xyzs_unif, labels
def initialize_rope(xyzs, plotting=False):
pdists = ssd.squareform(ssd.pdist(xyzs))
for (i_from, row) in enumerate(pdists):
to_inds = np.flatnonzero(row[:i_from] < MAX_DIST)
for i_to in to_inds:
G.add_edge(i_from, i_to, weight=pdists[i_from, i_to])
A = nx.floyd_warshall_numpy(G)
A[np.isinf(A)] = 0
(i_from_long, i_to_long) = np.unravel_index(A.argmax(), A.shape)
path = nx.shortest_path(G, source=i_from_long, target=i_to_long)
xyz_path = xyz[path, :]
xyzs_unif = curves.unif_resample(total_path, N_CTRL_PTS, tol=.002)
labels = np.ones(len(xyzs_unif), 'int')
labels[[1, -1]] = 2
return xyzs_unif, labels
def initialize_rope(label_img, xyz,bgr, plotting=False):
# XXX that sucks
rope_mask = (label_img==1) | ndi.morphology.binary_dilation(label_img==2,np.ones((5,5)))
rope_mask = ndi.binary_opening(rope_mask, np.ones((3,3)))
rope_mask = ndi.binary_dilation(rope_mask, np.ones((15,15)))
skel = mip.skeletonize(rope_mask)
if plotting:
cv2.imshow('bgr',bgr.copy())
cv2.imshow('labels',label_img.astype('uint8')*50)
cv2.imshow("skel", skel.astype('uint8')*50)
cv2.imshow("rope_mask", rope_mask.astype('uint8')*50)
cv2.waitKey(5)
#start_end = (uv_start, uv_end) = get_cc_centers(label_img==2,2)
G = skel2graph(skel)
path2d = longest_shortest_path(G)
path3d = to3d(path2d,xyz)
xyzs_unif = curves.unif_resample(path3d,N_CTRL_PTS,tol=.0025)
#us,vs = xyz2uv(xyzs_unif).T
#labels = label_img[us,vs]
labels = np.ones(xyzs_unif.shape[0]-1,'int')
labels[0] = labels[-1] = 2
if plotting:
xs,ys,zs = xyzs_unif.T
us_skel, vs_skel = np.nonzero(skel)
xs_skel, ys_skel, zs_skel = to3d(np.c_[us_skel, vs_skel], xyz).T
import matplotlib.pyplot as plt, enthought.mayavi.mlab as mlab
mlab.plot3d(xs,ys,zs,np.arange(len(xs)),tube_radius=.0025, colormap='spectral')
mlab.points3d(xs_skel, ys_skel, zs_skel, scale_factor=.02, color=(1,1,1),opacity=.1)
for (i,path) in enumerate(path3d):
mlab.text3d(path[0,0],path[0,1],path[0,2],str(2*i),scale=.01,color=(0,0,0))
mlab.text3d(path[-1,0],path[-1,1],path[-1,2],str(2*i+1),scale=.01,color=(0,0,0))
mlab.show()
return xyzs_unif, labels
def initialize_rope(label_img, xyz,bgr, plotting=False):
rope_mask = (label_img==1) | ndi.morphology.binary_dilation(label_img==2,np.ones((5,5)))
rope_mask = ndi.binary_opening(rope_mask, np.ones((3,3)))
if plotting:
cv2.imshow('bgr',bgr.copy())
cv2.imshow('labels',label_img.astype('uint8')*50)
cv2.waitKey(5)
start_end = (uv_start, uv_end) = get_cc_centers(label_img==2,2)
skel = mip.skeletonize(rope_mask)
skel = skeletons.removeSpurs1(skel,10)
if plotting: cv2.imshow('skel',skel*100)
paths2d = skeletons.get_paths_2d(skel)
endpts2d = []
for path in paths2d:
endpts2d.append(path[0])
endpts2d.append(path[-1])
i_start = nearest_neighbor(uv_start, endpts2d)
i_end = nearest_neighbor(uv_end, endpts2d)
print "start,end",i_start,i_end
paths3d = [to3d(path,xyz) for path in paths2d]
if plotting:
starts_ends = []
for path in paths2d:
starts_ends.append(path[0])
starts_ends.append(path[-1])
plot_colorskel(paths2d,bgr,start_end,starts_ends)
C = skeletons.make_cost_matrix(paths3d)
PG = skeletons.make_path_graph(C, [len(path) for path in paths3d])
(_,nodes) = skeletons.longest_path_between(PG,i_start, set([]), i_end)
if nodes is None: raise Exception("longest path failed")
print nodes
total_path = []
for node in nodes[::2]:
if node%2 == 0:
total_path.extend(paths3d[node//2])
else:
total_path.extend(paths3d[node//2][::-1])
total_path = np.array(total_path)
xyzs_unif = curves.unif_resample(total_path,N_SEGS+1,tol=.0025)
#us,vs = xyz2uv(xyzs_unif).T
#labels = label_img[us,vs]
labels = np.ones(xyzs_unif.shape[0]-1,'int')
labels[0] = labels[-1] = 2
if plotting:
xs,ys,zs = xyzs_unif.T
us_skel, vs_skel = np.nonzero(skel)
xs_skel, ys_skel, zs_skel = to3d(np.c_[us_skel, vs_skel], xyz).T
import matplotlib.pyplot as plt, enthought.mayavi.mlab as mlab
mlab.plot3d(xs,ys,zs,np.arange(len(xs)),tube_radius=.0025, colormap='spectral')
mlab.points3d(xs_skel, ys_skel, zs_skel, scale_factor=.02, color=(1,1,1),opacity=.1)
for (i,path) in enumerate(paths3d):
mlab.text3d(path[0,0],path[0,1],path[0,2],str(2*i),scale=.01,color=(0,0,0))
mlab.text3d(path[-1,0],path[-1,1],path[-1,2],str(2*i+1),scale=.01,color=(0,0,0))
mlab.show()
return xyzs_unif, labels
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('--table_corners')
parser.add_argument("outfile")
args = parser.parse_args()
assert os.path.exists(os.path.dirname(args.outfile))
table_corners = np.loadtxt(args.table_corners)
dx_table = table_corners[1] - table_corners[0]
dy_table = table_corners[3] - table_corners[0]
z = table_corners[0,2]
nrows = 300
length_per_pix = norm(dx_table) / nrows
ncols = norm(dy_table) / length_per_pix
img = np.zeros((nrows, ncols),'uint8')+255
nout = 100
poly = roi.get_polyline(img,"curves")
curve = curves.unif_resample(poly,nout,tol=0)
curve3d = (table_corners[0]+np.r_[0,0,.03])[None,:] + np.c_[curve * length_per_pix, np.zeros((nout,1))]
print curve3d
np.savetxt(args.outfile,curve3d,fmt="%.3f")
#!/usr/bin/env python
from jds_image_proc import interactive_roi as roi
import os,cv2
from os.path import dirname, abspath,join
from jds_image_proc import curves, pcd_io
import numpy as np
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('pcdfile')
parser.add_argument('outfile')
args = parser.parse_args()
xyz,bgr = pcd_io.load_xyzrgb(args.pcdfile)
print xyz.shape, bgr.shape
cv2.namedWindow('draw curve',cv2.cv.CV_WINDOW_NORMAL)
xys = roi.get_polyline(bgr,'draw curve')
uvs = np.int32(xys)[:,::-1]
us,vs = uvs.T
xyzs = xyz[us,vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good"%(len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good,100,tol=.002)
#if bool(args.outfile) and not os.path.exists(dirname(args.outfile)): os.mkdir(dirname(args.outfile))
print "writing",args.outfile
np.savetxt(args.outfile,xyzs_unif)
#!/usr/bin/env python
from jds_image_proc import interactive_roi as roi
import os, cv2
from os.path import dirname, abspath, join
from jds_image_proc import curves, pcd_io
import numpy as np
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('pcdfile')
parser.add_argument('outfile')
args = parser.parse_args()
xyz, bgr = pcd_io.load_xyzrgb(args.pcdfile)
print xyz.shape, bgr.shape
cv2.namedWindow('draw curve', cv2.cv.CV_WINDOW_NORMAL)
xys = roi.get_polyline(bgr, 'draw curve')
uvs = np.int32(xys)[:, ::-1]
us, vs = uvs.T
xyzs = xyz[us, vs]
xyzs_good = xyzs[np.isfinite(xyzs).all(axis=1)]
print "%i out of %i labeled points are good" % (len(xyzs_good), len(xyzs))
xyzs_unif = curves.unif_resample(xyzs_good, 100, tol=.002)
#if bool(args.outfile) and not os.path.exists(dirname(args.outfile)): os.mkdir(dirname(args.outfile))
print "writing", args.outfile
np.savetxt(args.outfile, xyzs_unif)