def test_fit_nr():
from jds_image_proc.clouds import voxel_downsample
from brett2.ros_utils import RvizWrapper
import lfd.registration as lr
import lfd.warping as lw
if rospy.get_name() == "/unnamed":
rospy.init_node("test_rigidity", disable_signals=True)
from time import sleep
sleep(1)
rviz = RvizWrapper.create()
pts0 = np.loadtxt("test/rope_control_points.txt")
pts1 = np.loadtxt("test/bad_rope.txt")
pts_rigid = voxel_downsample(pts0[:10], .02)
lr.Globals.setup()
np.seterr(all='ignore')
np.set_printoptions(suppress=True)
lin_ag, trans_g, w_eg, x_ea = tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton",plotting=1)
#lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01)
#assert np.allclose(w_ng, w_ng2)
def eval_partial(x_ma):
return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea)
lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008)
#handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint')
grads = tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea)
print "worst violation:",np.max(np.abs([grad.T.dot(grad)-np.eye(3) for grad in grads]))
from brett2 import ros_utils
import geometry_msgs.msg as gm
import rospy
import numpy as np
rospy.init_node("test_draw_gripper")
from jds_utils.conversions import array_to_pose_array
from brett2.ros_utils import RvizWrapper
rviz = RvizWrapper.create()
rospy.sleep(.5)
array = np.array([[0, 0, 0], [.2, 0, 0], [.4, 0, 0]])
pose_array = array_to_pose_array(array, 'base_footprint')
handles = rviz.draw_trajectory(pose_array, [0, .04, .08], 'r')
def draw(self):
origin = [self.cx, self.cy, self.cz]
quat = conversions.yaw_to_quat(self.theta)
pose = conversions.trans_rot_to_pose(origin, quat)
ps = gm.PoseStamped()
ps.pose = pose
ps.header.frame_id = "/base_link"
ps.header.stamp = rospy.Time(0)
print "(%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f), (%.3f, %.3f, %.3f)"%(
self.cx, self.cy, self.cz, quat[0], quat[1], quat[2], quat[3], self.sx/2., self.sy/2., self.sz/2.)
#rviz.draw_marker(ps, 0, type=Marker.CUBE, rgba=(0,1,0,.25), scale=(self.sx,self.sy,self.sz))
self.marker_handle = rviz.place_kin_tree_from_link(ps, "base_link")
view = View(
VGroup(
HGroup(Item("cx"), Item("cy"), Item("cz")),
HGroup(Item("sx"), Item("sy"), Item("sz")),
Item("theta")
),
buttons = OKCancelButtons,
width = 500,
resizable = True)
if __name__ == "__main__":
rospy.init_node("draw_box",disable_signals = True)
rviz = RvizWrapper()
box = Box(rviz)
box.configure_traits()
def setup():
Globals.pr2 = PR2.create()
Globals.rviz = RvizWrapper.create()
rospy.sleep(1)
import h5py
from brett2.PR2 import PR2,IKFail
from brett2 import trajectories
from kinematics import kinematics_utils
import rospy
from brett2.ros_utils import RvizWrapper,Marker
from numpy import asarray
import numpy as np
import geometry_msgs.msg as gm
from utils import conversions
if rospy.get_name()=="/unnamed":
rospy.init_node("playback_demo")
rviz = RvizWrapper.create()
pr2 = PR2.create()
rospy.sleep(1)
traj = h5py.File(args.file, 'r')[args.group]
ps = gm.PoseStamped(pose = gm.Pose(
position = gm.Point(*traj["object_pose"]),
orientation = gm.Quaternion(*traj["object_orientation"])))
ps.header.frame_id = 'base_link'
rviz.draw_marker(
ps,
id=1,
type=Marker.CUBE,
rgba = (0,1,0,1),
def setup():
Globals.rviz = RvizWrapper.create()
def setup():
if Globals.rviz is None:
from brett2.ros_utils import RvizWrapper
Globals.rviz = RvizWrapper.create()
import time
time.sleep(.2)
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 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 setup():
if Globals.rviz is None:
from brett2.ros_utils import RvizWrapper
Globals.rviz = RvizWrapper.create()
import time
time.sleep(.2)
def setup():
cv2.namedWindow("rgb")
Globals.rviz = RvizWrapper.create()
def setup():
Globals.rviz = RvizWrapper.create()
def setup():
print "setup"
Globals.rviz = RvizWrapper.create()
def setup():
cv2.namedWindow("rgb")
Globals.rviz = RvizWrapper.create()