29 lines
910 B
Python
29 lines
910 B
Python
|
# ===== Open3D helpers =====
|
|||
|
|
import numpy as np
|
|||
|
|
import open3d as o3d
|
|||
|
|
|
|||
|
|
def o3d_sphere(center, radius=0.015, color=(1.0, 0.0, 0.0)):
|
|||
|
|
sp = o3d.geometry.TriangleMesh.create_sphere(radius=radius)
|
|||
|
|
sp.compute_vertex_normals()
|
|||
|
|
sp.paint_uniform_color(color)
|
|||
|
|
T = np.eye(4)
|
|||
|
|
T[:3, 3] = center
|
|||
|
|
sp.transform(T)
|
|||
|
|
return sp
|
|||
|
|
|
|||
|
|
def o3d_lineset(points, color=(1.0, 0.0, 0.0)):
|
|||
|
|
# points: shape (N,3) — we’ll connect 0-1, 1-2, 2-3
|
|||
|
|
pts = o3d.utility.Vector3dVector(points)
|
|||
|
|
lines = o3d.utility.Vector2iVector([[0,1],[1,2],[2,3]])
|
|||
|
|
ls = o3d.geometry.LineSet(points=pts, lines=lines)
|
|||
|
|
cols = [color, color, color]
|
|||
|
|
ls.colors = o3d.utility.Vector3dVector(cols)
|
|||
|
|
return ls
|
|||
|
|
|
|||
|
|
def o3d_frame(R=np.eye(3), t=np.zeros(3), size=0.08):
|
|||
|
|
fr = o3d.geometry.TriangleMesh.create_coordinate_frame(size=size)
|
|||
|
|
T = np.eye(4)
|
|||
|
|
T[:3,:3] = R
|
|||
|
|
T[:3, 3] = t
|
|||
|
|
fr.transform(T)
|
|||
|
|
return fr
|