def test_registration(self):
np.random.seed(101)
source_points = np.random.rand(3, NUM_POINTS).astype(np.float32)
source_normals = np.random.rand(3, NUM_POINTS).astype(np.float32)
source_normals = source_normals / np.tile(
np.linalg.norm(source_normals, axis=0)[np.newaxis, :], [3, 1])
source_point_cloud = PointCloud(source_points, frame='world')
source_normal_cloud = NormalCloud(source_normals, frame='world')
matcher = PointToPlaneFeatureMatcher()
solver = PointToPlaneICPSolver(sample_size=NUM_POINTS)
# 3d registration
tf = RigidTransform(rotation=RigidTransform.random_rotation(),
translation=RigidTransform.random_translation(),
from_frame='world',
to_frame='world')
tf = RigidTransform(from_frame='world',
to_frame='world').interpolate_with(tf, 0.01)
target_point_cloud = tf * source_point_cloud
target_normal_cloud = tf * source_normal_cloud
result = solver.register(source_point_cloud,
target_point_cloud,
source_normal_cloud,
target_normal_cloud,
matcher,
num_iterations=NUM_ITERS)
self.assertTrue(
np.allclose(tf.matrix, result.T_source_target.matrix, atol=1e-3))
# 2d registration
theta = 0.1 * np.random.rand()
t = 0.005 * np.random.rand(3, 1)
t[2] = 0
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
tf = RigidTransform(R, t, from_frame='world', to_frame='world')
target_point_cloud = tf * source_point_cloud
target_normal_cloud = tf * source_normal_cloud
result = solver.register_2d(source_point_cloud,
target_point_cloud,
source_normal_cloud,
target_normal_cloud,
matcher,
num_iterations=NUM_ITERS)
self.assertTrue(
np.allclose(tf.matrix, result.T_source_target.matrix, atol=1e-3))
def transform_pt_grid_to_obj(self, x_grid, direction = False):
""" Converts a point in grid coords to the world basis. If direction then don't translate.
Parameters
----------
x_grid : numpy 3xN ndarray or numeric scalar
points to transform from grid basis to sdf basis in meters
Returns
-------
x_sdf : numpy 3xN ndarray
points in sdf basis (meters)
"""
if isinstance(x_grid, Number):
return self.T_grid_world_.scale * x_grid
if direction:
points_grid = NormalCloud(x_grid.astype(np.float32), frame='grid')
else:
points_grid = PointCloud(x_grid.astype(np.float32), frame='grid')
x_sdf = self.T_grid_world_ * points_grid
return x_sdf.data
def transform_pt_obj_to_grid(self, x_sdf, direction = False):
""" Converts a point in sdf coords to the grid basis. If direction then don't translate.
Parameters
----------
x_sdf : numpy 3xN ndarray or numeric scalar
points to transform from sdf basis in meters to grid basis
Returns
-------
x_grid : numpy 3xN ndarray or scalar
points in grid basis
"""
if isinstance(x_sdf, Number):
return self.T_world_grid_.scale * x_sdf
if direction:
points_sdf = NormalCloud(x_sdf.astype(np.float32), frame='world')
else:
points_sdf = PointCloud(x_sdf.astype(np.float32), frame='world')
x_grid = self.T_world_grid_ * points_sdf
return x_grid.data
# find the transformation from the object coordinates to world coordinates... somehow
grasp_indices, best_metric_indices = sorted_contacts(
vertices, normals, T_ar_object)
for indices in best_metric_indices[0:5]:
a = grasp_indices[indices][0]
b = grasp_indices[indices][1]
normal1, normal2 = normals[a], normals[b]
contact1, contact2 = vertices[a], vertices[b]
# visualize the mesh and contacts
vis.figure()
vis.mesh(mesh)
vis.normals(
NormalCloud(np.hstack(
(normal1.reshape(-1, 1), normal2.reshape(-1, 1))),
frame='test'),
PointCloud(np.hstack(
(contact1.reshape(-1, 1), contact2.reshape(-1, 1))),
frame='test'))
# vis.pose(T_obj_gripper, alpha=0.05)
vis.show()
if BAXTER_CONNECTED:
repeat = True
while repeat:
# come in from the top...
T_obj_gripper = contacts_to_baxter_hand_pose(
contact1, contact2, np.array([0, 0, -1]))
execute_grasp(T_obj_gripper, T_ar_object, ar_tag)
repeat = bool(raw_input("repeat?"))
vis_normals = False
# read data
mesh_filename = args.mesh_filename
_, mesh_ext = os.path.splitext(mesh_filename)
if mesh_ext != '.obj':
raise ValueError('Must provide mesh in Wavefront .OBJ format!')
orig_mesh = ObjFile(mesh_filename).read()
mesh = orig_mesh.subdivide(min_tri_length=0.01)
mesh.compute_vertex_normals()
stable_poses = mesh.stable_poses()
if vis_normals:
vis3d.figure()
vis3d.mesh(mesh)
vis3d.normals(NormalCloud(mesh.normals.T),
PointCloud(mesh.vertices.T),
subsample=10)
vis3d.show()
d = utils.sqrt_ceil(len(stable_poses))
vis.figure(size=(16, 16))
table_mesh = ObjFile('data/meshes/table.obj').read()
table_mesh = table_mesh.subdivide()
table_mesh.compute_vertex_normals()
table_mat_props = MaterialProperties(color=(0, 255, 0),
ambient=0.5,
diffuse=1.0,
specular=1,
shininess=0)