I have a 3D bounding box defined by 8 vertices (values might vary from positive to negative etc), and I want to get the pose of the center of the box to be aligned with the left (y_max), bottom (z_min), nearest (x_min) corner edges of the bounding box (in the given example here, it should be corner 0, but y axis is flipped):
I have tried this:
#!/usr/bin/env python3
import numpy as np
import open3d as o3d
import matplotlib.pyplot as plt
def create_frame(
translation: np.ndarray = np.zeros((3, 1), dtype=np.float32),
orientation: np.ndarray = np.eye(3, dtype=np.float32),
frame_size=0.1,
) -> o3d.geometry.TriangleMesh:
frame_pose = o3d.geometry.TriangleMesh.create_coordinate_frame(
size=frame_size, origin=translation
)
frame_pose.rotate(orientation)
return frame_pose
def plot_box(vertices: np.ndarray) -> None:
# Define the edges of the cuboid
edges_ = [[0, 1],[0, 2],[0, 3],[1, 6],[1, 7],[2, 5],[2, 7],[3, 5],[3, 6],[4, 5],[4, 6],[4, 7],]
# Create a 3D plot
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
ax.scatter(0.0, 0.0, 0.0, color="r")
# Plot the vertices
ax.scatter(vertices[:, 0], vertices[:, 1], vertices[:, 2], color="b")
# Draw the edges
for edge in edges_:
ax.plot(vertices[edge, 0], vertices[edge, 1], vertices[edge, 2], color="c")
# Annotate each vertex with its index
for i, (x, y, z) in enumerate(vertices):
ax.text(x, y, z, f"{i}", size=20, zorder=1, color="k")
# Set labels
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
plt.show()
def pose_from_points(vertices: np.ndarray) -> np.ndarray:
# # Define edges of the cube
edges = {
0: [(0, 1), (0, 2), (0, 3)],
1: [(0, 1), (1, 6), (1, 7)],
2: [(0, 2), (2, 5), (2, 7)],
3: [(0, 3), (3, 5), (3, 6)],
4: [(4, 5), (4, 6), (4, 7)],
5: [(2, 5), (3, 5), (4, 5)],
6: [(6, 1), (6, 3), (6, 4)],
7: [(1, 7), (2, 7), (4, 7)],
}
# Max X, Max Y, Min Z
zero_corner_idx = np.argmin(vertices[:, 0] - vertices[:, 1] + vertices[:, 2])
zero_edges = edges[zero_corner_idx]
# Calculate edge lengths
edge_lengths = np.linalg.norm(
vertices[np.array(zero_edges)[:, 0]] - vertices[np.array(zero_edges)[:, 1]],
axis=1,
)
print(f"zero corner ({zero_corner_idx}): {vertices[zero_corner_idx]}")
print("edge lenghts ", edge_lengths)
# Find longest and shortest edges
longest_edge_idx = np.argmax(edge_lengths)
shortest_edge_idx = np.argmin(edge_lengths)
# Define x-axis direction vector
x_axis = (
vertices[zero_edges[longest_edge_idx][1]]
- vertices[zero_edges[longest_edge_idx][0]]
)
x_axis = x_axis / np.linalg.norm(x_axis)
if x_axis[0] < 0:
print("--- Negative X axis! ---")
x_axis = -x_axis
# Define z-axis direction vector
z_axis = (
vertices[zero_edges[shortest_edge_idx][1]]
- vertices[zero_edges[shortest_edge_idx][0]]
)
z_axis = z_axis / np.linalg.norm(z_axis)
if z_axis[2] < 0:
print("--- Negative Z axis! ---")
z_axis = -z_axis
# Calculate y-axis direction vector
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / np.linalg.norm(y_axis)
# Build transformation matrix
center = np.mean(vertices, axis=0)
pose = np.eye(4)
# Translation
pose[:3, 3] = center
# Rotation
pose[:3, 0] = x_axis
pose[:3, 1] = y_axis
pose[:3, 2] = z_axis
return pose
if __name__ == "__main__":
root_frame = create_frame(frame_size=0.08)
# Vertices of the cuboid
vertices = np.array(
[
[0.758587, 0.219573, 0.227423],
[0.642588, -0.175735, 0.221591],
[1.06273, 0.129976, 0.251165],
[0.737332, 0.221676, 0.50764],
[0.925473, -0.26323, 0.52555],
[1.04147, 0.132079, 0.531382],
[0.621333, -0.173633, 0.501808],
[0.946728, -0.265333, 0.245333],
]
)
obbox = o3d.geometry.OrientedBoundingBox.create_from_points(
o3d.utility.Vector3dVector(vertices)
)
obbox.color = [0.5, 0.2, 0.8]
pose = pose_from_points(vertices=vertices)
pose_frame = create_frame(
translation=pose[:3, 3],
orientation=pose[:3, :3],
)
# print(obbox.extent)
# pose_frame.translate(-obbox.extent / 2, relative=True)
o3d.visualization.draw_geometries([root_frame, pose_frame, obbox])
plot_box(vertices=vertices)