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
import image_proc.interactive_roi as roi
import os,cv2
from os.path import dirname, abspath,join
from image_proc import curves
import numpy as np
X = np.zeros((100,100),'uint8')+255
poly0 = roi.get_polyline(X,"curves")
poly1 = roi.get_polyline(X,"curves")
curve0 = curves.unif_resample(poly0,100)
curve1 = curves.unif_resample(poly1,100)
np.savetxt("curve0.txt",curve0)
np.savetxt("curve1.txt",curve1)
#!/usr/bin/env python
from image_proc import interactive_roi as roi
import os,cv2
from os.path import dirname, abspath,join
from 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)
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("outfile")
args = parser.parse_args()
assert os.path.exists(os.path.dirname(args.outfile))
table_corners = np.loadtxt("/home/joschu/bulletsim/data/knots/table_corners.txt");
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")
