Example #1
0
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
Example #3
0
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
Example #4
0
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
Example #5
0
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
Example #6
0
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
Example #7
0
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
Example #8
0
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")
Example #9
0
#!/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)

Example #10
0
#!/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)