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 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(): cv2.namedWindow("rgb") Globals.rviz = RvizWrapper.create()
def setup(): print "setup" Globals.rviz = RvizWrapper.create()