def test_cups():
import rospy, itertools, glob
from jds_utils.colorize import colorize
from jds_image_proc.pcd_io import load_xyzrgb
if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True)
data_dir = "/home/joschu/Data/cups"
xyz1, rgb1 = load_xyzrgb(osp.join(data_dir,"cups1.pcd"))
def preproc(xyz1):
xyz1 = xyz1.reshape(-1,3)
xyz1 = np.dot(xyz1, np.diag([1,-1,-1]))
xyz1 = xyz1[(xyz1[:,2] > .02) & (xyz1[:,2] < .2)
& (np.abs(xyz1[:,0]) < .15) & (np.abs(xyz1[:,1]) < .3)]
xyz1 = voxel_downsample(xyz1, .015)
return xyz1
xyz1 = preproc(xyz1)
#from mayavi import mlab
#mlab.points3d(*xyz1.T,color=(1,1,1),scale_factor=.01)
xyz2, rgb2 = load_xyzrgb(osp.join(data_dir,"cups4.pcd"))
xyz2 = preproc(xyz2)
f = registration.tps_rpm(3*xyz1, 3*xyz2, plotting=4,reg_init=1,reg_final=.05,n_iter=200, verbose=False)
#!/usr/bin/env python
import argparse, cv2, os
parser = argparse.ArgumentParser()
parser.add_argument("pcd_file")
parser.add_argument("label_file")
parser.add_argument("outfile")
parser.add_argument("--plotting", action="store_true")
args = parser.parse_args()
from jds_image_proc import pcd_io
from jds_image_proc.rope_initialization_lite import initialize_rope, RopeInitMessage
xyz, bgr = pcd_io.load_xyzrgb(args.pcd_file)
# get rope mask
label_img = cv2.imread(args.label_file, 0)
if label_img is None:
raise Exception("could not read image %s" % args.label_file)
xyzs_unif, labels = initialize_rope(label_img,
xyz,
bgr,
plotting=args.plotting)
if not os.path.exists(os.path.dirname(args.outfile)):
os.mkdir(dirname(args.outfile))
print "writing", args.outfile
RopeInitMessage((xyzs_unif, labels)).toFiles(args.outfile, "/tmp/junk.json")
def readDataFrom(self, fname):
return pcd_io.load_xyzrgb(fname)
from jds_image_proc import pcd_io
import numpy as np
import cv2
comm_name = "pr2_rope1"
pcdfile = "/home/joschu/comm/%s/kinect/data000000000000.pcd" % comm_name
xyz, rgb = pcd_io.load_xyzrgb(pcdfile)
table_corners = np.loadtxt("/home/joschu/comm/%s/once/table_corners.txt" %
comm_name)
x_ax = table_corners[1] - table_corners[0]
y_ax = table_corners[2] - table_corners[1]
z_ax = np.cross(x_ax, y_ax)
zimg = (z_ax[None, None, :] * xyz).sum(axis=2)
table_height = np.dot(z_ax, table_corners[0])
tablemask = np.abs(zimg - table_height) <= .03
cv2.imshow('hi', (tablemask * 100).astype('uint8'))
cv2.waitKey(10)
from __future__ import division
from pylab import *
import scipy.spatial.distance as ssd
from jds_image_proc.clouds import voxel_downsample
from mayavi import mlab
from jds_image_proc.pcd_io import load_xyzrgb
from jds_image_proc.pcl_utils import xyz2uv, uv2xyz
import scipy.ndimage as ndi
xyz_orig, _ = load_xyzrgb("/home/joschu/Proj/shape_comp/cup.pcd")
us, vs = mgrid[:640, :480]
umin, umax = 220, 330
vmin, vmax = 230, 290
xres = yres = zres = .002
xyz = xyz_orig[umin:umax, vmin:vmax, :]
us = us[umin:umax, vmin:vmax]
vs = vs[umin:umax, vmin:vmax]
good_pts = xyz[np.isfinite(xyz[:, :, 2])]
def laplacian(F):
Fxx = F[2:, 1:-1, 1:-1] + F[:-2, 1:-1, 1:-1] - 2 * F[1:-1, 1:-1, 1:-1]
Fyy = F[1:-1, 2:, 1:-1] + F[1:-1, :-2, 1:-1] - 2 * F[1:-1, 1:-1, 1:-1]
Fzz = F[1:-1, 1:-1, 2:] + F[1:-1, 1:-1, :-2] - 2 * F[1:-1, 1:-1, 1:-1]
return Fxx + Fyy + Fzz
def curv_grad(F):
from jds_image_proc import pcl_utils, pcd_io, utils_images
import cPickle
from mlabraw_jds_image_processing import remove_holes
import cv2
import numpy as np
import scipy.ndimage as ndi
xyz, bgr = pcd_io.load_xyzrgb("/home/joschu/comm/pr2_knot/kinect/data000000000100.pcd")
bgr = bgr.copy()
mask = cv2.imread("/home/joschu/comm/pr2_knot/once/roi_mask.png")[:,:,0].copy()
with open("/home/joschu/comm/pr2_knot/human_labels/classifier.pkl","r") as fh: cls = cPickle.load(fh)
labels = cls.predict(bgr, mask=mask)
x,y,z = xyz[:,:,0], xyz[:,:,1], xyz[:,:,2]
depth = np.sqrt(x**2+y**2+z**2)
depth[np.isnan(depth)] = np.inf
bad = np.zeros(depth.shape,bool)
bad[1:] |= np.abs(depth[1:] - depth[:-1]) > .01
bad[:,1:] |= np.abs(depth[:,1:] - depth[:,:-1]) > .1
bad = ndi.binary_dilation(bad, utils_images.disk(2))
cv2.imshow('bad', bad.astype('uint8')*100)
cv2.imshow('labels',labels*100*~bad)
labels1 = labels*~bad
cv2.imshow('cleaned labels',labels1*100*~bad)
# make graph of reasonably nearby points
# make a spanning tree
from jds_image_proc import pcd_io
import scipy.spatial.distance as ssd, numpy as np
import networkx as nx
MAX_DIST = .03
(xyz,) , rgb = pcd_io.load_xyzrgb("/tmp/comm/rope_pts/data000000000093.pcd")
pdists = ssd.squareform(ssd.pdist(xyz))
G = nx.Graph()
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,:]
import enthought.mayavi.mlab as mlab
mlab.clf()
mlab.plot3d(*xyz_path.T,tube_radius=.02)
mlab.points3d(*xyz.T, scale_factor=.025, color=(1,0,0))
#!/usr/bin/env python
import argparse, cv2, os
parser = argparse.ArgumentParser()
parser.add_argument("pcd_file")
parser.add_argument("label_file")
parser.add_argument("outfile")
parser.add_argument("--plotting",action="store_true")
args = parser.parse_args()
from jds_image_proc import pcd_io
from jds_image_proc.rope_initialization_lite import initialize_rope,RopeInitMessage
xyz,bgr = pcd_io.load_xyzrgb(args.pcd_file)
# get rope mask
label_img = cv2.imread(args.label_file,0)
if label_img is None: raise Exception("could not read image %s"%args.label_file)
xyzs_unif,labels = initialize_rope(label_img,xyz, bgr,plotting=args.plotting)
if not os.path.exists(os.path.dirname(args.outfile)): os.mkdir(dirname(args.outfile))
print "writing",args.outfile
RopeInitMessage((xyzs_unif,labels)).toFiles(args.outfile,"/tmp/junk.json")
from pylab import *
import scipy.spatial.distance as ssd
from jds_image_proc.clouds import voxel_downsample
from mayavi import mlab
from jds_image_proc.pcd_io import load_xyzrgb
xyz, bgr = load_xyzrgb("/home/joschu/Proj/shape_comp/cup.pcd")
xyz = xyz[200:330, 200:330, :]
good_pts = xyz[np.isfinite(xyz[:,:,2])]
ctrl_pts = []
for pt in good_pts:
for s in [1,.95,.9]:
ctrl_pts.append(s * pt)
#ctrl_pts.extend(good_pts)
ctrl_pts.append([0,0,0])
ctrl_pts = np.array(ctrl_pts)
def voxel_downsample(xyz,s, return_inds = False):
xyz = xyz.reshape(-1,3)
xyz = xyz[np.isfinite(xyz[:,0])]
pts = []
keys = set([])
for (i,pt) in enumerate(xyz):
x,y,z = pt
key = (int(x//s),int(y//s),int(z//s))
def readDataFrom(self,fname):
return pcd_io.load_xyzrgb(fname)
from __future__ import division
from pylab import *
import scipy.spatial.distance as ssd
from jds_image_proc.clouds import voxel_downsample
from mayavi import mlab
from jds_image_proc.pcd_io import load_xyzrgb
from jds_image_proc.pcl_utils import xyz2uv, uv2xyz
import scipy.ndimage as ndi
xyz_orig, _ = load_xyzrgb("/home/joschu/Proj/shape_comp/cup.pcd")
us, vs = mgrid[:640, :480]
umin, umax = 220,330
vmin, vmax = 230,290
xres = yres = zres = .002
xyz = xyz_orig[umin:umax, vmin:vmax, :]
us = us[umin:umax, vmin:vmax]
vs = vs[umin:umax, vmin:vmax]
good_pts = xyz[np.isfinite(xyz[:,:,2])]
def laplacian(F):
Fxx = F[2:,1:-1,1:-1] + F[:-2,1:-1,1:-1] - 2*F[1:-1,1:-1,1:-1]
Fyy = F[1:-1,2:,1:-1] + F[1:-1,:-2,1:-1] - 2*F[1:-1,1:-1,1:-1]
Fzz = F[1:-1,1:-1,2:] + F[1:-1,1:-1,:-2] - 2*F[1:-1,1:-1,1:-1]
return Fxx + Fyy + Fzz
def curv_grad(F):
return laplacian(laplacian(F))
def curv_cost(F):
from pylab import *
import scipy.spatial.distance as ssd
from jds_image_proc.clouds import voxel_downsample
from mayavi import mlab
from jds_image_proc.pcd_io import load_xyzrgb
xyz, bgr = load_xyzrgb("/home/joschu/Proj/shape_comp/cup.pcd")
xyz = xyz[200:330, 200:330, :]
good_pts = xyz[np.isfinite(xyz[:, :, 2])]
ctrl_pts = []
for pt in good_pts:
for s in [1, .95, .9]:
ctrl_pts.append(s * pt)
#ctrl_pts.extend(good_pts)
ctrl_pts.append([0, 0, 0])
ctrl_pts = np.array(ctrl_pts)
def voxel_downsample(xyz, s, return_inds=False):
xyz = xyz.reshape(-1, 3)
xyz = xyz[np.isfinite(xyz[:, 0])]
pts = []
keys = set([])
for (i, pt) in enumerate(xyz):
x, y, z = pt
key = (int(x // s), int(y // s), int(z // s))
if key not in keys:
from jds_image_proc import pcl_utils, pcd_io, utils_images
import cPickle
from mlabraw_jds_image_processing import remove_holes
import cv2
import numpy as np
import scipy.ndimage as ndi
xyz, bgr = pcd_io.load_xyzrgb(
"/home/joschu/comm/pr2_knot/kinect/data000000000100.pcd")
bgr = bgr.copy()
mask = cv2.imread("/home/joschu/comm/pr2_knot/once/roi_mask.png")[:, :,
0].copy()
with open("/home/joschu/comm/pr2_knot/human_labels/classifier.pkl", "r") as fh:
cls = cPickle.load(fh)
labels = cls.predict(bgr, mask=mask)
x, y, z = xyz[:, :, 0], xyz[:, :, 1], xyz[:, :, 2]
depth = np.sqrt(x**2 + y**2 + z**2)
depth[np.isnan(depth)] = np.inf
bad = np.zeros(depth.shape, bool)
bad[1:] |= np.abs(depth[1:] - depth[:-1]) > .01
bad[:, 1:] |= np.abs(depth[:, 1:] - depth[:, :-1]) > .1
bad = ndi.binary_dilation(bad, utils_images.disk(2))
cv2.imshow('bad', bad.astype('uint8') * 100)
cv2.imshow('labels', labels * 100 * ~bad)
labels1 = labels * ~bad
cv2.imshow('cleaned labels', labels1 * 100 * ~bad)
from jds_image_proc import pcd_io
import numpy as np
import cv2
comm_name = "pr2_rope1"
pcdfile = "/home/joschu/comm/%s/kinect/data000000000000.pcd"%comm_name
xyz,rgb = pcd_io.load_xyzrgb(pcdfile)
table_corners = np.loadtxt("/home/joschu/comm/%s/once/table_corners.txt"%comm_name)
x_ax = table_corners[1] - table_corners[0]
y_ax = table_corners[2]-table_corners[1]
z_ax = np.cross(x_ax, y_ax)
zimg = (z_ax[None,None,:] * xyz).sum(axis=2)
table_height = np.dot(z_ax, table_corners[0])
tablemask = np.abs(zimg-table_height) <= .03
cv2.imshow('hi',(tablemask*100).astype('uint8'))
cv2.waitKey(10)
# make graph of reasonably nearby points
# make a spanning tree
from jds_image_proc import pcd_io
import scipy.spatial.distance as ssd, numpy as np
import networkx as nx
MAX_DIST = .03
(xyz, ), rgb = pcd_io.load_xyzrgb("/tmp/comm/rope_pts/data000000000093.pcd")
pdists = ssd.squareform(ssd.pdist(xyz))
G = nx.Graph()
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, :]
import enthought.mayavi.mlab as mlab
mlab.clf()
mlab.plot3d(*xyz_path.T, tube_radius=.02)
mlab.points3d(*xyz.T, scale_factor=.025, color=(1, 0, 0))
