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