def get_image_depth_and_mask(scene: pyrender.Scene,
scene_setup_loader: DiceScene.SceneSetupLoader,
width: int, height: int,
keep_nodes_in_scene: bool):
"""Renders an image ggiven a scene and seetup, along with the depth and segmentation mask labelling each die."""
r = pyrender.OffscreenRenderer(width, height)
color_bg, depth_bg = r.render(scene)
depth_nodes = []
for node in scene_setup_loader.dice_nodes:
scene.add_node(node)
color_node, depth_node = r.render(scene)
depth_nodes.append(depth_node)
scene.remove_node(node)
scene_setup_loader.add_loaded_to_scene(scene)
color_final, depth_final = r.render(scene)
if not keep_nodes_in_scene:
scene_setup_loader.remove_nodes_from_scene(scene)
#Initialize labels of pixels to -1 (for background)
labels_mask = np.ones((height, width), dtype=np.int8) * -1
for index, depth_for_node in enumerate(depth_nodes):
depth_not_background = np.not_equal(depth_bg, depth_for_node)
depth_at_foreground = np.equal(depth_final, depth_for_node)
depth_at_dice = np.logical_and(depth_not_background,
depth_at_foreground)
labels_mask[depth_at_dice] = index
return color_final, depth_final, labels_mask
def _get_dice_to_camera_transform(camera_node : pyrender.Node, dice_node : pyrender.Node, scene: pyrender.Scene):
"""Gets the transformation matrix from dice node to camera node coordinates."""
dice_scene_pose = scene.get_pose(dice_node)
camera_scene_pose = scene.get_pose(camera_node)
scene_to_camera_pose = pose_inverse(camera_scene_pose)
dice_to_camera_transform = scene_to_camera_pose @ dice_scene_pose
return dice_to_camera_transform
def _build_scene(self, path, size, light, y_fov):
self.scene = Scene(bg_color=[0, 0, 0, 0])
self.light = self.scene.add(DirectionalLight([1.0, 1.0, 1.0], light))
self.camera = self.scene.add(
PerspectiveCamera(y_fov, aspectRatio=np.divide(*size)))
self.mesh = self.scene.add(
Mesh.from_trimesh(trimesh.load(path), smooth=True))
self.world_origin = self.mesh.mesh.centroid
def quick_color_visualize():
scene = Scene(bg_color=[0, 0, 0])
root = os.path.expanduser('~')
mesh_path = '.keras/paz/datasets/ycb_models/035_power_drill/textured.obj'
path = os.path.join(root, mesh_path)
mesh = color_object(path)
scene.add(mesh)
Viewer(scene, use_raymond_lighting=True, flags=RenderFlags.FLAT)
def _create_scene(self):
self._scene = Scene()
camera = PerspectiveCamera(yfov=0.833,
znear=0.05,
zfar=3.0,
aspectRatio=1.0)
cn = Node()
cn.camera = camera
pose_m = np.array([[0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.88], [0.0, 0.0, 0.0, 1.0]])
pose_m[:, 1:3] *= -1.0
cn.matrix = pose_m
self._scene.add_node(cn)
self._scene.main_camera_node = cn
def __init__(self):
"""
"""
self.scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
self.scene.add(PointLight(color=[0.5, 0.2, 0.3], intensity=2.0))
self.scene.add(
SpotLight(color=[0.1, 0.6, 0.3],
intensity=2.0,
innerConeAngle=0.05,
outerConeAngle=0.5))
self.scene.add(
DirectionalLight(color=[0.33, 0.33, 0.33], intensity=2.0))
self.root_node = None
class PixelMaskRenderer():
"""Render-ready scene composed of a single object and a single moving camera.
# Arguments
path_OBJ: String containing the path to an OBJ file.
viewport_size: List, specifying [H, W] of rendered image.
y_fov: Float indicating the vertical field of view in radians.
distance: List of floats indicating [max_distance, min_distance]
light: List of floats indicating [max_light, min_light]
top_only: Boolean. If True images are only take from the top.
roll: Float, to sample [-roll, roll] rolls of the Z OpenGL camera axis.
shift: Float, to sample [-shift, shift] to move in X, Y OpenGL axes.
"""
def __init__(self, path_OBJ, viewport_size=(128, 128), y_fov=3.14159 / 4.0,
distance=[0.3, 0.5], light=[0.5, 30], top_only=False,
roll=None, shift=None):
self.distance, self.roll, self.shift = distance, roll, shift
self.light_intensity, self.top_only = light, top_only
self._build_scene(path_OBJ, viewport_size, light, y_fov)
self.renderer = OffscreenRenderer(viewport_size[0], viewport_size[1])
self.flags_RGBA = RenderFlags.RGBA
self.flags_FLAT = RenderFlags.RGBA | RenderFlags.FLAT
self.epsilon = 0.01
def _build_scene(self, path, size, light, y_fov):
self.scene = Scene(bg_color=[0, 0, 0, 0])
self.light = self.scene.add(
DirectionalLight([1.0, 1.0, 1.0], np.mean(light)))
self.camera = self.scene.add(
PerspectiveCamera(y_fov, aspectRatio=np.divide(*size)))
self.pixel_mesh = self.scene.add(color_object(path))
self.mesh = self.scene.add(
Mesh.from_trimesh(trimesh.load(path), smooth=True))
self.world_origin = self.mesh.mesh.centroid
def _sample_parameters(self):
distance = sample_uniformly(self.distance)
camera_origin = sample_point_in_sphere(distance, self.top_only)
camera_origin = random_perturbation(camera_origin, self.epsilon)
light_intensity = sample_uniformly(self.light_intensity)
return camera_origin, light_intensity
def render(self):
camera_origin, intensity = self._sample_parameters()
camera_to_world, world_to_camera = compute_modelview_matrices(
camera_origin, self.world_origin, self.roll, self.shift)
self.light.light.intensity = intensity
self.scene.set_pose(self.camera, camera_to_world)
self.scene.set_pose(self.light, camera_to_world)
self.pixel_mesh.mesh.is_visible = False
image, depth = self.renderer.render(self.scene, self.flags_RGBA)
self.pixel_mesh.mesh.is_visible = True
image, alpha = split_alpha_channel(image)
self.mesh.mesh.is_visible = False
RGB_mask, _ = self.renderer.render(self.scene, self.flags_FLAT)
self.mesh.mesh.is_visible = True
return image, alpha, RGB_mask
def __init__(self, path_OBJ, camera_pose, min_corner, max_corner,
symmetric_transforms, viewport_size=(128, 128),
y_fov=3.14159 / 4.0, light_intensity=[0.5, 30]):
self.light_intensity = light_intensity
self.symmetric_transforms = symmetric_transforms
self.min_corner, self.max_corner = min_corner, max_corner
self.scene = Scene(bg_color=[0, 0, 0, 0])
self.light = self._build_light(light_intensity, camera_pose)
self.camera = self._build_camera(y_fov, viewport_size, camera_pose)
self.pixel_mesh = self.scene.add(color_object(path_OBJ))
self.mesh = self.scene.add(
Mesh.from_trimesh(trimesh.load(path_OBJ), smooth=True))
self.renderer = OffscreenRenderer(viewport_size[0], viewport_size[1])
self.flags_RGBA = RenderFlags.RGBA
self.flags_FLAT = RenderFlags.RGBA | RenderFlags.FLAT
def get_scene_space_up_face_index(dice_node : pyrender.Node, scene: pyrender.Scene):
"""Gets the face-up index of a given dice node.
Indices start at 1, for face with 1 dot up"""
face_normals = DiceConfig.get_local_face_normals()
dice_pose = scene.get_pose(dice_node)
scene_face_normals = transform_points_3d(face_normals, dice_pose, True)
scene_face_normals_y = scene_face_normals[1,:]
index = np.argmax(scene_face_normals_y)
index_starting_at_1 = index + 1
return index_starting_at_1
def figure(bg_color=(1.0, 1.0, 1.0), size=(1000, 1000)):
"""Create a viewing window.
Parameters
----------
bg_color : (3,) float
The background RGB color.
size : (2,) int
The width and height of the window in pixels.
"""
Visualizer3D._scene = Scene(bg_color=np.array(bg_color))
Visualizer3D._size = size
def render_big_gallery(results_dir,
nb=30,
pts_colors=[0.5, 0.5, 0.5],
draw_text=False):
'''
pts_colors: [0,0,0]
return np array of a big image
'''
cam = PerspectiveCamera(yfov=(YFOV))
cam_pose = CAM_POSE
point_l = PointLight(color=np.ones(3), intensity=POINT_LIGHT_INTENSITY)
scene = Scene(bg_color=np.array([1, 1, 1, 0]))
# cam and light
_ = scene.add(cam, pose=cam_pose)
_ = scene.add(point_l, pose=cam_pose)
input_ply_filenames = get_all_filnames(results_dir, nb)
r = OffscreenRenderer(viewport_width=640 * 2,
viewport_height=480 * 2,
point_size=POINT_SIZE)
pc_pose = PC_POSE
images = []
for _, input_pf in enumerate(input_ply_filenames):
input_pc = read_ply_xyz(input_pf)
colors = np.array(pts_colors)
colors = np.tile(colors, (input_pc.shape[0], 1))
input_pc_node = add_point_cloud_mesh_to_scene(input_pc, scene, pc_pose,
colors)
renderred_color, _ = r.render(scene)
scene.remove_node(input_pc_node)
if draw_text:
im_here = Image.fromarray(renderred_color)
d = ImageDraw.Draw(im_here)
fnt = ImageFont.truetype(
font='/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf',
size=100)
d.text((0, 0),
input_pf.split('/')[-1],
fill=(0, 0, 0, 255),
font=fnt)
renderred_color = np.array(im_here)
images.append(renderred_color)
big_gallery = np.concatenate(images, axis=0)
r.delete()
return big_gallery
def build_scene(num_cubes, color_candidates):
# Generate positions of each cube
cube_position_array, barycenter = generate_block_positions(num_cubes)
assert len(cube_position_array) == num_cubes
# Place cubes
scene = Scene(bg_color=np.array([0.0, 0.0, 0.0]),
ambient_light=np.array([0.3, 0.3, 0.3, 1.0]))
cube_nodes = []
for position in cube_position_array:
mesh = trimesh.creation.box(extents=cube_size * np.ones(3))
mesh = Mesh.from_trimesh(mesh, smooth=False)
node = Node(mesh=mesh,
translation=np.array(([
position[0] - barycenter[0],
position[1] - barycenter[1],
position[2] - barycenter[2],
])))
scene.add_node(node)
cube_nodes.append(node)
update_cube_color_and_position(cube_nodes, color_candidates)
# Place a light
light = DirectionalLight(color=np.ones(3), intensity=15.0)
quaternion_yaw = pyrender.quaternion.from_yaw(math.pi / 4)
quaternion_pitch = pyrender.quaternion.from_pitch(-math.pi / 5)
quaternion = pyrender.quaternion.multiply(quaternion_pitch, quaternion_yaw)
quaternion = quaternion / np.linalg.norm(quaternion)
node = Node(light=light,
rotation=quaternion,
translation=np.array([1, 1, 1]))
scene.add_node(node)
return scene, cube_nodes
def _reset_scene(self, scale_factor=1.0):
""" Resets the scene.
Parameters
----------
scale_factor : float
optional scale factor to apply to the image dimensions
"""
# delete scene
if self._scene is not None:
self._scene.clear()
del self._scene
# create scene
scene = Scene()
# setup camera
camera = IntrinsicsCamera(self.camera.intrinsics.fx, self.camera.intrinsics.fy,
self.camera.intrinsics.cx, self.camera.intrinsics.cy)
pose_m = self.camera.pose.matrix.copy()
pose_m[:,1:3] *= -1.0
scene.add(camera, pose=pose_m, name=self.camera.frame)
scene.main_camera_node = next(iter(scene.get_nodes(name=self.camera.frame)))
# add workspace objects
for obj_key in self.state.workspace_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
# add scene objects
for obj_key in self.state.obj_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
self._scene = scene
def __init__(self,
filepath,
viewport_size=(128, 128),
y_fov=3.14159 / 4.,
distance=0.30,
top_only=False,
light=5.0,
theta_steps=10,
phi_steps=10):
self.scene = Scene(bg_color=[0, 0, 0])
self.camera = self.scene.add(
PerspectiveCamera(y_fov, aspectRatio=np.divide(*viewport_size)))
self.mesh = self.scene.add(
Mesh.from_trimesh(trimesh.load(filepath), smooth=True))
self.world_origin = self.mesh.mesh.centroid
self.light = self.scene.add(DirectionalLight([1.0, 1.0, 1.0], light))
self.distance = distance
# 0.1 values are to avoid gimbal lock
theta_max = np.pi / 2.0 if top_only else np.pi
self.thetas = np.linspace(0.1, theta_max - 0.1, theta_steps)
self.phis = np.linspace(0.1, 2 * np.pi - 0.1, phi_steps)
self.renderer = OffscreenRenderer(*viewport_size)
self.RGBA = RenderFlags.RGBA
def render_big_gallery_overlay(dir_1,
dir_2,
pts_color_1=[0.5, 0.5, 0.5],
pts_color_2=[0.5, 0.5, 0.5],
nb=30):
'''
return np array of a big image
'''
cam = PerspectiveCamera(yfov=(YFOV))
cam_pose = CAM_POSE
point_l = PointLight(color=np.ones(3), intensity=POINT_LIGHT_INTENSITY)
scene = Scene(bg_color=np.array([1, 1, 1, 0]))
# cam and light
_ = scene.add(cam, pose=cam_pose)
_ = scene.add(point_l, pose=cam_pose)
input_ply_filenames_1 = get_all_filnames(dir_1, nb)
input_ply_filenames_2 = get_all_filnames(dir_2, nb)
r = OffscreenRenderer(viewport_width=640 * 2,
viewport_height=480 * 2,
point_size=POINT_SIZE)
pc_pose = PC_POSE
images = []
for idx, input_pf in enumerate(input_ply_filenames_1):
input_pc_1 = read_ply_xyz(input_pf)
input_pc_2 = read_ply_xyz(input_ply_filenames_2[idx])
color_1 = np.array(pts_color_1)
color_1 = np.tile(color_1, (input_pc_1.shape[0], 1))
color_2 = np.array(pts_color_2)
color_2 = np.tile(color_2, (input_pc_2.shape[0], 1))
input_pc_node_1 = add_point_cloud_mesh_to_scene(
input_pc_1, scene, pc_pose, color_1)
input_pc_node_2 = add_point_cloud_mesh_to_scene(
input_pc_2, scene, pc_pose, color_2)
renderred_color, _ = r.render(scene)
scene.remove_node(input_pc_node_1)
scene.remove_node(input_pc_node_2)
images.append(renderred_color)
big_gallery = np.concatenate(images, axis=0)
r.delete()
return big_gallery
def build_scene(max_num_cubes, color_candidates, probabilities):
num_cubes = np.random.choice(np.arange(1, max_num_cubes + 1),
size=1,
p=probabilities)[0]
# Generate positions of each cube
cube_position_array, barycenter = generate_block_positions(num_cubes)
assert len(cube_position_array) == num_cubes
# Place cubes
scene = Scene(bg_color=np.array([0.0, 0.0, 0.0]),
ambient_light=np.array([0.3, 0.3, 0.3, 1.0]))
cube_nodes = []
for position in cube_position_array:
mesh = trimesh.creation.box(extents=cube_size * np.ones(3))
mesh = Mesh.from_trimesh(mesh, smooth=False)
node = Node(mesh=mesh,
translation=np.array(([
position[0] - barycenter[0],
position[1] - barycenter[1],
position[2] - barycenter[2],
])))
scene.add_node(node)
cube_nodes.append(node)
# Generate positions of each cube
cube_position_array, barycenter = generate_block_positions(num_cubes)
for position, node in zip(cube_position_array, cube_nodes):
color = np.array(random.choice(color_candidates))
vertex_colors = np.broadcast_to(
color, node.mesh.primitives[0].positions.shape)
node.mesh.primitives[0].color_0 = vertex_colors
node.translation = np.array(([
position[0] - barycenter[0],
position[1] - barycenter[1],
position[2] - barycenter[2],
]))
# Place a light
light = DirectionalLight(color=np.ones(3), intensity=15.0)
quaternion_yaw = pyrender.quaternion.from_yaw(math.pi / 4)
quaternion_pitch = pyrender.quaternion.from_pitch(-math.pi / 5)
quaternion = pyrender.quaternion.multiply(quaternion_pitch, quaternion_yaw)
quaternion = quaternion / np.linalg.norm(quaternion)
node = Node(light=light,
rotation=quaternion,
translation=np.array([1, 1, 1]))
scene.add_node(node)
return scene, cube_nodes
def render_mesh(mesh, h=256, w=256):
"""https://pyrender.readthedocs.io/en/latest/examples/quickstart.html"""
mesh = pyrender.Mesh.from_trimesh(mesh.trimesh())
scene = Scene()
scene.add(mesh)
# z-axis away from the scene, x-axis right, y-axis up
pose = np.eye(4)
pose[2, 3] = 250
# add camera
camera = PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.0)
scene.add(camera, pose=pose)
# add light
# light = DirectionalLight(color=np.ones(3), intensity=5.0)
light = PointLight(color=[1.0, 1.0, 1.0], intensity=2.0)
scene.add(light, pose=pose)
r = OffscreenRenderer(h, w)
color, depth = r.render(scene)
return color
def get_y_rotation_angle_relative_to_camera(dice_node : pyrender.Node, scene: pyrender.Scene, dice_top_face_index : int):
"""Gets the rotation of a dice node around its vertical aspect with respect to the camera, given the top face index.
Note that for each upward-pointing face is defined another face as the forward-pointing face given by zero y-rotation."""
dice_to_camera_transform = _get_dice_to_camera_transform(scene.main_camera_node, dice_node, scene)
dice_pose = scene.get_pose(dice_node)
local_forward_axis = DiceConfig.get_local_face_forward(dice_top_face_index)[:, np.newaxis]
dice_scene_forward = transform_points_3d(local_forward_axis, dice_pose, True)
dice_scene_forward[1] = 0
dice_scene_forward /= np.linalg.norm(dice_scene_forward)
dice_to_camera_translation = dice_to_camera_transform[0:3,3]
direction_camera_from_dice = -dice_to_camera_translation[:, np.newaxis]
direction_camera_from_dice[1] = 0
direction_camera_from_dice /= np.linalg.norm(direction_camera_from_dice)
#Note x axis takes place of 'y' in arctan while z axis takes place of 'x' since y-axis rotations rotates x into -z (equivalently, z into x)
angle_forward = np.arctan2(dice_scene_forward[0], dice_scene_forward[2])
angle_direction = np.arctan2(direction_camera_from_dice[0], direction_camera_from_dice[2])
angle_difference = angle_forward - angle_direction
angle_difference_wrapped = (angle_difference + np.pi) % (2.0 * np.pi) - np.pi
return angle_difference_wrapped
def render_sensor(
point_set,
render_sensor_path="/Users/macbookpro15/Documents/mujoco_hand_exps/data/sensor_render"
):
"""
pointset: it is collectiono of sensor points for all timesteps
"""
# first take one of the point, subtract the center from it which
# I know is the 0-th position out of the 220 points
# form the mesh from this
if not os.path.exists(render_sensor_path):
os.makedirs(render_sensor_path)
time_steps = len(point_set)
for t in range(time_steps):
sensor = trimesh.load_mesh(
f'../data/mesh_dir/mesh_{t}_out/mc_mesh_out.ply')
sensor_mesh = Mesh.from_trimesh(sensor)
# Light for the scene
direc_l = DirectionalLight(color=np.ones(3), intensity=1.0)
spot_l = SpotLight(color=np.ones(3),
intensity=10.0,
innerConeAngle=np.pi / 16,
outerConeAngle=np.pi / 6)
point_l = PointLight(color=np.ones(3), intensity=10.0)
# add camera to the scene
cam = PerspectiveCamera(yfov=(np.pi / 3.0))
cam_pose = np.array([[0.0, -np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.5], [1.0, 0.0, 0.0, 0.0],
[0.0, np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.4], [0.0, 0.0, 0.0, 1.0]])
# create the scene
scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
point_mesh_node = scene.add(sensor_mesh)
direc_l_node = scene.add(direc_l, pose=cam_pose)
spot_l_node = scene.add(spot_l, pose=cam_pose)
cam_node = scene.add(cam, pose=cam_pose)
print('rendering the scene offline')
r = OffscreenRenderer(viewport_width=640, viewport_height=480)
color, depth = r.render(scene)
r.delete()
plt.figure()
plt.imshow(color)
plt.savefig(f'{render_sensor_path}/img_{t}.jpg')
def build_scene(floor_textures, wall_textures, fix_light_position=False):
scene = Scene(bg_color=np.array([153 / 255, 226 / 255, 249 / 255]),
ambient_light=np.array([0.5, 0.5, 0.5, 1.0]))
floor_trimesh = trimesh.load("{}/floor.obj".format(object_directory))
mesh = Mesh.from_trimesh(floor_trimesh, smooth=False)
node = Node(mesh=mesh,
rotation=pyrender.quaternion.from_pitch(-math.pi / 2),
translation=np.array([0, 0, 0]))
texture_path = random.choice(floor_textures)
set_random_texture(node, texture_path)
scene.add_node(node)
texture_path = random.choice(wall_textures)
wall_trimesh = trimesh.load("{}/wall.obj".format(object_directory))
mesh = Mesh.from_trimesh(wall_trimesh, smooth=False)
node = Node(mesh=mesh, translation=np.array([0, 1.15, -3.5]))
set_random_texture(node, texture_path)
scene.add_node(node)
mesh = Mesh.from_trimesh(wall_trimesh, smooth=False)
node = Node(mesh=mesh,
rotation=pyrender.quaternion.from_yaw(math.pi),
translation=np.array([0, 1.15, 3.5]))
set_random_texture(node, texture_path)
scene.add_node(node)
mesh = Mesh.from_trimesh(wall_trimesh, smooth=False)
node = Node(mesh=mesh,
rotation=pyrender.quaternion.from_yaw(-math.pi / 2),
translation=np.array([3.5, 1.15, 0]))
set_random_texture(node, texture_path)
scene.add_node(node)
mesh = Mesh.from_trimesh(wall_trimesh, smooth=False)
node = Node(mesh=mesh,
rotation=pyrender.quaternion.from_yaw(math.pi / 2),
translation=np.array([-3.5, 1.15, 0]))
set_random_texture(node, texture_path)
scene.add_node(node)
light = DirectionalLight(color=np.ones(3), intensity=10)
if fix_light_position == True:
translation = np.array([1, 1, 1])
else:
xz = np.random.uniform(-1, 1, size=2)
translation = np.array([xz[0], 1, xz[1]])
yaw, pitch = compute_yaw_and_pitch(translation)
node = Node(light=light,
rotation=genearte_camera_quaternion(yaw, pitch),
translation=translation)
scene.add_node(node)
return scene
def test_scenes():
# Basics
s = Scene()
assert np.allclose(s.bg_color, np.ones(4))
assert np.allclose(s.ambient_light, np.zeros(3))
assert len(s.nodes) == 0
assert s.name is None
s.name = 'asdf'
s.bg_color = None
s.ambient_light = None
assert np.allclose(s.bg_color, np.ones(4))
assert np.allclose(s.ambient_light, np.zeros(3))
assert s.nodes == set()
assert s.cameras == set()
assert s.lights == set()
assert s.point_lights == set()
assert s.spot_lights == set()
assert s.directional_lights == set()
assert s.meshes == set()
assert s.camera_nodes == set()
assert s.light_nodes == set()
assert s.point_light_nodes == set()
assert s.spot_light_nodes == set()
assert s.directional_light_nodes == set()
assert s.mesh_nodes == set()
assert s.main_camera_node is None
assert np.all(s.bounds == 0)
assert np.all(s.centroid == 0)
assert np.all(s.extents == 0)
assert np.all(s.scale == 0)
# From trimesh scene
tms = trimesh.load('tests/data/WaterBottle.glb')
s = Scene.from_trimesh_scene(tms)
assert len(s.meshes) == 1
assert len(s.mesh_nodes) == 1
# Test bg color formatting
s = Scene(bg_color=[0, 1.0, 0])
assert np.allclose(s.bg_color, np.array([0.0, 1.0, 0.0, 1.0]))
# Test constructor for nodes
n1 = Node()
n2 = Node()
n3 = Node()
nodes = [n1, n2, n3]
s = Scene(nodes=nodes)
n1.children.append(n2)
s = Scene(nodes=nodes)
n3.children.append(n2)
with pytest.raises(ValueError):
s = Scene(nodes=nodes)
n3.children = []
n2.children.append(n3)
n3.children.append(n2)
with pytest.raises(ValueError):
s = Scene(nodes=nodes)
# Test node accessors
n1 = Node()
n2 = Node()
n3 = Node()
nodes = [n1, n2]
s = Scene(nodes=nodes)
assert s.has_node(n1)
assert s.has_node(n2)
assert not s.has_node(n3)
# Test node poses
for n in nodes:
assert np.allclose(s.get_pose(n), np.eye(4))
with pytest.raises(ValueError):
s.get_pose(n3)
with pytest.raises(ValueError):
s.set_pose(n3, np.eye(4))
tf = np.eye(4)
tf[:3, 3] = np.ones(3)
s.set_pose(n1, tf)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), np.eye(4))
nodes = [n1, n2, n3]
tf2 = np.eye(4)
tf2[:3, :3] = np.diag([-1, -1, 1])
n1.children.append(n2)
n1.matrix = tf
n2.matrix = tf2
s = Scene(nodes=nodes)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), tf.dot(tf2))
assert np.allclose(s.get_pose(n3), np.eye(4))
n1 = Node()
n2 = Node()
n3 = Node()
n1.children.append(n2)
s = Scene()
s.add_node(n1)
with pytest.raises(ValueError):
s.add_node(n2)
s.set_pose(n1, tf)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), tf)
s.set_pose(n2, tf2)
assert np.allclose(s.get_pose(n2), tf.dot(tf2))
# Test node removal
n1 = Node()
n2 = Node()
n3 = Node()
n1.children.append(n2)
n2.children.append(n3)
s = Scene(nodes=[n1, n2, n3])
s.remove_node(n2)
assert len(s.nodes) == 1
assert n1 in s.nodes
assert len(n1.children) == 0
assert len(n2.children) == 1
s.add_node(n2, parent_node=n1)
assert len(n1.children) == 1
n1.matrix = tf
n3.matrix = tf2
assert np.allclose(s.get_pose(n3), tf.dot(tf2))
# Now test ADD function
s = Scene()
m = Mesh([], name='m')
cp = PerspectiveCamera(yfov=2.0)
co = OrthographicCamera(xmag=1.0, ymag=1.0)
dl = DirectionalLight()
pl = PointLight()
sl = SpotLight()
n1 = s.add(m, name='mn')
assert n1.mesh == m
assert len(s.nodes) == 1
assert len(s.mesh_nodes) == 1
assert n1 in s.mesh_nodes
assert len(s.meshes) == 1
assert m in s.meshes
assert len(s.get_nodes(node=n2)) == 0
n2 = s.add(m, pose=tf)
assert len(s.nodes) == len(s.mesh_nodes) == 2
assert len(s.meshes) == 1
assert len(s.get_nodes(node=n1)) == 1
assert len(s.get_nodes(node=n1, name='mn')) == 1
assert len(s.get_nodes(name='mn')) == 1
assert len(s.get_nodes(obj=m)) == 2
assert len(s.get_nodes(obj=m, obj_name='m')) == 2
assert len(s.get_nodes(obj=co)) == 0
nsl = s.add(sl, name='sln')
npl = s.add(pl, parent_name='sln')
assert nsl.children[0] == npl
ndl = s.add(dl, parent_node=npl)
assert npl.children[0] == ndl
nco = s.add(co)
ncp = s.add(cp)
assert len(s.light_nodes) == len(s.lights) == 3
assert len(s.point_light_nodes) == len(s.point_lights) == 1
assert npl in s.point_light_nodes
assert len(s.spot_light_nodes) == len(s.spot_lights) == 1
assert nsl in s.spot_light_nodes
assert len(s.directional_light_nodes) == len(s.directional_lights) == 1
assert ndl in s.directional_light_nodes
assert len(s.cameras) == len(s.camera_nodes) == 2
assert s.main_camera_node == nco
s.main_camera_node = ncp
s.remove_node(ncp)
assert len(s.cameras) == len(s.camera_nodes) == 1
assert s.main_camera_node == nco
s.remove_node(n2)
assert len(s.meshes) == 1
s.remove_node(n1)
assert len(s.meshes) == 0
s.remove_node(nsl)
assert len(s.lights) == 0
s.remove_node(nco)
assert s.main_camera_node is None
s.add_node(n1)
s.clear()
assert len(s.nodes) == 0
# Trigger final errors
with pytest.raises(ValueError):
s.main_camera_node = None
with pytest.raises(ValueError):
s.main_camera_node = ncp
with pytest.raises(ValueError):
s.add(m, parent_node=n1)
with pytest.raises(ValueError):
s.add(m, name='asdf')
s.add(m, name='asdf')
s.add(m, parent_name='asdf')
with pytest.raises(ValueError):
s.add(m, parent_name='asfd')
with pytest.raises(TypeError):
s.add(None)
s.clear()
# Test bounds
m1 = Mesh.from_trimesh(trimesh.creation.box())
m2 = Mesh.from_trimesh(trimesh.creation.box())
m3 = Mesh.from_trimesh(trimesh.creation.box())
n1 = Node(mesh=m1)
n2 = Node(mesh=m2, translation=[1.0, 0.0, 0.0])
n3 = Node(mesh=m3, translation=[0.5, 0.0, 1.0])
s.add_node(n1)
s.add_node(n2)
s.add_node(n3)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [1.5, 0.5, 1.5]])
s.clear()
s.add_node(n1)
s.add_node(n2, parent_node=n1)
s.add_node(n3, parent_node=n2)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [2.0, 0.5, 1.5]])
tf = np.eye(4)
tf[:3, 3] = np.ones(3)
s.set_pose(n3, tf)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [2.5, 1.5, 1.5]])
s.remove_node(n2)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [0.5, 0.5, 0.5]])
s.clear()
assert np.allclose(s.bounds, 0.0)
class DictionaryView():
"""Render-ready scene composed of a single object and a single moving camera.
# Arguments
filepath: String containing the path to an OBJ file.
viewport_size: List, specifying [H, W] of rendered image.
y_fov: Float indicating the vertical field of view in radians.
distance: List of floats indicating [max_distance, min_distance]
top_only: Boolean. If True images are only take from the top.
light: List of floats indicating [max_light, min_light]
"""
def __init__(self,
filepath,
viewport_size=(128, 128),
y_fov=3.14159 / 4.,
distance=0.30,
top_only=False,
light=5.0,
theta_steps=10,
phi_steps=10):
self.scene = Scene(bg_color=[0, 0, 0])
self.camera = self.scene.add(
PerspectiveCamera(y_fov, aspectRatio=np.divide(*viewport_size)))
self.mesh = self.scene.add(
Mesh.from_trimesh(trimesh.load(filepath), smooth=True))
self.world_origin = self.mesh.mesh.centroid
self.light = self.scene.add(DirectionalLight([1.0, 1.0, 1.0], light))
self.distance = distance
# 0.1 values are to avoid gimbal lock
theta_max = np.pi / 2.0 if top_only else np.pi
self.thetas = np.linspace(0.1, theta_max - 0.1, theta_steps)
self.phis = np.linspace(0.1, 2 * np.pi - 0.1, phi_steps)
self.renderer = OffscreenRenderer(*viewport_size)
self.RGBA = RenderFlags.RGBA
def render(self):
dictionary_data = []
for theta_arg, theta in enumerate(self.thetas):
for phi_arg, phi in enumerate(self.phis):
x = self.distance * np.sin(theta) * np.cos(phi)
y = self.distance * np.sin(theta) * np.sin(phi)
z = self.distance * np.cos(theta)
matrices = compute_modelview_matrices(np.array([x, z, y]),
self.world_origin)
camera_to_world, world_to_camera = matrices
self.scene.set_pose(self.camera, camera_to_world)
self.scene.set_pose(self.light, camera_to_world)
camera_to_world = camera_to_world.flatten()
world_to_camera = world_to_camera.flatten()
image, depth = self.renderer.render(self.scene,
flags=self.RGBA)
image, alpha = split_alpha_channel(image)
matrices = np.vstack([world_to_camera, camera_to_world])
sample = {
'image': image,
'alpha': alpha,
'depth': depth,
'matrices': matrices
}
dictionary_data.append(sample)
return dictionary_data
def remove_nodes_from_scene(self, scene: pyrender.Scene):
"""Remove nodes in scene corresponding to internal list of dice nodes."""
for mesh_node in self.dice_nodes:
if scene.has_node(mesh_node):
scene.remove_node(mesh_node)
class HVTPClient:
def __init__(self):
"""
"""
self.scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
self.scene.add(PointLight(color=[0.5, 0.2, 0.3], intensity=2.0))
self.scene.add(
SpotLight(color=[0.1, 0.6, 0.3],
intensity=2.0,
innerConeAngle=0.05,
outerConeAngle=0.5))
self.scene.add(
DirectionalLight(color=[0.33, 0.33, 0.33], intensity=2.0))
self.root_node = None
# self.scene.add_node(self.root_node)
def create(self, ip, port):
"""
"""
self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.client_socket.connect((ip, port))
self.client_socket.setblocking(True)
def destroy(self):
"""
"""
self.client_socket.shutdown(socket.SHUT_RDWR)
self.client_socket.close()
def run(self):
"""
"""
self.viewer = Viewer(self.scene,
use_raymond_lighting=False,
cull_faces=False,
run_in_thread=True)
threading.Thread(target=self.accept_messages).start()
def accept_messages(self):
"""
Polls our socket for changes that have been sent by the server.
"""
print("Starting message acceptor")
while True:
try:
packet_header = self.client_socket.recv(
HVTPConstants.HVTP_HEADER_SIZE_IN_BYTES)
print("Received!")
if len(packet_header) == 0:
print("Connection closed by server")
sys.exit()
# Read the header
i = 0
offs = 4
hvtp_magic = packet_header[(i *
offs):(offs + i * offs)].decode(
HVTPConstants.HVTP_ENCODING)
i += 1
hvtp_version = int.from_bytes(
packet_header[(i * offs):(offs + i * offs)],
byteorder='big',
signed=False)
i += 1
hvtp_length = int.from_bytes(
packet_header[(i * offs):(offs + i * offs)],
byteorder='big',
signed=False)
i += 1
hvtp_type = packet_header[(i * offs):(offs + i * offs)].decode(
HVTPConstants.HVTP_ENCODING)
print(hvtp_magic)
print(hvtp_version)
print(hvtp_length)
print(hvtp_type)
# Read the rest of the packet
payload = self.client_socket.recv(hvtp_length)
loaded_payload = trimesh.load(BytesIO(payload),
file_type='glb')
mesh = Mesh.from_trimesh(list(loaded_payload.dump()))
# Add to the scene
self.add_to_scene(mesh)
except IOError as e:
# This is normal on non blocking connections - when there are no incoming data error is going to be raised
# Some operating systems will indicate that using AGAIN, and some using WOULDBLOCK error code
# We are going to check for both - if one of them - that's expected, means no incoming data, continue as normal
# If we got different error code - something happened
if e.errno != errno.EAGAIN and e.errno != errno.EWOULDBLOCK:
print('Reading error: {}'.format(str(e)))
sys.exit()
except Exception as e:
# Any other exception - something happened, exit
print('Reading error: {}'.format(str(e)))
sys.exit()
def add_to_scene(self, mesh):
"""
"""
# Grab the viewer mutex
self.viewer.render_lock.acquire()
# Remove anything before and insert the mesh into the scene-graph
if self.root_node != None:
self.scene.remove_node(self.root_node)
self.root_node = self.scene.add(mesh)
# Release the viewer mutex
self.viewer.render_lock.release()
# ==============================================================================
# Camera creation
# ==============================================================================
cam = PerspectiveCamera(yfov=(np.pi / 3.0))
cam_pose = np.array([[0.0, -np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.5], [1.0, 0.0, 0.0, 0.0],
[0.0, np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.4], [0.0, 0.0, 0.0, 1.0]])
# ==============================================================================
# Scene creation
# ==============================================================================
scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
# ==============================================================================
# Adding objects to the scene
# ==============================================================================
# ------------------------------------------------------------------------------
# By manually creating nodes
# ------------------------------------------------------------------------------
fuze_node = Node(mesh=fuze_mesh,
translation=np.array(
[0.1, 0.15, -np.min(fuze_trimesh.vertices[:, 2])]))
scene.add_node(fuze_node)
boxv_node = Node(mesh=boxv_mesh, translation=np.array([-0.1, 0.10, 0.05]))
scene.add_node(boxv_node)
boxf_node = Node(mesh=boxf_mesh, translation=np.array([-0.1, -0.10, 0.05]))
point_l_N = spatial_transform_Matrix(roll=np.pi/12, t_y=1/((side_closeness)+0.5),t_z=side_height+1/8)
point_l_W = spatial_transform_Matrix(roll=np.pi/12, yaw=-np.pi/2, t_x=1/((side_closeness)+0.5),t_z=side_height+1/8)
point_l_S = spatial_transform_Matrix(roll=np.pi/12, yaw=np.pi, t_y=-1/((side_closeness)+0.5),t_z=side_height+1/8)
wall_light_poses = [point_l_E, point_l_N, point_l_W, point_l_S]
# Pose for light on flower
light_pose, _ = lookAt(view=CameraPose.TOP, distance=CameraDistance.MEDIUM)
# Flower pose
flower_pose = get_flower_pose(flower_type, file_name)
#==============================================================================
# Scene creation
#==============================================================================
scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
#==============================================================================
# Adding objects to the scene
#==============================================================================
# Flower
flower_mesh = mesh_list.pop(index_1)
flower_node = scene.add(flower_mesh, pose=flower_pose, name='flower')
# Center (aka stigma)
if flower_type in no_center_flowers:
center_mesh = flower_mesh
else:
center_mesh = mesh_list.pop(index_2)
center_node = scene.add(center_mesh, pose=flower_pose, name='center')
class Visualizer3D(object):
"""
Class with static PyOpenGL-based 3D visualization tools.
Should be thought of as a namespace rather than a class.
"""
_scene = Scene(bg_color=np.ones(3))
_size = np.array([640, 480])
_kwargs = {}
_save_directory = None
CENTER = TextAlign.CENTER
CENTER_LEFT = TextAlign.CENTER_LEFT
CENTER_RIGHT = TextAlign.CENTER_RIGHT
BOTTOM_LEFT = TextAlign.BOTTOM_LEFT
BOTTOM_RIGHT = TextAlign.BOTTOM_RIGHT
BOTTOM_CENTER = TextAlign.BOTTOM_CENTER
TOP_LEFT = TextAlign.TOP_LEFT
TOP_RIGHT = TextAlign.TOP_RIGHT
TOP_CENTER = TextAlign.TOP_CENTER
@staticmethod
def figure(bg_color=(1.0, 1.0, 1.0), size=(1000, 1000)):
"""Create a viewing window.
Parameters
----------
bg_color : (3,) float
The background RGB color.
size : (2,) int
The width and height of the window in pixels.
"""
Visualizer3D._scene = Scene(bg_color=np.array(bg_color))
Visualizer3D._size = size
@staticmethod
def show(asynch=False, animate=False, **kwargs):
"""Display the current figure and enable interaction.
Parameters
----------
asynch : bool
Whether to run Viewer in separate thread
animate : bool
Whether or not to animate the scene.
kwargs : dict
Other keyword arguments for the Viewer instance.
"""
Visualizer3D._kwargs.update({
'rotate': animate,
'run_in_thread': asynch
})
Visualizer3D._kwargs.update(kwargs)
viewer = Visualizer3D._run(Visualizer3D._kwargs)
@staticmethod
def render(n_frames=1, **kwargs):
"""Render frames from the viewer.
Parameters
----------
n_frames : int
Number of frames to render. If more than one, the scene will animate.
kwargs : dict
Other keyword arguments for the Viewer instance.
Returns
-------
list of perception.ColorImage
A list of ColorImages rendered from the viewer.
"""
Visualizer3D._kwargs.update({
'run_in_thread': True,
'record': True,
'rotate': (n_frames > 1)
})
Visualizer3D._kwargs.update(kwargs)
viewer = Visualizer3D._run(Visualizer3D._kwargs)
while len(viewer._saved_frames) < n_frames:
pass
viewer.close_external()
if n_frames > 1:
return viewer._saved_frames
else:
return [viewer._saved_frames[0]]
@staticmethod
def save(filename, n_frames=1, **kwargs):
"""Save frames from the viewer out to a file.
Parameters
----------
filename : str
The filename in which to save the output image. If more than one frame,
should have extension .gif.
n_frames : int
Number of frames to render. If more than one, the scene will animate.
kwargs : dict
Other keyword arguments for the SceneViewer instance.
"""
frames = Visualizer3D.render(n_frames=n_frames, **kwargs)
framerate = kwargs['refresh_rate'] if 'refresh_rate' in kwargs else 30.0
if n_frames > 1:
imageio.mimwrite(filename,
frames,
fps=framerate,
palettesize=128,
subrectangles=True)
else:
imageio.imwrite(filename, frames[0])
@staticmethod
def _run(kwargs):
"""Internal method that runs the viewer
Parameters
----------
kwargs : dict
Other keyword arguments for the Viewer instance.
"""
if 'use_raymond_lighting' not in kwargs:
kwargs['use_raymond_lighting'] = True
viewer = Viewer(Visualizer3D._scene,
viewport_size=Visualizer3D._size,
**kwargs)
if viewer.viewer_flags['save_directory']:
Visualizer3D._save_directory = viewer.viewer_flags[
'save_directory']
Visualizer3D._kwargs = {}
return viewer
@staticmethod
def save_loop(filename, framerate=30.0, time=3.0, **kwargs):
"""Off-screen save a GIF of one rotation about the scene.
Parameters
----------
filename : str
The filename in which to save the output image (should have extension .gif)
framerate : int
The frame rate at which to animate motion.
time : float
The number of seconds for one rotation.
kwargs : dict
Other keyword arguments for the Viewer instance.
"""
n_frames = int(framerate * time)
Visualizer3D.save(filename,
n_frames=n_frames,
rotate_rate=(2.0 * np.pi / time),
refresh_rate=framerate,
**kwargs)
@staticmethod
def get_object_keys():
"""Return the visualizer's object keys.
Returns
-------
list of str
The keys for the visualizer's objects.
"""
return [n.name for n in Visualizer3D._scene.mesh_nodes]
@staticmethod
def get_object(name):
"""Return a Node corresponding to the given name.
Returns
-------
pyrender.Node
The corresponding Node.
"""
return next(iter(Visualizer3D._scene.get_nodes(name=name)))
@staticmethod
def points(points,
name=None,
T_points_world=None,
color=None,
material=None,
n_cuts=20,
scale=0.01):
"""Scatter a point cloud in pose T_points_world.
Parameters
----------
points : (n,3) float
The point set to visualize.
name : str
A name for the object to be added.
T_points_world : autolab_core.RigidTransform
Pose of points, specified as a transformation from point frame to world frame.
color : (3,) or (n,3) float
Color of whole cloud or per-point colors
material:
Material of mesh
n_cuts : int
Number of longitude/latitude lines on sphere points.
scale : float
Radius of each point.
"""
n = Visualizer3D._create_node_from_points(points,
name=name,
tube_radius=scale,
pose=T_points_world,
color=color,
material=material,
n_divs=n_cuts)
Visualizer3D._scene.add_node(n)
return n
@staticmethod
def mesh(mesh,
name=None,
T_mesh_world=None,
style='surface',
color=(0.5, 0.5, 0.5),
material=None,
smooth=False):
"""Visualize a 3D triangular mesh.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
name : str
A name for the object to be added.
T_mesh_world : autolab_core.RigidTransform
The pose of the mesh, specified as a transformation from mesh frame to world frame.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
color : 3-tuple
Color tuple.
material:
Material of mesh
smooth : bool
If true, the mesh is smoothed before rendering.
"""
if not isinstance(mesh, trimesh.Trimesh):
raise ValueError('Must provide a trimesh.Trimesh object')
n = Visualizer3D._create_node_from_mesh(
mesh,
name=name,
pose=T_mesh_world,
color=color,
material=material,
poses=None,
wireframe=(style == 'wireframe'),
smooth=smooth)
Visualizer3D._scene.add_node(n)
return n
@staticmethod
def mesh_stable_pose(mesh,
T_obj_table,
T_table_world=RigidTransform(from_frame='table',
to_frame='world'),
style='surface',
smooth=False,
color=(0.5, 0.5, 0.5),
material=None,
dim=0.15,
plot_table=True,
plot_com=False,
name=None):
"""Visualize a mesh in a stable pose.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
T_obj_table : autolab_core.RigidTransform
Pose of object relative to table.
T_table_world : autolab_core.RigidTransform
Pose of table relative to world.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
smooth : bool
If true, the mesh is smoothed before rendering.
color : 3-tuple
Color tuple.
material:
Material of mesh
dim : float
The side-length for the table.
plot_table : bool
If true, a table is visualized as well.
plot_com : bool
If true, a ball is visualized at the object's center of mass.
name : str
A name for the object to be added.
Returns
-------
autolab_core.RigidTransform
The pose of the mesh in world frame.
"""
T_obj_table = T_obj_table.as_frames('obj', 'table')
T_obj_world = T_table_world * T_obj_table
Visualizer3D.mesh(mesh,
T_mesh_world=T_obj_world,
style=style,
smooth=smooth,
color=color,
material=material,
name=name)
if plot_table:
Visualizer3D.table(T_table_world, dim=dim)
if plot_com:
Visualizer3D.points(Point(np.array(mesh.center_mass), 'obj'),
T_points_world=T_obj_world,
scale=0.01)
return T_obj_world
@staticmethod
def plot3d(points,
tube_radius=None,
name=None,
pose=None,
color=(0.5, 0.5, 0.5),
material=None,
n_components=30,
smooth=True):
"""Plot a 3d curve through a set of points using tubes.
Parameters
----------
points : (n,3) float
A series of 3D points that define a curve in space.
tube_radius : float
Radius of tube representing curve.
name : str
A name for the object to be added.
pose : autolab_core.RigidTransform
Pose of object relative to world.
color : (3,) float
The color of the tube.
material:
Material of mesh
n_components : int
The number of edges in each polygon representing the tube.
smooth : bool
If true, the mesh is smoothed before rendering.
"""
# Generate circular polygon
vec = np.array([0.0, 1.0]) * tube_radius
angle = 2 * np.pi / n_components
rotmat = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
perim = []
for _ in range(n_components):
perim.append(vec)
vec = np.dot(rotmat, vec)
poly = Polygon(perim)
# Sweep it along the path
mesh = trimesh.creation.sweep_polygon(poly, points)
return Visualizer3D.mesh(mesh,
name=name,
T_mesh_world=pose,
color=color,
material=material,
smooth=smooth)
@staticmethod
def arrow(start_point,
direction,
tube_radius=0.005,
color=(0.5, 0.5, 0.5),
material=None,
n_components=30,
smooth=True):
"""Plot an arrow with start and end points.
Parameters
----------
start_point : (3,) float
Origin point for the arrow
direction : (3,) float
Vector defining the arrow
tube_radius : float
Radius of plotted x,y,z axes.
color : (3,) float
The color of the tube.
material:
Material of mesh
n_components : int
The number of edges in each polygon representing the tube.
smooth : bool
If true, the mesh is smoothed before rendering.
"""
end_point = start_point + direction
arrow_head = Visualizer3D._create_arrow_head(
length=np.linalg.norm(direction), tube_radius=tube_radius)
arrow_head_rot = trimesh.geometry.align_vectors(
np.array([0, 0, 1]), direction)
arrow_head_tf = np.matmul(
trimesh.transformations.translation_matrix(end_point),
arrow_head_rot)
vec = np.array([start_point, end_point])
Visualizer3D.plot3d(vec, tube_radius=tube_radius, color=color)
Visualizer3D.mesh(arrow_head,
T_mesh_world=arrow_head_tf,
color=color,
material=material,
smooth=smooth)
@staticmethod
def pose(T_frame_world=None,
length=0.1,
tube_radius=0.005,
center_scale=0.01):
"""Plot a 3D pose as a set of axes (x red, y green, z blue).
Parameters
----------
T_frame_world : autolab_core.RigidTransform
The pose relative to world coordinates.
length : float
Length of plotted x,y,z axes.
tube_radius : float
Radius of plotted x,y,z axes.
center_scale : float
Radius of the pose's origin ball.
"""
if T_frame_world is None:
R = np.eye(3)
t = np.zeros(3)
else:
R = T_frame_world.rotation
t = T_frame_world.translation
Visualizer3D.points(t, color=(1, 1, 1), scale=center_scale)
Visualizer3D.arrow(t,
length * R[:, 0],
tube_radius=tube_radius,
color=(1, 0, 0))
Visualizer3D.arrow(t,
length * R[:, 1],
tube_radius=tube_radius,
color=(0, 1, 0))
Visualizer3D.arrow(t,
length * R[:, 2],
tube_radius=tube_radius,
color=(0, 0, 1))
@staticmethod
def table(
T_table_world=RigidTransform(from_frame='table', to_frame='world'),
dim=0.16,
color=(0.3, 0.3, 0.3)):
"""Plot a table mesh in 3D.
Parameters
----------
T_table_world : autolab_core.RigidTransform
Pose of table relative to world.
dim : float
The side-length for the table.
color : 3-tuple
Color tuple.
"""
table_mesh = trimesh.creation.box(extents=(dim, dim, dim / 10))
table_mesh.apply_translation(-np.array([0, 0, dim / 20]))
table_mesh.apply_transform(T_table_world.matrix)
Visualizer3D.mesh(table_mesh,
style='surface',
smooth=True,
color=color)
@staticmethod
def caption(text,
location=TextAlign.TOP_RIGHT,
font_name='OpenSans-Regular',
font_pt=20,
color=None,
scale=1.0):
"""Displays text on the visualization.
Parameters
----------
text : str
The text to be displayed
location : int
Enum of location for the text
font_name : str
Valid font to be used
font_pt : int
Size of font to be used
color : 3-tuple
Color tuple.
scale : float
Scale of text
"""
caption_dict = {
'text': text,
'location': location,
'font_name': font_name,
'font_pt': font_pt,
'color': color,
'scale': scale
}
if 'caption' in Visualizer3D._kwargs:
Visualizer3D._kwargs['caption'].append(caption_dict)
else:
Visualizer3D._kwargs['caption'] = [caption_dict]
@staticmethod
def _create_node_from_mesh(mesh,
name=None,
pose=None,
color=None,
material=None,
poses=None,
wireframe=False,
smooth=True):
"""Helper method that creates a pyrender.Node from a trimesh.Trimesh"""
# Create default pose
if pose is None:
pose = np.eye(4)
elif isinstance(pose, RigidTransform):
pose = pose.matrix
# Create vertex colors if needed
if color is not None:
color = np.asanyarray(color, dtype=np.float)
if color.ndim == 1 or len(color) != len(mesh.vertices):
color = np.repeat(color[np.newaxis, :],
len(mesh.vertices),
axis=0)
mesh.visual.vertex_colors = color
if material is None and mesh.visual.kind != 'texture':
if color is not None:
material = None
else:
material = MetallicRoughnessMaterial(baseColorFactor=np.array(
[1.0, 1.0, 1.0, 1.0]),
metallicFactor=0.2,
roughnessFactor=0.8)
m = Mesh.from_trimesh(mesh,
material=material,
poses=poses,
wireframe=wireframe,
smooth=smooth)
return Node(mesh=m, name=name, matrix=pose)
@staticmethod
def _create_node_from_points(points,
name=None,
pose=None,
color=None,
material=None,
tube_radius=None,
n_divs=20):
"""Helper method that creates a pyrender.Node from an array of points"""
points = np.asanyarray(points)
if points.ndim == 1:
points = np.array([points])
if pose is None:
pose = np.eye(4)
else:
pose = pose.matrix
# Create vertex colors if needed
if color is not None:
color = np.asanyarray(color, dtype=np.float)
if color.ndim == 1 or len(color) != len(points):
color = np.repeat(color[np.newaxis, :], len(points), axis=0)
if tube_radius is not None:
poses = None
mesh = trimesh.creation.uv_sphere(tube_radius, [n_divs, n_divs])
if color is not None:
mesh.visual.vertex_colors = color[0]
poses = np.tile(np.eye(4),
(len(points), 1)).reshape(len(points), 4, 4)
poses[:, :3, 3::4] = points[:, :, None]
m = Mesh.from_trimesh(mesh, material=material, poses=poses)
else:
m = Mesh.from_points(points, colors=color)
return Node(mesh=m, name=name, matrix=pose)
@staticmethod
def _create_arrow_head(length=0.1, tube_radius=0.005, n_components=30):
radius = tube_radius * 1.5
height = length * 0.1
# create a 2D pie out of wedges
theta = np.linspace(0, np.pi * 2, n_components)
vertices = np.column_stack(
(np.sin(theta), np.cos(theta), np.zeros(len(theta)))) * radius
# the single vertex at the center of the circle
# we're overwriting the duplicated start/end vertex
# plus add vertex at tip of cone
vertices[0] = [0, 0, 0]
vertices = np.append(vertices, [[0, 0, height]], axis=0)
# whangle indexes into a triangulation of the pie wedges
index = np.arange(1, len(vertices)).reshape((-1, 1))
index[-1] = 1
faces_2d = np.tile(index, (1, 2)).reshape(-1)[1:-1].reshape((-1, 2))
faces = np.column_stack((np.zeros(len(faces_2d),
dtype=np.int), faces_2d))
# add triangles connecting to vertex above
faces = np.append(
faces,
np.column_stack(
((len(faces_2d) + 1) * np.ones(len(faces_2d), dtype=np.int),
faces_2d))[:, ::-1],
axis=0)
arrow_head = trimesh.Trimesh(faces=faces, vertices=vertices)
return arrow_head
def add_loaded_to_scene(self, scene: pyrender.Scene):
"""Add to scene all dice nodes in loaded internal list."""
for node in self.dice_nodes:
scene.add_node(node)
from pyrender import Mesh, Scene, Viewer
from io import BytesIO
import numpy as np
import trimesh
import requests
import time
duck_source = "https://github.com/KhronosGroup/glTF-Sample-Models/raw/master/2.0/Duck/glTF-Binary/Duck.glb"
duck = trimesh.load(BytesIO(requests.get(duck_source).content),
file_type='glb')
duckmesh = Mesh.from_trimesh(list(duck.geometry.values())[0])
scene = Scene(ambient_light=np.array([1.0, 1.0, 1.0, 1.0]))
scene.add(duckmesh)
orig_positions = duckmesh.primitives[0].positions
vwr = Viewer(scene,
run_in_thread=True,
render_flags={
"cull_faces": False,
"all_wireframe": False
},
use_raymond_lighting=True,
bg_color=[255, 0, 255])
while vwr.is_active:
with vwr.render_lock:
duckmesh.primitives[0].positions = orig_positions + np.random.randn(
*orig_positions.shape) * 2
time.sleep(0.1)
aspectRatio=1.0 * 640 / 480,
znear=0.1,
zfar=6)
cam_pose = np.array([[0.0, -np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.5], [1.0, 0.0, 0.0, 0.0],
[0.0, np.sqrt(2) / 2,
np.sqrt(2) / 2, 0.4], [0.0, 0.0, 0.0, 1.0]])
proj = cam.get_projection_matrix()
mvp = proj @ np.linalg.inv(cam_pose)
inv_mvp = np.linalg.inv(mvp)
#==============================================================================
# Scene creation
#==============================================================================
scene = Scene(ambient_light=np.array([0.02, 0.02, 0.02, 1.0]))
cam_node = scene.add(cam, pose=cam_pose)
scene.main_camera_node = cam_node
#==============================================================================
drill_node = scene.add(drill_mesh)
r = OffscreenRenderer(viewport_width=640, viewport_height=480)
rf = pyrender.RenderFlags.NONE
rf |= pyrender.RenderFlags.USE_RAW_DEPTH
color, raw_depth = r.render(scene, flags=rf)
r.delete()
# unproject to get point cloud
pcd = unproj(inv_mvp, raw_depth)
