def test_zrot2(): #def test_plate(): import rospy, itertools, glob from jds_utils.colorize import colorize from jds_utils.math_utils import normr, norms if rospy.get_name() == "/unnamed": rospy.init_node('test_registration_3d',disable_signals=True) xyz1 = np.random.randn(300,3)*.2 xyz2 = np.random.randn(300,3)*.2 xyz1 = xyz1[norms(xyz1,1) < 1]*.2 xyz2 = xyz2[norms(xyz1,1) < 1]*.2 from numpy import sin, cos, pi f = registration.tps_rpm_zrot(xyz1, xyz2, plotting=1,reg_init=2,reg_final=.05,n_iter=8, verbose=False, cost_per_radian = 2)
def point_shape_context(pt, cloud, hist_dim): cloud = np.atleast_2d(cloud) - np.atleast_2d(pt) r = math_utils.norms(cloud,1) r_med = np.median(r) if hist_dim == 2: num_r_bins = 4 num_theta_bins = 9 r_bin_edges = r_med/2**(num_r_bins-1) * 2**np.arange(0, num_r_bins+1) theta = np.arctan2(cloud[:,1], cloud[:,0]) theta_bin_edges = np.linspace(-np.pi, np.pi, num_theta_bins+1, endpoint = True) hist,_,_ = np.histogram2d(r,theta, bins = (r_bin_edges, theta_bin_edges)) areas = np.pi * (r_bin_edges[1:]**2 - r_bin_edges[:-1]**2)/num_theta_bins features = ((areas**(-1/2))[:,None] * hist).flatten() features /= features.sum() return features else: raise NotImplementedError
def point_shape_context(pt, cloud, hist_dim): cloud = np.atleast_2d(cloud) - np.atleast_2d(pt) r = math_utils.norms(cloud, 1) r_med = np.median(r) if hist_dim == 2: num_r_bins = 4 num_theta_bins = 9 r_bin_edges = r_med / 2**(num_r_bins - 1) * 2**np.arange( 0, num_r_bins + 1) theta = np.arctan2(cloud[:, 1], cloud[:, 0]) theta_bin_edges = np.linspace(-np.pi, np.pi, num_theta_bins + 1, endpoint=True) hist, _, _ = np.histogram2d(r, theta, bins=(r_bin_edges, theta_bin_edges)) areas = np.pi * (r_bin_edges[1:]**2 - r_bin_edges[:-1]**2) / num_theta_bins features = ((areas**(-1 / 2))[:, None] * hist).flatten() features /= features.sum() return features else: raise NotImplementedError
def unif_resample(x, n, tol=1, deg=3): x = np.atleast_2d(x) x = mu.remove_duplicate_rows(x) dl = mu.norms(x[1:] - x[:-1], 1) l = np.cumsum(np.r_[0, dl]) (tck, _) = si.splprep(x.T, k=deg, s=tol ** 2 * len(x), u=l) newu = np.linspace(0, l[-1], n) return np.array(si.splev(newu, tck)).T
def unif_resample(x, n, tol=0, deg=None): if deg is None: deg = min(3, len(x) - 1) x = np.atleast_2d(x) x = math_utils.remove_duplicate_rows(x) dl = math_utils.norms(x[1:] - x[:-1], 1) l = np.cumsum(np.r_[0, dl]) (tck, _) = si.splprep(x.T, k=deg, s=tol**2 * len(x), u=l) newu = np.linspace(0, l[-1], n) return np.array(si.splev(newu, tck)).T
def unif_resample(x,n,tol=0,deg=None): if deg is None: deg = min(3, len(x) - 1) x = np.atleast_2d(x) x = math_utils.remove_duplicate_rows(x) dl = math_utils.norms(x[1:] - x[:-1],1) l = np.cumsum(np.r_[0,dl]) (tck,_) = si.splprep(x.T,k=deg,s = tol**2*len(x),u=l) newu = np.linspace(0,l[-1],n) return np.array(si.splev(newu,tck)).T
def unif_resample(x, n, weights, tol=.001, deg=3): x = np.atleast_2d(x) weights = np.atleast_2d(weights) x = mu.remove_duplicate_rows(x) x_scaled = x * weights dl = mu.norms(x_scaled[1:] - x_scaled[:-1], 1) l = np.cumsum(np.r_[0, dl]) (tck, _) = si.splprep(x_scaled.T, k=deg, s=tol**2 * len(x), u=l) newu = np.linspace(0, l[-1], n) out_scaled = np.array(si.splev(newu, tck)).T out = out_scaled / weights return out
def unif_resample(x,n,weights,tol=.001,deg=3): x = np.atleast_2d(x) weights = np.atleast_2d(weights) x = mu.remove_duplicate_rows(x) x_scaled = x * weights dl = mu.norms(x_scaled[1:] - x_scaled[:-1],1) l = np.cumsum(np.r_[0,dl]) (tck,_) = si.splprep(x_scaled.T,k=deg,s = tol**2*len(x),u=l) newu = np.linspace(0,l[-1],n) out_scaled = np.array(si.splev(newu,tck)).T out = out_scaled/weights return out
def path_length(x): """ sum of absolute values of all differences between time steps """ if x.ndim == 1: x = x.reshape(-1, 1) return norms(x[1:] - x[:-1], 1).sum()
def nearest_neighbor(x, ys): return mu.norms(ys - x[None,:],1).argmin()
def segment_tabletop_scene(xyz, frame_id='base_footprint', plotting2d=False, plotting3d=False, plotting2d_all=False): """ Initial segmentation: 1. Median filter point cloud to remove NaNs 2. Select the points that lie above the table 3. Select the points that don't lie above or left of a discontinuity in x, y, or z 4. Find connected components Second pass: 5. Fit cylinders to each cluster 6. Merge overlapping cylinders In the future, fit ellipses or boxes in addition to cylinders, since some objects are oblong. """ assert xyz.ndim == 3 and xyz.shape[0] > 1 and xyz.shape[1] > 1 np.putmask(xyz, np.isnan(xyz), -np.inf) xyz_mf = ndi.median_filter(xyz, size=(3, 3, 1)) rgrad = xyz_mf[1:, :-1, :] - xyz_mf[:-1, :-1, :] cgrad = xyz_mf[:-1, 1:, :] - xyz_mf[:-1, :-1, :] rdiff = (rgrad**2).sum(axis=2) cdiff = (cgrad**2).sum(axis=2) small_diff_mask_interior = (rdiff < OBJECT_CLUSTER_TOL** 2) & (cdiff < OBJECT_CLUSTER_TOL**2) small_diff_mask = np.zeros(xyz.shape[:2], bool) small_diff_mask[:-1, :-1] = small_diff_mask_interior table_height = get_table_height(xyz) z = xyz[:, :, 2] above_table_mask = np.isfinite(z) & (z > table_height + ABOVE_TABLE_CUTOFF) both_mask = small_diff_mask & above_table_mask labels, max_label = ndi.label(both_mask) label_counts = np.bincount(labels.flatten()) n_clu = (label_counts[1:] >= MIN_CLUSTER_SIZE).sum() old2new = np.arange(max_label + 1) old2new[label_counts < MIN_CLUSTER_SIZE] = 0 old2new[1:][label_counts[1:] >= MIN_CLUSTER_SIZE] = np.arange(n_clu) + 1 labels = old2new[labels] clusters = [xyz[labels == i] for i in xrange(1, n_clu + 1)] merge_to = np.arange(n_clu) centers_radii = [ cv2.minEnclosingCircle(clu[:, :2].reshape(-1, 1, 2).astype('float32') * 100000) for clu in clusters ] for i in xrange(n_clu): for j in xrange(i + 1, n_clu): (x0, y0), r0 = centers_radii[i] (x1, y1), r1 = centers_radii[j] dist = math.hypot(x1 - x0, y1 - y0) rad_sum = r0 + r1 if dist < rad_sum: # i,j,dist,rad_sum merge_to[merge_to == merge_to[j]] = merge_to[i] final_clusters = [ np.concatenate([clusters[i] for i in np.flatnonzero(merge_to == m)], 0) for m in np.flatnonzero(np.bincount(merge_to)) ] if plotting2d: import matplotlib.pyplot as plt plt.close('all') plt.figure(FIG_LABELS) labels_merged = merge_to[labels - 1] + 1 labels_merged[labels == 0] = 0 plt.imshow(labels_merged) plt.title("after merging") for (i, cluster) in enumerate(final_clusters): row, col = np.unravel_index( norms(xyz - cluster.mean(axis=0)[None, None, :], 2).argmin(), xyz.shape[0:2]) # print "center pixel:",i,row,col plt.text(col, row, str(i)) if plotting2d_all: plt.figure(FIG_ABOVE) plt.imshow(both_mask) plt.title("above table & continuity") plt.figure(FIG_FP_LABELS) plt.imshow(labels) plt.title("first-pass labels") plt.figure(FIG_DEPTH) plt.imshow(z) if plotting2d: plt.show() if plotting3d: global marker_handles marker_handles = [] for clu in final_clusters: x, y, z, r, h = get_cylinder(clu) rviz = RvizWrapper.create() ps = gm.PoseStamped() ps.pose.position = gm.Point(x, y, z) ps.pose.orientation = gm.Quaternion(0, 0, 0, 1) ps.header.frame_id = frame_id marker_handles.append( rviz.draw_marker(ps, type=Marker.CYLINDER, scale=(2 * r, 2 * r, h), rgba=(1, 1, 0, .2), ns="segment_tabletop_scene")) return final_clusters
def path_length(x): """ sum of absolute values of all differences between time steps """ if x.ndim == 1: x = x.reshape(-1,1) return norms(x[1:] - x[:-1],1).sum()
def segment_tabletop_scene(xyz, frame_id='base_footprint', plotting2d = False, plotting3d = False, plotting2d_all = False): """ Initial segmentation: 1. Median filter point cloud to remove NaNs 2. Select the points that lie above the table 3. Select the points that don't lie above or left of a discontinuity in x, y, or z 4. Find connected components Second pass: 5. Fit cylinders to each cluster 6. Merge overlapping cylinders In the future, fit ellipses or boxes in addition to cylinders, since some objects are oblong. """ assert xyz.ndim == 3 and xyz.shape[0] > 1 and xyz.shape[1] > 1 np.putmask(xyz, np.isnan(xyz), -np.inf) xyz_mf = ndi.median_filter(xyz, size=(3,3,1)) rgrad = xyz_mf[1:,:-1,:] - xyz_mf[:-1,:-1,:] cgrad = xyz_mf[:-1,1:,:] - xyz_mf[:-1,:-1,:] rdiff = (rgrad**2).sum(axis=2) cdiff = (cgrad**2).sum(axis=2) small_diff_mask_interior = (rdiff < OBJECT_CLUSTER_TOL**2) & (cdiff < OBJECT_CLUSTER_TOL**2) small_diff_mask = np.zeros(xyz.shape[:2],bool) small_diff_mask[:-1, :-1] = small_diff_mask_interior table_height = get_table_height(xyz) z = xyz[:,:,2] above_table_mask = np.isfinite(z) & (z > table_height + ABOVE_TABLE_CUTOFF) both_mask = small_diff_mask & above_table_mask labels, max_label = ndi.label(both_mask) label_counts = np.bincount(labels.flatten()) n_clu = (label_counts[1:] >= MIN_CLUSTER_SIZE).sum() old2new = np.arange(max_label+1) old2new[label_counts < MIN_CLUSTER_SIZE] = 0 old2new[1:][label_counts[1:] >= MIN_CLUSTER_SIZE] = np.arange(n_clu)+1 labels = old2new[labels] clusters = [xyz[labels == i] for i in xrange(1, n_clu+1)] merge_to = np.arange(n_clu) centers_radii = [cv2.minEnclosingCircle(clu[:,:2].reshape(-1,1,2).astype('float32')*100000) for clu in clusters] for i in xrange(n_clu): for j in xrange(i+1, n_clu): (x0, y0), r0 = centers_radii[i] (x1, y1), r1 = centers_radii[j] dist = math.hypot(x1-x0, y1-y0) rad_sum = r0 + r1 if dist < rad_sum: # i,j,dist,rad_sum merge_to[merge_to==merge_to[j]] = merge_to[i] final_clusters = [np.concatenate([clusters[i] for i in np.flatnonzero(merge_to==m)],0) for m in np.flatnonzero(np.bincount(merge_to))] if plotting2d: import matplotlib.pyplot as plt plt.close('all') plt.figure(FIG_LABELS) labels_merged = merge_to[labels-1]+1 labels_merged[labels==0] = 0 plt.imshow(labels_merged) plt.title("after merging") for (i,cluster) in enumerate(final_clusters): row,col = np.unravel_index(norms(xyz - cluster.mean(axis=0)[None,None,:],2).argmin(),xyz.shape[0:2]) # print "center pixel:",i,row,col plt.text(col,row,str(i)) if plotting2d_all: plt.figure(FIG_ABOVE) plt.imshow(both_mask) plt.title("above table & continuity") plt.figure(FIG_FP_LABELS) plt.imshow(labels) plt.title("first-pass labels") plt.figure(FIG_DEPTH) plt.imshow(z) if plotting2d: plt.show() if plotting3d: global marker_handles marker_handles = [] for clu in final_clusters: x,y,z,r,h = get_cylinder(clu) rviz = RvizWrapper.create() ps = gm.PoseStamped() ps.pose.position = gm.Point(x,y,z) ps.pose.orientation = gm.Quaternion(0,0,0,1) ps.header.frame_id = frame_id marker_handles.append(rviz.draw_marker(ps, type=Marker.CYLINDER, scale=(2*r, 2*r, h), rgba=(1,1,0,.2), ns = "segment_tabletop_scene")) return final_clusters