def propose_grasps(pc, radius, num_grasps=1, vis=False):
output_grasps = []
for _ in range(num_grasps):
center_index = np.random.randint(pc.shape[0])
center_point = pc[center_index, :].copy()
d = np.sqrt(np.sum(np.square(pc - np.expand_dims(center_point, 0)),
-1))
index = np.where(d < radius)[0]
neighbors = pc[index, :]
eigen_values, eigen_vectors = cov_matrix(center_point, neighbors)
direction = eigen_vectors[:, 2]
direction = choose_direction(direction, center_point)
surface_orientation = trimesh.geometry.align_vectors([0, 0, 1],
direction)
roll_orientation = tra.quaternion_matrix(
tra.quaternion_about_axis(np.random.uniform(0, 2 * np.pi),
[0, 0, 1]))
gripper_transform = surface_orientation.dot(roll_orientation)
gripper_transform[:3, 3] = center_point
translation_transform = np.eye(4)
translation_transform[2, 3] = -np.random.uniform(0.0669, 0.1122)
gripper_transform = gripper_transform.dot(translation_transform)
output_grasps.append(gripper_transform.copy())
if vis:
draw_scene(pc, grasps=output_grasps)
mlab.show()
return np.asarray(output_grasps)
def rotate(self, v, origin="local"):
obj = self.copy()
rot = quaternion_from_euler(v[0], v[1], v[2], "sxyz")
rotation_matrix = transformations.quaternion_matrix(rot)
#rot = transformations.euler_matrix(v[0], v[1], v[2], 'sxyz')
center_coords = None
if origin == 'local':
center_coords = None
elif origin == 'bounds_center': # group_centroid, use for children
((xmin, ymin, zmin), (xmax, ymax, zmax)) = self.bounds()
center_coords = [(xmin + xmax) / 2, (ymin + ymax) / 2, (zmin + zmax) / 2]
elif origin:
center_coords = origin
if center_coords:
#translate_before = transformations.translation_matrix(np.array(center_coords) * -1)
#translate_after = transformations.translation_matrix(np.array(center_coords))
#transf = translate_before * rot # * rot * translate_after # doesn't work, these matrifes are 4x3, not 4x4 HTM
obj.center = obj.center - np.array(center_coords)
obj.center = np.dot(rotation_matrix, list(obj.center) + [1])[:3] # Hack: use matrices
obj.center = obj.center + np.array(center_coords)
obj.size = np.abs(np.dot(rotation_matrix, list(obj.size) + [1])[:3]) # Hack: use matrices
else:
#transf = rot
obj.center = np.dot(rotation_matrix, list(obj.center) + [1])[:3] # Hack: use matrices
obj.size = np.abs(np.dot(rotation_matrix, list(obj.size) + [1])[:3]) # Hack: use matrices
return obj
def main_check_pointcloud(iterate_all_viewpoints=True):
from visualization_utils import draw_scene
import mayavi.mlab as mlab
import matplotlib.pyplot as plt
import glob
import random
import cv2
import time
import json
quaternions = [
l[:-1].split('\t')
for l in open('uniform_quaternions/data2_4608.qua', 'r').readlines()
]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
all_eulers = [tra.quaternion_matrix(q) for q in quaternions]
all_eulers = []
for az in np.linspace(0, math.pi * 2, 30):
for el in np.linspace(-math.pi / 2, math.pi / 2, 30):
all_eulers.append(tra.euler_matrix(el, az, 0))
renderer = OnlineObjectRendererMultiProcess(caching=True)
renderer.start()
grasps_path = glob.glob('unified_grasp_data/grasps/*.json')
random.shuffle(grasps_path)
#mesh_paths = ['unified_grasp_data/meshes/Bowl/9a52843cc89cd208362be90aaa182ec6.stl']
for main_iter in range(5 * len(grasps_path)):
gpath = grasps_path[main_iter % len(grasps_path)]
json_dict = json.load(open(gpath))
mpath = os.path.join('unified_grasp_data', json_dict['object'])
scale = json_dict['object_scale']
#mpath = 'unified_grasp_data/meshes/Bottle/10d3d5961e00b2133ff038bc77759685.stl'
#mpath = 'unified_grasp_data/meshes/Bottle/ef631a2ce94fae3ab8966911a5afa22c.stl'
print(main_iter, mpath)
start_time = time.time()
renderer.change_object(mpath, scale)
if iterate_all_viewpoints == True:
viewpoints = all_eulers
else:
viewpoints = [all_eulers[np.random.randint(len(all_eulers))]]
for view in viewpoints:
image, depth, pc, _ = renderer.render(view)
print(time.time() - start_time)
print('depth min = {} max = {} npoints = {}'.format(
np.min(depth), np.max(depth), pc.shape))
draw_scene(pc, None)
mlab.show()
renderer.terminate()
renderer.join()
def __init__(
self,
root_folder,
batch_size=1,
raw_num_points = 20000,
estimate_normals = False,
caching=True,
use_uniform_quaternions=False,
scene_obj_scales=None,
scene_obj_paths=None,
scene_obj_transforms=None,
num_train_samples=None,
num_test_samples=None,
use_farthest_point = False,
intrinsics = None,
distance_range = (0.9,1.3),
elevation = (30,150),
pc_augm_config = None,
depth_augm_config = None
):
self._root_folder = root_folder
self._batch_size = batch_size
self._raw_num_points = raw_num_points
self._caching = caching
self._num_train_samples = num_train_samples
self._num_test_samples = num_test_samples
self._estimate_normals = estimate_normals
self._use_farthest_point = use_farthest_point
self._scene_obj_scales = scene_obj_scales
self._scene_obj_paths = scene_obj_paths
self._scene_obj_transforms = scene_obj_transforms
self._distance_range = distance_range
self._pc_augm_config = pc_augm_config
self._depth_augm_config = depth_augm_config
self._current_pc = None
self._cache = {}
self._renderer = SceneRenderer(caching=True, intrinsics=intrinsics)
if use_uniform_quaternions:
quat_path = os.path.join(self._root_folder, 'uniform_quaternions/data2_4608.qua')
quaternions = [l[:-1].split('\t') for l in open(quat_path, 'r').readlines()]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
self._all_poses = [tra.quaternion_matrix(q) for q in quaternions]
else:
self._cam_orientations = []
self._elevation = np.array(elevation)/180.
for az in np.linspace(0, np.pi * 2, 30):
for el in np.linspace(self._elevation[0], self._elevation[1], 30):
self._cam_orientations.append(tra.euler_matrix(0, -el, az))
self._coordinate_transform = tra.euler_matrix(np.pi/2, 0, 0).dot(tra.euler_matrix(0, np.pi/2, 0))
def rotate(angle, mesh: trimesh):
# matrix_x = tf.rotation_matrix(math.radians(angle[0]), [1,0,0], mesh.centroid)
# matrix_y = tf.rotation_matrix(math.radians(angle[1]), [0,1,0], mesh.centroid)
# matrix_z = tf.rotation_matrix(math.radians(angle[2]), [0,0,1], mesh.centroid)
mesh.apply_transform(
tf.quaternion_matrix(
tf.quaternion_from_euler(math.radians(angle[0]),
math.radians(angle[1]),
math.radians(angle[2]))))
def _transform(self):
link = self._optimizer.target
quaternion = cuda.to_cpu(link.quaternion.array)
translation = cuda.to_cpu(link.translation.array)
transform = tf.quaternion_matrix(quaternion)
transform = morefusion.geometry.compose_transform(
transform[:3, :3], translation
)
return transform
def rotate(self, v):
obj = self.copy()
rot = quaternion_from_euler(v[0], v[1], v[2], "sxyz")
rotation_matrix = transformations.quaternion_matrix(rot)
obj.center = np.dot(rotation_matrix,
list(obj.center) + [1])[:3] # Hack: use matrices
obj.size = np.abs(np.dot(rotation_matrix,
list(obj.size) +
[1])[:3]) # Hack: use matrices
return obj
def main():
models = morefusion.datasets.YCBVideoModels()
points = models.get_pcd(class_id=2)
quaternion_true = tf.random_quaternion()
quaternion_pred = quaternion_true + [0.1, 0, 0, 0]
transform_true = tf.quaternion_matrix(quaternion_true)
transform_pred = tf.quaternion_matrix(quaternion_pred)
scenes = {}
for use_translation in [False, True]:
if use_translation:
translation_true = np.random.uniform(-0.02, 0.02, (3,))
translation_pred = np.random.uniform(-0.02, 0.02, (3,))
transform_true[:3, 3] = translation_true
transform_pred[:3, 3] = translation_pred
add = morefusion.metrics.average_distance(
[points], [transform_true], [transform_pred]
)[0][0]
# ---------------------------------------------------------------------
scene = trimesh.Scene()
points_true = tf.transform_points(points, transform_true)
colors = np.full((points_true.shape[0], 3), [1.0, 0, 0])
geom = trimesh.PointCloud(vertices=points_true, color=colors)
scene.add_geometry(geom)
points_pred = tf.transform_points(points, transform_pred)
colors = np.full((points_true.shape[0], 3), [0, 0, 1.0])
geom = trimesh.PointCloud(vertices=points_pred, color=colors)
scene.add_geometry(geom)
scenes[f"use_translation: {use_translation}, add: {add}"] = scene
if scenes:
camera_transform = list(scenes.values())[0].camera_transform
scene.camera_transform = camera_transform
morefusion.extra.trimesh.display_scenes(scenes)
def uniform_quaternions():
quaternions = [
l[:-1].split('\t') for l in open(
'../uniform_quaternions/data2_4608.qua', 'r').readlines()
]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
return [tra.quaternion_matrix(q) for q in quaternions]
def setUp(self):
batch_size = 5
self.quaternions = np.array(
[tf.random_quaternion() for _ in range(batch_size)],
dtype=np.float32,
)
self.transforms = np.array(
[tf.quaternion_matrix(q) for q in self.quaternions],
dtype=np.float32,
)
self.gTs = np.random.uniform(-1, 1, (batch_size, 4, 4)).astype(
np.float32
)
self.check_backward_options = {"atol": 5e-4, "rtol": 5e-3}
def main():
models = morefusion.datasets.YCBVideoModels()
points = models.get_pcd(class_id=2)
n_test = 1000
transforms_true = []
transforms_pred = []
for i in range(n_test):
quaternion_true = tf.random_quaternion()
quaternion_pred = quaternion_true + [0.1, 0, 0, 0]
translation_true = np.random.uniform(-0.02, 0.02, (3, ))
translation_pred = np.random.uniform(-0.02, 0.02, (3, ))
transform_true = tf.quaternion_matrix(quaternion_true)
transform_true[:3, 3] = translation_true
transform_pred = tf.quaternion_matrix(quaternion_pred)
transform_pred[:3, 3] = translation_pred
transforms_true.append(transform_true)
transforms_pred.append(transform_pred)
adds = morefusion.metrics.average_distance([points] * n_test,
transforms_true,
transforms_pred)[0]
max_distance = 0.1
auc, x, y = morefusion.metrics.auc_for_errors(adds,
max_threshold=max_distance,
return_xy=True)
print("auc:", auc)
plt.plot(x, y)
plt.xlabel("add threshold")
plt.xlim(0, max_distance)
plt.ylabel("accuracy")
plt.ylim(0, 1)
plt.show()
def main():
models = morefusion.datasets.YCBVideoModels()
points = models.get_pcd(class_id=2)
quaternion_true = tf.random_quaternion()
quaternion_pred = quaternion_true + [0.1, 0, 0, 0]
transform_true = tf.quaternion_matrix(quaternion_true)
transform_pred = tf.quaternion_matrix(quaternion_pred)
# translation
translation_true = np.random.uniform(-0.02, 0.02, (3, ))
translation_pred = np.random.uniform(-0.02, 0.02, (3, ))
transform_true[:3, 3] = translation_true
transform_pred[:3, 3] = translation_pred
for symmetric in [False, True]:
add = morefusion.functions.loss.average_distance(
points,
transform_true,
transform_pred[None],
symmetric=symmetric,
)
print(f"symmetric={symmetric}, add={float(add.array[0])}")
def spherical_rotate(max_rot_degrees):
"""Calculate a random rotation matrix using angle magnitudes in max_rot."""
assert isinstance(max_rot_degrees, tuple) and len(max_rot_degrees) == 3
xrot, yrot, zrot = max_rot_degrees
# Build the global rotation matrix using quaternions
q_xr = tf.quaternion_about_axis(rand_step(xrot), [1, 0, 0])
q_yr = tf.quaternion_about_axis(rand_step(yrot), [0, 1, 0])
q_zr = tf.quaternion_about_axis(rand_step(zrot), [0, 0, 1])
# Multiply global and local rotations
rotation = tf.quaternion_multiply(q_xr, q_yr)
rotation = tf.quaternion_multiply(rotation, q_zr)
return tf.quaternion_matrix(rotation)
def get_empty_scene(self):
"""Creates an empty scene with a camera and spot light."""
scene = pyrender.Scene(ambient_light=[0.2, 0.2, 0.2],
bg_color=[0.1, 0.1, 0.1])
camera = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1)
camera_pose = translation_matrix([0, 3, 4]) @ quaternion_matrix(
quaternion_about_axis(np.deg2rad(-45.0), [1, 0, 0]))
scene.add(camera, pose=camera_pose)
light = pyrender.SpotLight(color=np.ones(3),
intensity=3.0,
innerConeAngle=np.pi / 16.0)
scene.add(light, pose=camera_pose)
return scene
def _filter_robot_poses(self, robot_poses):
filtered_robot_poses = list()
for robot_pose in robot_poses:
ori = robot_pose.orientation
R = ttf.quaternion_matrix(np.array([ori.w, ori.x, ori.y,
ori.z]))[0:3, 0:3]
z_vector = np.array([0.0, 0.0, 1.0]).transpose().reshape((3, 1))
down_z_vector = (np.array([0.0, 0.0, -1.0]).transpose().reshape(
(3, 1)))
robot_z_vector = np.matmul(R, z_vector)
angle_between = ttf.angle_between_vectors(robot_z_vector,
down_z_vector)
if angle_between < self._angle_from_vertical_limit:
filtered_robot_poses.append(robot_pose)
return filtered_robot_poses
def main(argv):
import argparse
parser = argparse.ArgumentParser(
description='Pre-render objects',
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('--cad_path', type=str, default=1)
parser.add_argument('--cad_scale', type=float, default=-1)
parser.add_argument('--quaternion_file',
type=str,
default='uniform_quaternions/data2_4608.qua')
parser.add_argument('--output_path', type=str)
args = parser.parse_args(sys.argv[1:])
if args.cad_scale <= 0:
raise ValueError("you have to set --cad_scale to positive number")
quaternions = [
l[:-1].split('\t')
for l in open(args.quaternion_file, 'r').readlines()
]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
all_eulers = [
tra.euler_from_matrix(tra.quaternion_matrix(q)) for q in quaternions
]
renderer = ObjectRenderer(object_paths=[args.cad_path],
object_scales=[args.cad_scale])
try:
renderer.render_all_and_save_to_h5(args.output_path,
all_eulers,
vis=True)
except Exception as e:
print(e)
os.system('rm {}'.format(args.output_path))
def _get_grid_full(self, examples, pitch, origin):
dims = (self._voxel_dim, ) * 3
grid_full = np.zeros(dims, dtype=np.int32)
for i, example in enumerate(examples):
T = tf.quaternion_matrix(example["quaternion_true"])
T = geometry_module.compose_transform(
R=T[:3, :3], t=example["translation_true"])
vox = self._models.get_solid_voxel_grid(example["class_id"])
points = trimesh.transform_points(vox.points, T)
indices = trimesh.voxel.ops.points_to_indices(points,
pitch=pitch,
origin=origin)
I, J, K = indices[:, 0], indices[:, 1], indices[:, 2]
keep = ((0 <= I)
& (I < dims[0])
& (0 <= J)
& (J < dims[1])
& (0 <= K)
& (K < dims[2]))
I, J, K = I[keep], J[keep], K[keep]
grid_full[I, J, K] = i + 1 # starts from 1
return grid_full
def get_adds(result_file):
dataset = morefusion.datasets.YCBVideoDataset
models = morefusion.datasets.YCBVideoModels()
result = scipy.io.loadmat(result_file,
chars_as_strings=True,
squeeze_me=True)
frame_id = "/".join(result["frame_id"].split("/")[1:])
frame = dataset.get_frame(frame_id)
adds = collections.defaultdict(list)
for gt_index, cls_id in enumerate(frame["meta"]["cls_indexes"]):
try:
pred_index = np.where(result["labels"] == cls_id)[0][0]
pose = result["poses"][pred_index]
T_pred = morefusion.geometry.compose_transform(
R=tf.quaternion_matrix(pose[:4])[:3, :3],
t=pose[4:],
)
T_true = np.r_[frame["meta"]["poses"][:, :, gt_index],
[[0, 0, 0, 1]], ]
pcd = models.get_pcd(class_id=cls_id)
add, add_s = morefusion.metrics.average_distance(
[pcd],
transform1=[T_true],
transform2=[T_pred],
)
add = add[0]
add_s = add_s[0]
except IndexError:
add = np.inf
add_s = np.inf
print(f"{result_file}, {cls_id:04d}, {add:.3f}, {add_s:.3f}")
adds[cls_id].append((add, add_s))
return adds
def _pcd_files_to_pts(pcd_files,
pts_file_npy,
pts_file,
obj_locations,
obj_rotations,
min_pts_size=0,
debug=False):
"""
Convert pcd blensor results to xyz or directly to npy files. Merge front and back scans.
Moving the object instead of the camera because the point cloud is in some very weird space that behaves
crazy when the camera moves. A full day wasted on this shit!
:param pcd_files:
:param pts_file_npy:
:param pts_file:
:param trafos_inv:
:param debug:
:return:
"""
import gzip
def revert_offset(pts_data: np.ndarray, inv_offset: np.ndarray):
pts_reverted = pts_data
# don't just check the header because missing rays may be added with NaNs
if pts_reverted.shape[0] > 0:
pts_offset_correction = np.broadcast_to(inv_offset,
pts_reverted.shape)
pts_reverted += pts_offset_correction
return pts_reverted
# https://www.blensor.org/numpy_import.html
def extract_xyz_from_blensor_numpy(arr_raw):
# timestamp
# yaw, pitch
# distance,distance_noise
# x,y,z
# x_noise,y_noise,z_noise
# object_id
# 255*color[0]
# 255*color[1]
# 255*color[2]
# idx
hits = arr_raw[arr_raw[:, 3] != 0.0] # distance != 0.0 --> hit
noisy_xyz = hits[:, [8, 9, 10]]
return noisy_xyz
pts_data_to_cat = []
for fi, f in enumerate(pcd_files):
try:
if f.endswith('.numpy.gz'):
pts_data_vs = extract_xyz_from_blensor_numpy(
np.loadtxt(gzip.GzipFile(f, "r")))
elif f.endswith('.numpy'):
pts_data_vs = extract_xyz_from_blensor_numpy(np.loadtxt(f))
elif f.endswith('.pcd'):
pts_data_vs, header_info = point_cloud.load_pcd(file_in=f)
else:
raise ValueError(
'Input file {} has an unknown format!'.format(f))
except EOFError as er:
print('Error processing {}: {}'.format(f, er))
continue
# undo coordinate system changes
pts_data_vs = utils.right_handed_to_left_handed(pts_data_vs)
# move back from camera distance, always along x axis
obj_location = np.array(obj_locations[fi])
revert_offset(pts_data_vs, -obj_location)
# get and apply inverse rotation matrix of camera
scanner_rotation_inv = trafo.quaternion_matrix(
trafo.quaternion_conjugate(obj_rotations[fi]))
pts_data_ws_test_inv = trafo.transform_points(pts_data_vs,
scanner_rotation_inv,
translate=False)
pts_data_ws = pts_data_ws_test_inv
if pts_data_ws.shape[0] > 0:
pts_data_to_cat += [pts_data_ws.astype(np.float32)]
# debug outputs to check the rotations... the pointcloud MUST align exactly with the mesh
if debug:
point_cloud.write_xyz(file_path=os.path.join(
'debug', 'test_{}.xyz'.format(str(fi))),
points=pts_data_ws)
if len(pts_data_to_cat) > 0:
pts_data = np.concatenate(tuple(pts_data_to_cat), axis=0)
if pts_data.shape[0] > min_pts_size:
point_cloud.write_xyz(file_path=pts_file, points=pts_data)
np.save(pts_file_npy, pts_data)
mask_bg = np.ones(rgb.shape[:2], dtype=bool)
for ins_id, mask in zip(result["labels"], result["masks"]):
mask = mask.astype(bool)
mapping.initialize(ins_id, pitch=0.01)
mapping.integrate(ins_id, mask, pcd_scene)
mask_bg = mask_bg & (~mask)
mapping.initialize(0, pitch=0.01)
mapping.integrate(0, mask_bg, pcd_scene)
instance_ids_all = np.r_[0, frame["meta"]["cls_indexes"]]
with morefusion.utils.timer(frame_id):
poses_refined = np.zeros_like(result["poses"])
for i, (cls_id, mask, pose) in enumerate(
zip(result["labels"], result["masks"], result["poses"])):
transform_init = morefusion.geometry.compose_transform(
R=tf.quaternion_matrix(pose[:4])[:3, :3],
t=pose[4:],
)
pcd_cad = models.get_pcd(class_id=cls_id)
mask = mask.astype(bool) & nonnan
pcd_depth = pcd_scene[mask]
centroid = np.mean(pcd_depth, axis=0)
diagonal = models.get_bbox_diagonal(class_id=cls_id)
aabb_min = centroid - diagonal / 2
aabb_max = aabb_min + diagonal
occupied_t, empty_i = mapping.get_target_pcds(
cls_id, aabb_min, aabb_max)
occupied_u = []
def __init__(
self,
root_folder,
batch_size,
num_grasp_clusters,
npoints,
min_difference_allowed=(0, 0, 0),
max_difference_allowed=(3, 3, 0),
occlusion_nclusters=0,
occlusion_dropout_rate=0.,
caching=True,
run_in_another_process=True,
collision_hard_neg_min_translation=(-0.03,-0.03,-0.03),
collision_hard_neg_max_translation=(0.03,0.03,0.03),
collision_hard_neg_min_rotation=(-0.6,-0.2,-0.6),
collision_hard_neg_max_rotation=(+0.6,+0.2,+0.6),
collision_hard_neg_num_perturbations=10,
use_uniform_quaternions=False,
ratio_of_grasps_used=1.0,
ratio_positive=0.3,
ratio_hardnegative=0.4,
balanced_data=True,
):
self._root_folder = root_folder
self._batch_size = batch_size
self._num_grasp_clusters = num_grasp_clusters
self._max_difference_allowed = max_difference_allowed
self._min_difference_allowed = min_difference_allowed
self._npoints = npoints
self._occlusion_nclusters = occlusion_nclusters
self._occlusion_dropout_rate = occlusion_dropout_rate
self._caching = caching
self._collision_hard_neg_min_translation = collision_hard_neg_min_translation
self._collision_hard_neg_max_translation = collision_hard_neg_max_translation
self._collision_hard_neg_min_rotation = collision_hard_neg_min_rotation
self._collision_hard_neg_max_rotation = collision_hard_neg_max_rotation
self._collision_hard_neg_num_perturbations = collision_hard_neg_num_perturbations
self._collision_hard_neg_queue = {}
self._ratio_of_grasps_used = ratio_of_grasps_used
self._ratio_positive = ratio_positive
self._ratio_hardnegative = ratio_hardnegative
self._balanced_data = balanced_data
for i in range(3):
assert(collision_hard_neg_min_rotation[i] <= collision_hard_neg_max_rotation[i])
assert(collision_hard_neg_min_translation[i] <= collision_hard_neg_max_translation[i])
self._current_pc = None
self._cache = {}
if run_in_another_process:
self._renderer = OnlineObjectRendererMultiProcess(caching=True)
else:
self._renderer = OnlineObjectRenderer(caching=True)
self._renderer.start()
if use_uniform_quaternions:
quaternions = [l[:-1].split('\t') for l in open('uniform_quaternions/data2_4608.qua', 'r').readlines()]
quaternions = [[float(t[0]),
float(t[1]),
float(t[2]),
float(t[3])] for t in quaternions]
quaternions = np.asarray(quaternions)
quaternions = np.roll(quaternions, 1, axis=1)
self._all_poses = [tra.quaternion_matrix(q) for q in quaternions]
else:
self._all_poses = []
for az in np.linspace(0, np.pi * 2, 30):
for el in np.linspace(-np.pi / 2, np.pi / 2, 30):
self._all_poses.append(tra.euler_matrix(el, az, 0))
self._eval_files = [json.load(open(f)) for f in glob.glob(os.path.join(self._root_folder, 'splits', '*.json'))]
def sample_multiple_grasps(number_of_candidates,
mesh,
gripper_name,
systematic_sampling,
surface_density=0.005 * 0.005,
standoff_density=0.01,
roll_density=15,
type_of_quality='antipodal',
min_quality=-1.0,
silent=False):
"""Sample a set of grasps for an object.
Arguments:
number_of_candidates {int} -- Number of grasps to sample
mesh {trimesh} -- Object mesh
gripper_name {str} -- Name of gripper model
systematic_sampling {bool} -- Whether to use grid sampling for roll
Keyword Arguments:
surface_density {float} -- surface density, in m^2 (default: {0.005*0.005})
standoff_density {float} -- density for standoff, in m (default: {0.01})
roll_density {float} -- roll density, in deg (default: {15})
type_of_quality {str} -- quality metric (default: {'antipodal'})
min_quality {float} -- minimum grasp quality (default: {-1})
silent {bool} -- verbosity (default: {False})
Raises:
Exception: Unknown quality metric
Returns:
[type] -- points, normals, transforms, roll_angles, standoffs, collisions, quality
"""
origins = []
orientations = []
transforms = []
standoffs = []
roll_angles = []
gripper = create_gripper(gripper_name)
if systematic_sampling:
# systematic sampling. input:
# - Surface density:
# - Standoff density:
# - Rotation density:
# Resulting number of samples:
# (Area/Surface Density) * (Finger length/Standoff density) * (360/Rotation Density)
surface_samples = int(np.ceil(mesh.area / surface_density))
standoff_samples = np.linspace(
gripper.standoff_range[0], gripper.standoff_range[1],
max(1, (gripper.standoff_range[1] - gripper.standoff_range[0]) /
standoff_density))
rotation_samples = np.arange(0, 1 * np.pi, np.deg2rad(roll_density))
number_of_candidates = surface_samples * \
len(standoff_samples) * len(rotation_samples)
tmp_points, face_indices = mesh.sample(surface_samples,
return_index=True)
tmp_normals = mesh.face_normals[face_indices]
number_of_candidates = len(tmp_points) * \
len(standoff_samples) * len(rotation_samples)
print("Number of samples ", number_of_candidates, "(", len(tmp_points),
" x ", len(standoff_samples), " x ", len(rotation_samples), ")")
points = []
normals = []
position_idx = []
pos_cnt = 0
cnt = 0
batch_position_idx = []
batch_points = []
batch_normals = []
batch_roll_angles = []
batch_standoffs = []
batch_transforms = []
for point, normal in tqdm(zip(tmp_points, tmp_normals),
total=len(tmp_points),
disable=silent):
for roll in rotation_samples:
for standoff in standoff_samples:
batch_position_idx.append(pos_cnt)
batch_points.append(point)
batch_normals.append(normal)
batch_roll_angles.append(roll)
batch_standoffs.append(standoff)
orientation = tra.quaternion_matrix(
tra.quaternion_about_axis(roll, [0, 0, 1]))
origin = point + normal * standoff
batch_transforms.append(
np.dot(
np.dot(
tra.translation_matrix(origin),
trimesh.geometry.align_vectors([0, 0, -1],
normal)),
orientation))
cnt += 1
pos_cnt += 1
if cnt % 1000 == 0 or cnt == len(tmp_points):
valid = raycast_collisioncheck(np.asarray(batch_transforms),
np.asarray(batch_points), mesh)
transforms.extend(np.array(batch_transforms)[valid])
position_idx.extend(np.array(batch_position_idx)[valid])
points.extend(np.array(batch_points)[valid])
normals.extend(np.array(batch_normals)[valid])
roll_angles.extend(np.array(batch_roll_angles)[valid])
standoffs.extend(np.array(batch_standoffs)[valid])
batch_position_idx = []
batch_points = []
batch_normals = []
batch_roll_angles = []
batch_standoffs = []
batch_transforms = []
points = np.array(points)
normals = np.array(normals)
position_idx = np.array(position_idx)
else:
points, face_indices = mesh.sample(number_of_candidates,
return_index=True)
normals = mesh.face_normals[face_indices]
# generate transformations
for point, normal in tqdm(zip(points, normals),
total=len(points),
disable=silent):
# roll along approach vector
angle = np.random.rand() * 2 * np.pi
roll_angles.append(angle)
orientations.append(
tra.quaternion_matrix(
tra.quaternion_about_axis(angle, [0, 0, 1])))
# standoff from surface
standoff = (gripper.standoff_range[1] - gripper.standoff_range[0]) * np.random.rand() \
+ gripper.standoff_range[0]
standoffs.append(standoff)
origins.append(point + normal * standoff)
transforms.append(
np.dot(
np.dot(tra.translation_matrix(origins[-1]),
trimesh.geometry.align_vectors([0, 0, -1], normal)),
orientations[-1]))
verboseprint("Checking collisions...")
collisions = in_collision_with_gripper(mesh,
transforms,
gripper_name=gripper_name,
silent=silent)
verboseprint("Labelling grasps...")
quality = {}
quality_key = 'quality_' + type_of_quality
if type_of_quality == 'antipodal':
quality[quality_key] = grasp_quality_antipodal(
transforms,
collisions,
object_mesh=mesh,
gripper_name=gripper_name,
silent=silent)
elif type_of_quality == 'number_of_contacts':
quality[quality_key] = grasp_quality_point_contacts(
transforms,
collisions,
object_mesh=mesh,
gripper_name=gripper_name,
silent=silent)
else:
raise Exception("Quality metric unknown: ", quality)
# Filter out by quality
quality_np = np.array(quality[quality_key])
collisions = np.array(collisions)
f_points = []
f_normals = []
f_transforms = []
f_roll_angles = []
f_standoffs = []
f_collisions = []
f_quality = []
for i, _ in enumerate(transforms):
if quality_np[i] >= min_quality:
f_points.append(points[i])
f_normals.append(normals[i])
f_transforms.append(transforms[i])
f_roll_angles.append(roll_angles[i])
f_standoffs.append(standoffs[i])
f_collisions.append(int(collisions[i]))
f_quality.append(quality_np[i])
points = np.array(f_points)
normals = np.array(f_normals)
transforms = np.array(f_transforms)
roll_angles = np.array(f_roll_angles)
standoffs = np.array(f_standoffs)
collisions = f_collisions
quality[quality_key] = f_quality
return points, normals, transforms, roll_angles, standoffs, collisions, quality
def generate_candidates(mesh_properties):
"""Generates grasp candidates via predicted 6DoF pose of gripper."""
bbox = mesh_properties['bbox']
work2obj = mesh_properties['work2obj']
obj2grip = mesh_properties['obj2grip']
# Extract the bounding box corners and planes of the object
_, planes = get_corners_and_plances(bbox)
xyz_grid = [to_rad(x, y, z)
for x in range(0, 360, GLOBAL_X) \
for y in range(0, 360, GLOBAL_Y) \
for z in range(0, 360, GLOBAL_Z)]
total_trials = len(xyz_grid)**2
# Angles holds the information about global and local rotations
# Matrices holds information about the final transformation.
angles = np.empty((total_trials, 6))
matrices = np.empty((total_trials, 12))
# Going to extend a vector along z-direction of palm. The extra 1 o the end
# is for multiplying with a 4x4 HT matrix
palm_axis = np.atleast_2d([0, 0, 10, 1]).T
curr_trial = 0
success = 0
# Exhaustively evaluate local and global rotations
for gx, gy, gz in xyz_grid:
for lx, ly, lz in xyz_grid:
# Monitor our status, print something every 10%
if curr_trial % int(0.1 * total_trials) == 0:
print 'Trial %d/%d, successful: %d'%\
(curr_trial, total_trials, success)
curr_trial += 1
# Build the global rotation matrix using quaternions
q_gx = tf.quaternion_about_axis(gx + rand_step(GLOBAL_X),
[1, 0, 0])
q_gy = tf.quaternion_about_axis(gy + rand_step(GLOBAL_Y),
[0, 1, 0])
q_gz = tf.quaternion_about_axis(gz + rand_step(GLOBAL_Z),
[0, 0, 1])
# Build the local rotation matrix using quaternions
q_lx = tf.quaternion_about_axis(lx + rand_step(GLOBAL_X),
[1, 0, 0])
q_ly = tf.quaternion_about_axis(ly + rand_step(GLOBAL_Y),
[0, 1, 0])
q_lz = tf.quaternion_about_axis(lz + rand_step(GLOBAL_Z),
[0, 0, 1])
# Multiply global and local rotations
global_rotation = tf.quaternion_multiply(q_gx, q_gy)
global_rotation = tf.quaternion_multiply(global_rotation, q_gz)
global_rotation = tf.quaternion_matrix(global_rotation)
local_rotation = tf.quaternion_multiply(q_lx, q_ly)
local_rotation = tf.quaternion_multiply(local_rotation, q_lz)
local_rotation = tf.quaternion_matrix(local_rotation)
rotation = np.dot(np.dot(global_rotation, obj2grip),
local_rotation)
# Don't want to test any candidates that are below the workspace
workspace2grip = np.dot(work2obj, rotation)
if workspace2grip[2, 3] < 0:
continue
# Check if a line from the center of the grippers palm intersects
# with the planes of the object
line_start = rotation[:3, 3]
line_end = np.dot(rotation, palm_axis).flatten()[:3]
for plane in planes:
itx_plane = intersect_plane(line_start, line_end, *plane)
if intersect_box(itx_plane, bbox) is False:
continue
angles[success] = np.asarray([gx, gy, gz, lx, ly, lz])
matrices[success] = rotation[:3].flatten()
success = success + 1
break
# Only keep the successful transformations
angles = np.asarray(angles)[:success]
matrices = np.asarray(matrices)[:success]
return angles, matrices
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter, )
parser.add_argument("model", help="model file in a log dir")
parser.add_argument("--gpu", type=int, default=0, help="gpu id")
parser.add_argument("--save", action="store_true", help="save")
args = parser.parse_args()
args_file = path.Path(args.model).parent / "args"
with open(args_file) as f:
args_data = json.load(f)
pprint.pprint(args_data)
if args.gpu >= 0:
chainer.cuda.get_device_from_id(args.gpu).use()
model = singleview_3d.models.Model(
n_fg_class=len(args_data["class_names"][1:]),
pretrained_resnet18=args_data["pretrained_resnet18"],
with_occupancy=args_data["with_occupancy"],
loss=args_data["loss"],
loss_scale=args_data["loss_scale"],
)
if args.gpu >= 0:
model.to_gpu()
print(f"==> Loading trained model: {args.model}")
chainer.serializers.load_npz(args.model, model)
print("==> Done model loading")
split = "val"
dataset = morefusion.datasets.YCBVideoRGBDPoseEstimationDataset(
split=split)
dataset_reindexed = morefusion.datasets.YCBVideoRGBDPoseEstimationDatasetReIndexed( # NOQA
split=split,
class_ids=args_data["class_ids"],
)
transform = Transform(
train=False,
with_occupancy=args_data["with_occupancy"],
)
pprint.pprint(args.__dict__)
# -------------------------------------------------------------------------
depth2rgb = imgviz.Depth2RGB()
for index in range(len(dataset)):
frame = dataset.get_frame(index)
image_id = dataset._ids[index]
indices = dataset_reindexed.get_indices_from_image_id(image_id)
examples = dataset_reindexed[indices]
examples = [transform(example) for example in examples]
if not examples:
continue
inputs = chainer.dataset.concat_examples(examples, device=args.gpu)
with chainer.no_backprop_mode() and chainer.using_config(
"train", False):
quaternion_pred, translation_pred, confidence_pred = model.predict(
class_id=inputs["class_id"],
rgb=inputs["rgb"],
pcd=inputs["pcd"],
pitch=inputs.get("pitch"),
origin=inputs.get("origin"),
grid_nontarget_empty=inputs.get("grid_nontarget_empty"),
)
indices = model.xp.argmax(confidence_pred.array, axis=1)
quaternion_pred = quaternion_pred[
model.xp.arange(quaternion_pred.shape[0]), indices]
translation_pred = translation_pred[
model.xp.arange(translation_pred.shape[0]), indices]
reporter = chainer.Reporter()
reporter.add_observer("main", model)
observation = {}
with reporter.scope(observation):
model.evaluate(
class_id=inputs["class_id"],
quaternion_true=inputs["quaternion_true"],
translation_true=inputs["translation_true"],
quaternion_pred=quaternion_pred,
translation_pred=translation_pred,
)
# TODO(wkentaro)
observation_new = {}
for k, v in observation.items():
if re.match(r"main/add_or_add_s/[0-9]+/.+", k):
k_new = "/".join(k.split("/")[:-1])
observation_new[k_new] = v
observation = observation_new
print(f"[{index:08d}] {observation}")
# ---------------------------------------------------------------------
K = frame["intrinsic_matrix"]
height, width = frame["rgb"].shape[:2]
fovy = trimesh.scene.Camera(resolution=(width, height),
focal=(K[0, 0], K[1, 1])).fov[1]
batch_size = len(inputs["class_id"])
class_ids = cuda.to_cpu(inputs["class_id"])
quaternion_pred = cuda.to_cpu(quaternion_pred.array)
translation_pred = cuda.to_cpu(translation_pred.array)
quaternion_true = cuda.to_cpu(inputs["quaternion_true"])
translation_true = cuda.to_cpu(inputs["translation_true"])
Ts_pred = []
Ts_true = []
for i in range(batch_size):
# T_cad2cam
T_pred = tf.quaternion_matrix(quaternion_pred[i])
T_pred[:3, 3] = translation_pred[i]
T_true = tf.quaternion_matrix(quaternion_true[i])
T_true[:3, 3] = translation_true[i]
Ts_pred.append(T_pred)
Ts_true.append(T_true)
Ts = dict(true=Ts_true, pred=Ts_pred)
vizs = []
depth_viz = depth2rgb(frame["depth"])
for which in ["true", "pred"]:
pybullet.connect(pybullet.DIRECT)
for i, T in enumerate(Ts[which]):
cad_file = morefusion.datasets.YCBVideoModels().get_cad_file(
class_id=class_ids[i])
morefusion.extra.pybullet.add_model(
cad_file,
position=tf.translation_from_matrix(T),
orientation=tf.quaternion_from_matrix(T)[[1, 2, 3, 0]],
)
(
rgb_rend,
depth_rend,
segm_rend,
) = morefusion.extra.pybullet.render_camera(
np.eye(4), fovy, height, width)
pybullet.disconnect()
segm_rend = imgviz.label2rgb(segm_rend + 1,
img=frame["rgb"],
alpha=0.7)
depth_rend = depth2rgb(depth_rend)
rgb_input = imgviz.tile(cuda.to_cpu(inputs["rgb"]),
border=(255, 255, 255))
viz = imgviz.tile(
[
frame["rgb"],
depth_viz,
rgb_input,
segm_rend,
rgb_rend,
depth_rend,
],
(1, 6),
border=(255, 255, 255),
)
viz = imgviz.resize(viz, width=1800)
if which == "pred":
text = []
for class_id in np.unique(class_ids):
add = observation[f"main/add_or_add_s/{class_id:04d}"]
text.append(f"[{which}] [{class_id:04d}]: "
f"add/add_s={add * 100:.1f}cm")
text = "\n".join(text)
else:
text = f"[{which}]"
viz = imgviz.draw.text_in_rectangle(
viz,
loc="lt",
text=text,
size=20,
background=(0, 255, 0),
color=(0, 0, 0),
)
if which == "true":
viz = imgviz.draw.text_in_rectangle(
viz,
loc="rt",
text="singleview_3d",
size=20,
background=(255, 0, 0),
color=(0, 0, 0),
)
vizs.append(viz)
viz = imgviz.tile(vizs, (2, 1), border=(255, 255, 255))
if args.save:
out_file = path.Path(args.model).parent / f"video/{index:08d}.jpg"
out_file.parent.makedirs_p()
imgviz.io.imsave(out_file, viz)
yield viz
def _r_from_q(q):
q_wxyz = np.roll(q, 1)
return transformations.quaternion_matrix(q_wxyz)[:3, :3]
