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