def test_icp(): pts_model = np.load('canonical_model_cloud/binA.npy') pts_scene = np.load('canonical_model_cloud/binA_perturbed.npy') pts_model = pts_model[np.where(pts_model[:,2]!=0)] pts_scene = pts_scene[np.where(pts_scene[:,2]!=0)] pts_model = pts_model[::downsample_rate, :]/1000 pts_scene = pts_scene[::downsample_rate, :]/1000 # ax = render_3d_scatter(pts_scene.tolist()) # ax.set_xlim([-500,500]) # ax.set_ylim([-500,500]) # ax.set_zlim([0,1200]) # ax.view_init(elev=0, azim=90) # plt.show() print 'pts_model shape after downsampling:', pts_model.shape print 'pts_scene shape after downsampling:', pts_scene.shape print 'Making KD Tree' scene_tree = KDTree(pts_scene) print 'Done making KD Tree' R, t = icp.match(pts_model, scene_tree, iterations=20) print R, t pts_model_xformed = pts_model.dot(R.T) + t pts_model_xformed = np.hstack((pts_model_xformed, np.ones((pts_model_xformed.shape[0],1))*rgb_to_pcl_float(255,0,0))) pts_scene = np.hstack((pts_scene, np.ones((pts_scene.shape[0],1))*rgb_to_pcl_float(255,255,0))) ax = render_3d_scatter(np.vstack((pts_model_xformed, pts_scene)).tolist()) ax.set_xlim([-0.5,0.5]) ax.set_ylim([-0.5,0.5]) ax.set_zlim([0,1.2]) ax.view_init(elev=0, azim=90) plt.title('Shelf Overlay') # ax = render_3d_scatter(pts_model.tolist()) # ax.set_xlim([-500,500]) # ax.set_ylim([-500,500]) # ax.set_zlim([0,1200]) # ax.view_init(elev=0, azim=90) # plt.title('Model Original') # ax = render_3d_scatter(pts_model_xformed.tolist()) # ax.set_xlim([-500,500]) # ax.set_ylim([-500,500]) # ax.set_zlim([0,1200]) # ax.view_init(elev=0, azim=90) # plt.title('Model Xformed') # ax = render_3d_scatter(pts_scene.tolist()) # ax.set_xlim([-500,500]) # ax.set_ylim([-500,500]) # ax.set_zlim([0,1200]) # ax.view_init(elev=0, azim=90) # plt.title('Scene') plt.show()
def test_tote_subtraction(): perceiver = Perceiver(False) scene_cloud = np.load('canonical_model_cloud/tote_with_objects.npy') model_cloud = np.load('canonical_model_cloud/tote.npy') clean_cloud = perceiver.get_current_tote_content_cloud(scene_cloud, fit=True) scene_cloud = remove_invalid_points(scene_cloud)[::downsample_rate,:] if scene_cloud.max() > 50: scene_cloud /= 1000 model_cloud = remove_invalid_points(model_cloud)[::downsample_rate,:] if model_cloud.max() > 50: model_cloud /= 1000 render_3d_scatter(model_cloud.tolist()) render_3d_scatter(scene_cloud.tolist()) render_3d_scatter(clean_cloud.tolist()) plt.show()
# render_3d_scatter(tote_cloud) # render_3d_scatter(tote_with_objects_cloud) # plt.show() idxs = subtract_model(tote_cloud, scene_cloud=tote_with_objects_cloud) tote_content_cloud = tote_with_objects_cloud[idxs, :] xs = tote_content_cloud[:,0] ys = tote_content_cloud[:,1] zs = tote_content_cloud[:,2] keep_idxs = np.logical_and(np.logical_and(-0.23<xs, xs<0.2), np.logical_and(-0.2<ys, ys<0.15)) tote_content_cloud = tote_content_cloud[keep_idxs, :] xs = tote_content_cloud[:,0] ys = tote_content_cloud[:,1] zs = tote_content_cloud[:,2] render_3d_scatter(tote_content_cloud) # plt.show() plt.figure() plt.scatter(xs, ys) ''' ASSUMING UP IS Z DIRECTION ''' xs_sorted = sorted(list(xs.flat)) ys_sorted = sorted(list(ys.flat)) N = len(xs_sorted) xmin, xmax = xs_sorted[int(N*0.1)], xs_sorted[int(N*0.9)] # in the unit of meter ymin, ymax = ys_sorted[int(N*0.1)], ys_sorted[int(N*0.9)] x_inrange = np.logical_and(xs >= xmin, xs <= xmax) y_inrange = np.logical_and(ys >= ymin, ys <= ymax)