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 |