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