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)
