def approximate_mesh_box(dimensions):
width, height, depth = dimensions
length = depth
collection = GeometryCollection()
radius = min(width, height) / 2.0
fill_distance = max(width, height) / 2.0
cylinder_count = int(math.ceil(fill_distance / radius))
cylinder_spacing = fill_distance / cylinder_count
spacing_axis = 0 if width > height else 1
# Add an initial cylinder right in the middle
collection.add_geometry((0, 0, 0),
create_mesh_cylinder(height=length, radius=radius))
# Add extra cylinders, filling upwards
for i in range(1, cylinder_count):
collection.add_geometry(
make_offset_position(spacing_axis, i * cylinder_spacing),
create_mesh_cylinder(height=length, radius=radius))
# Add extra cylinders, filling downwards
for i in range(1, cylinder_count):
collection.add_geometry(
make_offset_position(spacing_axis, -i * cylinder_spacing),
create_mesh_cylinder(height=length, radius=radius))
return collection
def create_plane(center, normal, plane_size, axis_size=0.03):
mesh = o3d.TriangleMesh()
quat = np.r_[normal, 0]
e = np.array([0, 1, 0])
cross_vec_1 = np.cross(normal, e)
if (cross_vec_1[0] < 0):
cross_vec_1 *= -1
cross_vec_1 /= np.linalg.norm(cross_vec_1)
cross_vec_2 = np.cross(normal, cross_vec_1)
if (cross_vec_2[2] < 0):
cross_vec_2 *= -1
cross_vec_2 /= np.linalg.norm(cross_vec_2)
vec_set_basis = np.array([[0, 1, 0], [1, 0, 0], [0, 0, 1]])
vec_set_normal = np.array([normal, cross_vec_1, cross_vec_2])
rr = Rot.match_vectors(vec_set_basis, vec_set_normal)[0]
transform_mat = np.eye(4)
transform_mat[:3, :3] = rr.as_dcm().T
transform_mat[:3, 3] = center
p1 = np.array([-plane_size * 0.5, 0, -plane_size * 0.5])
p2 = np.array([plane_size * 0.5, 0, -plane_size * 0.5])
p3 = np.array([plane_size * 0.5, 0, plane_size * 0.5])
p4 = np.array([-plane_size * 0.5, 0, plane_size * 0.5])
_mesh_points = np.array([p1, p2, p3, p4], dtype=np.float)
mesh.vertices = o3d.Vector3dVector(_mesh_points)
_mesh_triangles = np.array([[0, 2, 1], [2, 0, 3]])
_mesh_triangles = np.concatenate(
[_mesh_triangles, _mesh_triangles[:, ::-1]], axis=0)
mesh.triangles = o3d.Vector3iVector(_mesh_triangles)
mesh.paint_uniform_color([1, 0.706, 0])
mesh.compute_vertex_normals()
mesh.transform(transform_mat)
mesh_cylinder = o3d.create_mesh_cylinder(radius=axis_size / 50,
height=axis_size)
mesh_cylinder.paint_uniform_color([0, 1.0, 0])
transform_yaw = np.array([[1, 0, 0, 0], [0, 0, 1, axis_size / 2],
[0, 1, 0, 0], [0, 0, 0, 1]])
mesh_cylinder.transform(transform_yaw)
mesh_cylinder.transform(transform_mat)
return mesh, mesh_cylinder
def addCylinder(self, start, end, rotate=True, color=[0.9, 0.0, 0.3]):
length = np.linalg.norm(start - end)
n = (end - start) / length
phi = np.arccos(n[2])
theta = np.arctan2(n[1], n[0])
theta_quat = Quaternion(axis=[0, 0, 1], angle=theta)
vprime = theta_quat.rotate([0, 1., 0.])
phi_quat = Quaternion(axis=vprime, angle=phi)
rot = phi_quat.rotation_matrix
cyl = o3d.create_mesh_cylinder(0.05, length)
if rotate:
cyl = cyl.transform(pose(np.array((start + end) / 2.0), rot))
# .transform(pose(center))
cyl.paint_uniform_color(color)
cyl.compute_vertex_normals()
self.showObjects.append(cyl)
def draw_open3D_box(self, label, expend_factor=(1.0, 1.0, 1.0)):
"""Draw a 3d box using open3d.
Args:
label: a dictionary containing "x3d", "y3d", "z3d", "yaw",
"height", "width", "lenth".
returns: a open3d mesh object.
"""
yaw = label['yaw']
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0],
[-np.sin(yaw), 0, np.cos(yaw)]])
Rh = np.array([[1, 0, 0], [0, 0, 1], [0, 1, 0]])
Rl = np.array([[0, 0, 1], [0, 1, 0], [1, 0, 0]])
h = label['height']
delta_h = h * (expend_factor[0] - 1)
w = label['width'] * expend_factor[1]
l = label['length'] * expend_factor[2]
print((l, w, h))
tx = label['x3d']
ty = label['y3d']
tz = label['z3d']
box_offset = np.array([[l / 2, -h / 2 - delta_h / 2, w / 2],
[l / 2, -h / 2 - delta_h / 2, -w / 2],
[-l / 2, -h / 2 - delta_h / 2, -w / 2],
[-l / 2, -h / 2 - delta_h / 2, w / 2],
[l / 2, delta_h / 2,
0], [-l / 2, delta_h / 2, 0],
[l / 2, -h - delta_h / 2, 0],
[-l / 2, -h - delta_h / 2, 0],
[0, delta_h / 2, w / 2],
[0, delta_h / 2, -w / 2],
[0, -h - delta_h / 2, w / 2],
[0, -h - delta_h / 2, -w / 2]])
transform = np.matmul(R, np.transpose(box_offset))
transform = transform + np.array([[tx], [ty], [tz]])
transform = np.vstack((transform, np.ones((1, 12))))
hrotation = np.vstack((R.dot(Rh), np.zeros((1, 3))))
lrotation = np.vstack((R.dot(Rl), np.zeros((1, 3))))
wrotation = np.vstack((R, np.zeros((1, 3))))
h1_cylinder = open3d.create_mesh_cylinder(radius=h / 100, height=h)
h1_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
h1_cylinder.transform(np.hstack((hrotation, transform[:, [0]])))
h2_cylinder = open3d.create_mesh_cylinder(radius=h / 100, height=h)
h2_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
h2_cylinder.transform(np.hstack((hrotation, transform[:, [1]])))
h3_cylinder = open3d.create_mesh_cylinder(radius=h / 100, height=h)
h3_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
h3_cylinder.transform(np.hstack((hrotation, transform[:, [2]])))
h4_cylinder = open3d.create_mesh_cylinder(radius=h / 100, height=h)
h4_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
h4_cylinder.transform(np.hstack((hrotation, transform[:, [3]])))
w1_cylinder = open3d.create_mesh_cylinder(radius=w / 100, height=w)
w1_cylinder.paint_uniform_color([0.9, 0.1, 0.1])
w1_cylinder.transform(np.hstack((wrotation, transform[:, [4]])))
w2_cylinder = open3d.create_mesh_cylinder(radius=w / 100, height=w)
w2_cylinder.paint_uniform_color([0.9, 0.1, 0.1])
w2_cylinder.transform(np.hstack((wrotation, transform[:, [5]])))
w3_cylinder = open3d.create_mesh_cylinder(radius=w / 100, height=w)
w3_cylinder.paint_uniform_color([0.9, 0.1, 0.1])
w3_cylinder.transform(np.hstack((wrotation, transform[:, [6]])))
w4_cylinder = open3d.create_mesh_cylinder(radius=w / 100, height=w)
w4_cylinder.paint_uniform_color([0.9, 0.1, 0.1])
w4_cylinder.transform(np.hstack((wrotation, transform[:, [7]])))
l1_cylinder = open3d.create_mesh_cylinder(radius=l / 100, height=l)
l1_cylinder.paint_uniform_color([0.1, 0.1, 0.9])
l1_cylinder.transform(np.hstack((lrotation, transform[:, [8]])))
l2_cylinder = open3d.create_mesh_cylinder(radius=l / 100, height=l)
l2_cylinder.paint_uniform_color([0.1, 0.1, 0.9])
l2_cylinder.transform(np.hstack((lrotation, transform[:, [9]])))
l3_cylinder = open3d.create_mesh_cylinder(radius=l / 100, height=l)
l3_cylinder.paint_uniform_color([0.1, 0.1, 0.9])
l3_cylinder.transform(np.hstack((lrotation, transform[:, [10]])))
l4_cylinder = open3d.create_mesh_cylinder(radius=l / 100, height=l)
l4_cylinder.paint_uniform_color([0.1, 0.1, 0.9])
l4_cylinder.transform(np.hstack((lrotation, transform[:, [11]])))
return [
h1_cylinder, h2_cylinder, h3_cylinder, h4_cylinder, w1_cylinder,
w2_cylinder, w3_cylinder, w4_cylinder, l1_cylinder, l2_cylinder,
l3_cylinder, l4_cylinder
]
def create3Dbbox(center,h,w,l,r_y,type_list,color_dicts): ## For Point Cloud, IN Open3d, to draw a cubic needs {8 Points, 12 Lines, Colors}
front_color = color_dicts[type_list]
R_Matrix_y = np.asarray([[math.cos(r_y),0,math.sin(r_y)],[0,1,0],[-math.sin(r_y),0,math.cos(r_y)]],dtype= np.float64)
R_Matrix_y_rect = np.asarray([[math.cos(r_y + np.pi/2),0,math.sin(r_y + np.pi/2)],[0,1,0],[-math.sin(r_y + np.pi/2),0,math.cos(r_y + np.pi/2)]],dtype= np.float64)
R_Matrix_x_rect = np.asarray([[1,0,0],[0,math.cos(np.pi/2),math.sin(np.pi/2)],[0,-math.sin(np.pi/2),math.cos(np.pi/2)]], dtype = np.float64)
p0 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,0,w/2.0],dtype = 'float32').flatten())
p1 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,0,w/2.0],dtype = 'float32').flatten())
p2 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,0,-w/2.0],dtype = 'float32').flatten())
p3 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,0,-w/2.0],dtype = 'float32').flatten())
p4 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,-h,w/2.0],dtype = 'float32').flatten())
p5 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,-h,w/2.0],dtype = 'float32').flatten())
p6 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,-h,-w/2.0],dtype = 'float32').flatten())
p7 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,-h,-w/2.0],dtype = 'float32').flatten())
p0_3 = center + np.dot(R_Matrix_y, np.asarray([l/2.0 ,0, 0], dtype= 'float32').flatten())
p1_2 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0 ,0, 0], dtype= 'float32').flatten())
p4_7 = center + np.dot(R_Matrix_y, np.asarray([l/2.0 ,-h, 0], dtype= 'float32').flatten())
p5_6 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0 ,-h, 0], dtype= 'float32').flatten())
p0_1 = center + np.dot(R_Matrix_y, np.asarray([0,0,w/2.0], dtype= 'float32').flatten())
p3_2 = center + np.dot(R_Matrix_y, np.asarray([0,0,-w/2.0], dtype= 'float32').flatten())
p4_5 = center + np.dot(R_Matrix_y, np.asarray([0,-h,w/2.0], dtype= 'float32').flatten())
p7_6 = center + np.dot(R_Matrix_y, np.asarray([0,-h,-w/2.0], dtype= 'float32').flatten())
p0_4 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,-h/2.0,w/2.0], dtype= 'float32').flatten())
p3_7 = center + np.dot(R_Matrix_y, np.asarray([l/2.0,-h/2.0,-w/2.0], dtype= 'float32').flatten())
p1_5 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,-h/2.0,w/2.0], dtype= 'float32').flatten())
p2_6 = center + np.dot(R_Matrix_y, np.asarray([-l/2.0,-h/2.0,-w/2.0], dtype= 'float32').flatten())
p0_1_3_2 = center
print(p0_1_3_2)
#print(p0_1_3_2)
length_0_3 = np.linalg.norm(p0-p3)
cylinder_0_3 = open3d.create_mesh_cylinder (radius = 0.025,height = length_0_3)
cylinder_0_3.compute_vertex_normals()
transform_0_3 = np.eye(4)
transform_0_3[0:3, 0:3] = R_Matrix_y
transform_0_3[0:3 , 3] = p0_3
cylinder_0_3.transform(transform_0_3)
cylinder_0_3.paint_uniform_color(front_color)
length_1_2 = np.linalg.norm(p1-p2)
cylinder_1_2 = open3d.create_mesh_cylinder (radius = 0.025,height = length_1_2)
cylinder_1_2.compute_vertex_normals()
transform_1_2 = np.eye(4)
transform_1_2[0:3, 0:3] = R_Matrix_y
transform_1_2[0:3 , 3] = p1_2
cylinder_1_2.transform(transform_1_2)
cylinder_1_2.paint_uniform_color(front_color)
length_4_7 = np.linalg.norm(p4-p7)
cylinder_4_7 = open3d.create_mesh_cylinder (radius = 0.025,height = length_4_7)
cylinder_4_7.compute_vertex_normals()
transform_4_7 = np.eye(4)
transform_4_7[0:3, 0:3] = R_Matrix_y
transform_4_7[0:3 , 3] = p4_7
cylinder_4_7.transform(transform_4_7)
cylinder_4_7.paint_uniform_color(front_color)
length_5_6 = np.linalg.norm(p5-p6)
cylinder_5_6 = open3d.create_mesh_cylinder (radius = 0.025,height = length_5_6)
cylinder_5_6.compute_vertex_normals()
transform_5_6 = np.eye(4)
transform_5_6[0:3, 0:3] = R_Matrix_y
transform_5_6[0:3 , 3] = p5_6
cylinder_5_6.transform(transform_5_6)
cylinder_5_6.paint_uniform_color(front_color)
length_0_1 = np.linalg.norm(p0-p1)
cylinder_0_1 = open3d.create_mesh_cylinder (radius = 0.025,height = length_0_1)
cylinder_0_1.compute_vertex_normals()
transform_0_1 = np.eye(4)
transform_0_1[0:3, 0:3] = R_Matrix_y_rect
transform_0_1[0:3 , 3] = p0_1
cylinder_0_1.transform(transform_0_1)
cylinder_0_1.paint_uniform_color(front_color)
length_3_2 = np.linalg.norm(p3-p2)
cylinder_3_2 = open3d.create_mesh_cylinder (radius = 0.025,height = length_3_2)
cylinder_3_2.compute_vertex_normals()
transform_3_2 = np.eye(4)
transform_3_2[0:3, 0:3] = R_Matrix_y_rect
transform_3_2[0:3 , 3] = p3_2
cylinder_3_2.transform(transform_3_2)
cylinder_3_2.paint_uniform_color(front_color)
length_4_5 = np.linalg.norm(p4-p5)
cylinder_4_5 = open3d.create_mesh_cylinder (radius = 0.025,height = length_4_5)
cylinder_4_5.compute_vertex_normals()
transform_4_5 = np.eye(4)
transform_4_5[0:3, 0:3] = R_Matrix_y_rect
transform_4_5[0:3 , 3] = p4_5
cylinder_4_5.transform(transform_4_5)
cylinder_4_5.paint_uniform_color(front_color)
length_7_6 = np.linalg.norm(p7-p6)
cylinder_7_6 = open3d.create_mesh_cylinder (radius = 0.025,height = length_7_6)
cylinder_7_6.compute_vertex_normals()
transform_7_6 = np.eye(4)
transform_7_6[0:3, 0:3] = R_Matrix_y_rect
transform_7_6[0:3 , 3] = p7_6
cylinder_7_6.transform(transform_7_6)
cylinder_7_6.paint_uniform_color(front_color)
length_0_4 = np.linalg.norm(p0-p4)
cylinder_0_4 = open3d.create_mesh_cylinder (radius = 0.025,height = length_0_4)
cylinder_0_4.compute_vertex_normals()
transform_0_4 = np.eye(4)
transform_0_4[0:3, 0:3] = np.dot(R_Matrix_y,R_Matrix_x_rect)
transform_0_4[0:3 , 3] = p0_4
cylinder_0_4.transform(transform_0_4)
cylinder_0_4.paint_uniform_color(front_color)
length_3_7 = np.linalg.norm(p3-p7)
cylinder_3_7 = open3d.create_mesh_cylinder (radius = 0.025,height = length_3_7)
cylinder_3_7.compute_vertex_normals()
transform_3_7 = np.eye(4)
transform_3_7[0:3, 0:3] = np.dot(R_Matrix_y,R_Matrix_x_rect)
transform_3_7[0:3 , 3] = p3_7
cylinder_3_7.transform(transform_3_7)
cylinder_3_7.paint_uniform_color(front_color)
length_1_5 = np.linalg.norm(p1-p5)
cylinder_1_5 = open3d.create_mesh_cylinder (radius = 0.025,height = length_1_5)
cylinder_1_5.compute_vertex_normals()
transform_1_5 = np.eye(4)
transform_1_5[0:3, 0:3] = np.dot(R_Matrix_y,R_Matrix_x_rect)
transform_1_5[0:3 , 3] = p1_5
cylinder_1_5.transform(transform_1_5)
cylinder_1_5.paint_uniform_color(front_color)
length_2_6 = np.linalg.norm(p2-p6)
cylinder_2_6 = open3d.create_mesh_cylinder (radius = 0.025,height = length_2_6)
cylinder_2_6.compute_vertex_normals()
transform_2_6 = np.eye(4)
transform_2_6[0:3, 0:3] = np.dot(R_Matrix_y,R_Matrix_x_rect)
transform_2_6[0:3 , 3] = p2_6
cylinder_2_6.transform(transform_2_6)
cylinder_2_6.paint_uniform_color(front_color)
length_0_1_3_2 = np.linalg.norm(p0_1 - p3_2)
cylinder_0_1_3_2 = open3d.create_mesh_cylinder(radius=0.025, height=length_0_1_3_2)
cylinder_0_1_3_2.compute_vertex_normals()
transform_0_1_3_2 = np.eye(4)
transform_0_1_3_2[0:3, 0:3] = R_Matrix_y
transform_0_1_3_2[0:3, 3] = p0_1_3_2
cylinder_0_1_3_2.transform(transform_0_1_3_2)
cylinder_0_1_3_2.paint_uniform_color(front_color)
#return [cylinder_0_1_3_2, cylinder_0_3, cylinder_1_2, cylinder_4_7, cylinder_5_6, cylinder_0_1, cylinder_3_2, cylinder_4_5, cylinder_7_6, cylinder_0_4, cylinder_3_7, cylinder_1_5, cylinder_2_6]
return [cylinder_0_3, cylinder_1_2, cylinder_4_7, cylinder_5_6, cylinder_0_1, cylinder_3_2, cylinder_4_5, cylinder_7_6, cylinder_0_4, cylinder_3_7, cylinder_1_5, cylinder_2_6]
from open3d import create_mesh_sphere, create_mesh_cylinder, create_mesh_coordinate_frame, draw_geometries
mesh_sphere = create_mesh_sphere(radius=1.0)
mesh_sphere.compute_vertex_normals()
mesh_sphere.paint_uniform_color([0.1, 0.1, 0.7])
mesh_cylinder = create_mesh_cylinder(radius=0.3, height=4.0)
mesh_cylinder.compute_vertex_normals()
mesh_cylinder.paint_uniform_color([0.1, 0.9, 0.1])
mesh_frame = create_mesh_coordinate_frame(size=0.6, origin=[-2, -2, -2])
print("We draw a few primitives using collection.")
draw_geometries([mesh_sphere, mesh_cylinder, mesh_frame])
def create_skeleton_geometry(self):
'''
Create human skeleton geometry
'''
geometries = []
joint_colors = [[255, 0, 0], [255, 85, 0], [255, 170,
0], [255, 255, 0],
[170, 255, 0], [85, 255, 0], [0, 255, 0], [0, 255, 85],
[0, 255, 170], [0, 255, 255], [0, 170, 255],
[0, 85, 255], [0, 0, 255], [85, 0, 255], [170, 0, 255],
[255, 0, 255], [255, 0, 170], [255, 0, 85]]
limb_colors = [
[0, 255, 0],
[0, 255, 85],
[0, 255, 170],
[0, 255, 255],
[0, 170, 255],
[0, 85, 255],
[255, 0, 0],
[255, 85, 0],
[255, 170, 0],
[255, 255, 0.],
[255, 0, 85],
[170, 255, 0],
[85, 255, 0],
[170, 0, 255.],
[0, 0, 255],
[0, 0, 255],
[255, 0, 255],
[170, 0, 255],
[255, 0, 170],
]
for i, (jointType, color) in enumerate(zip(JointType, joint_colors)):
if np.all(self.joints[jointType.name] != 0):
sphere = o3.create_mesh_sphere(radius=10.0)
pos = np.concatenate(
[np.asarray(self.joints[jointType.name]), [1]])
# create translation matrix
Tm = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0],
pos]).T
# move sphere
sphere.transform(Tm)
# paint sphere
sphere.paint_uniform_color([v / 255 for v in color])
geometries.append(sphere)
for i, (limb,
color) in enumerate(zip(params['limbs_point'], limb_colors)):
if i != 9 and i != 13: # don't show ear-shoulder connection
l1 = limb[0].name
l2 = limb[1].name
pl1 = self.joints[l1]
pl2 = self.joints[l2]
if np.any(pl1) and np.any(pl2):
dist = np.linalg.norm(pl1 - pl2)
midpoint = np.concatenate([(pl1 + pl2) / 2, [1]])
# orientation of cylindar (z axis)
vec_cylindar = np.array([0, 0, 1])
# normalized vector of the two points connected
norm = self.normalize(pl2 - pl1)
# get rotation matrix
R = self.get_rotation(vec_cylindar, norm).T
# create translation matrix
tm1 = np.concatenate([R[0], [0]])
tm2 = np.concatenate([R[1], [0]])
tm3 = np.concatenate([R[2], [0]])
Tm = np.array([tm1, tm2, tm3, midpoint]).T
# create the cylinder
cylinder = o3.create_mesh_cylinder(radius=5.0, height=dist)
# move the cylinder
cylinder.transform(Tm)
# paint the cylinder
cylinder.paint_uniform_color([v / 255 for v in color])
geometries.append(cylinder)
return geometries