from __future__ import division from pylab import * import scipy.spatial.distance as ssd from image_proc.clouds import voxel_downsample from mayavi import mlab from image_proc.pcd_io import load_xyzrgb from 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 image_proc.clouds import voxel_downsample from mayavi import mlab from 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, 0.95, 0.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))
from 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)
#!/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 image_proc import pcd_io from image_proc.rope_initialization 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")
# make graph of reasonably nearby points # make a spanning tree from 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))
def readDataFrom(self, fname): return pcd_io.load_xyzrgb(fname)
plt.figure(1) plt.imshow(distmat1) plt.title("distances") plt.figure(2) plt.imshow(distmat2) plt.title("corr_sums") np.savez("cross_registration_results", distmat = distmat1, names = files) if __name__ == "__main__": import rospy, itertools, glob from utils.colorize import colorize from 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)
from image_proc import pcl_utils, pcd_io, utils_images import cPickle from mlabraw_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 image_proc.pcd_io import load_xyzrgb import bulletsim.rope_initialization as ri import numpy as np xyz, rgb = load_xyzrgb("data000000000004.pcd") xyz = xyz.reshape(-1, 3) S = ri.skeletonize_point_cloud(xyz) print "done!" from mayavi import mlab xs, ys, zs, us, vs, ws = [], [], [], [], [], [] for (node0, node1) in S.edges(): (x0, y0, z0) = S.node[node0]["xyz"] (x1, y1, z1) = S.node[node1]["xyz"] xs.append(x0) ys.append(y0) zs.append(z0) us.append(x1 - x0) vs.append(y1 - y0) ws.append(z1 - z0) mlab.plot3d([x0, x1], [y0, y1], [z0, z1], color=(1, 0, 0), tube_radius=0.0025) # mlab.quiver3d(xs,ys,zs,us,vs,ws,mode= '2ddash',line_width=.001) # mlab.points3d(xs,ys,zs) mlab.points3d(*xyz.T, **dict(color=(0, 0, 1), scale_factor=0.01, opacity=0.25))