def meshcat_draw_frustrum(vis, TF, K, near_distance, far_distance, w, h):
# TODO(gizatt): This obviously isn't right -- the projected
# light doesn't match the drawn view frustrum.
# Not dealing with, for now; I think the issue is a combination
# of bad intrinsics and bugs related to flipped image coordinates
# somewhere along the pipeline.
image_bbox_verts = np.array([
[0., w, w, 0.],
[0., 0., h, h]
])
TF_inv = np.eye(4)
TF_inv[:3, :3] = TF[:3, :3].T
TF_inv[:3, 3] = -TF_inv[:3, :3].dot(TF[:3, 3])
TF = TF_inv
N = image_bbox_verts.shape[1]
Kinv = np.linalg.inv(K)
def project_bbox_verts(dist):
homog = np.concatenate(
[image_bbox_verts*dist, dist*np.ones((1, N))],
axis=0
)
pts = np.dot(Kinv, homog)
return ((TF[:3, :3].dot(pts)).T + TF[:3, 3]).T
near_pts = project_bbox_verts(near_distance)
far_pts= project_bbox_verts(far_distance)
near_colors = np.zeros((3, N))
near_colors[1, :] = 1.
far_colors = np.zeros((3, N))
far_colors[2, :] = 1.
vis['frustrum']['near'].set_object(g.LineLoop(
g.PointsGeometry(near_pts, color=near_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=0.1)))
vis['frustrum']['far'].set_object(g.LineLoop(
g.PointsGeometry(far_pts, color=far_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=0.1)))
connecting = np.zeros((3, N*2))
connecting[:, ::2] = near_pts
connecting[:, 1::2] = far_pts
connecting_colors = np.zeros((3, N*2))
connecting_colors[:, ::2] = near_colors
connecting_colors[:, 1::2] = far_colors
vis['frustrum']['connecting'].set_object(g.LineSegments(
g.PointsGeometry(connecting, color=connecting_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=1.)
))
# Draw a little box for the projector :)
vis['projector'].set_object(
g.Box([0.1, 0.1, 0.1]),
g.MeshLambertMaterial(
color=0xaaffaa))
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["box"].set_object(g.Box([1.0, 0.2, 0.3]))
v["box"].delete()
v["box"].set_object(g.Box([0.1, 0.2, 0.3]))
v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v["transparent_ellipsoid"].set_object(g.Mesh(
g.Ellipsoid([0.3, 0.1, 0.1]),
g.MeshLambertMaterial(color=0xffffff,
opacity=0.5)))
v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["meshes/convex"]
v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj"))))
v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii"))))
v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0]))
v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary"))))
v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0]))
v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae"))))
v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([0, 2, 0]))
verts = np.random.rand(3, 1000000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
v = self.vis["lines"]
v.set_transform(tf.translation_matrix(([-2, -3, 0])))
vertices = np.random.random((3, 10)).astype(np.float32)
v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices)))
v["line"].set_object(g.Line(g.PointsGeometry(vertices)))
v["line"].set_transform(tf.translation_matrix([0, 1, 0]))
v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices)))
v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0]))
v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000)))
v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0]))
colors = vertices # Color each line by treating its xyz coordinates as RGB colors
v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True)))
v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0]))
v["triad"].set_object(g.LineSegments(
g.PointsGeometry(position=np.array([
[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T,
color=np.array([
[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
),
g.LineBasicMaterial(vertexColors=True)))
v["triad"].set_transform(tf.translation_matrix(([0, 5, 0])))
v["triad_function"].set_object(g.triad(0.5))
v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))