example python code to plot the 3D pose transformation

[weisongwen]

`
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def plot_pose(ax, pose, label, color):
"""
Plots a 3D pose on the given axis.

Parameters:
ax (mpl_toolkits.mplot3d.Axes3D): The axis to plot on.
pose (np.ndarray): The 4x4 transformation matrix representing the pose.
label (str): The label for the pose.
color (str): The color for the pose.
"""
origin = pose[:3, 3]
x_axis = pose[:3, 0]
y_axis = pose[:3, 1]
z_axis = pose[:3, 2]

ax.quiver(*origin, *x_axis, color=color, length=1, normalize=True, label=f'{label} x-axis')
ax.quiver(*origin, *y_axis, color=color, length=1, normalize=True, linestyle='dashed', label=f'{label} y-axis')
ax.quiver(*origin, *z_axis, color=color, length=1, normalize=True, linestyle='dotted', label=f'{label} z-axis')
ax.text(*origin, label, color=color, fontsize=12)

def create_transformation_matrix(tx, ty, tz, roll, pitch, yaw):
"""
Creates a 4x4 transformation matrix for a 3D pose.

Parameters:
tx (float): Translation in x.
ty (float): Translation in y.
tz (float): Translation in z.
roll (float): Rotation around x-axis in radians.
pitch (float): Rotation around y-axis in radians.
yaw (float): Rotation around z-axis in radians.

Returns:
np.ndarray: The 4x4 transformation matrix.
"""
# Rotation matrices around x, y, and z axes
Rx = np.array([
    [1, 0, 0],
    [0, np.cos(roll), -np.sin(roll)],
    [0, np.sin(roll), np.cos(roll)]
])

Ry = np.array([
    [np.cos(pitch), 0, np.sin(pitch)],
    [0, 1, 0],
    [-np.sin(pitch), 0, np.cos(pitch)]
])

Rz = np.array([
    [np.cos(yaw), -np.sin(yaw), 0],
    [np.sin(yaw), np.cos(yaw), 0],
    [0, 0, 1]
])

# Combined rotation matrix
R = Rz @ Ry @ Rx

# Transformation matrix
transformation_matrix = np.eye(4)
transformation_matrix[:3, :3] = R
transformation_matrix[:3, 3] = [tx, ty, tz]

return transformation_matrix

Create the figure and axis

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim([-10, 10])
ax.set_ylim([-10, 10])
ax.set_zlim([-10, 10])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.grid(True)

Define the original pose (identity matrix)

original_pose = np.eye(4)

Define a transformed pose (translation + rotation)

tx, ty, tz = 5, 3, 2 # Translate by (5, 3, 2)
roll, pitch, yaw = np.pi / 6, np.pi / 4, np.pi / 3 # Rotate by 30, 45, and 60 degrees respectively
transformed_pose = create_transformation_matrix(tx, ty, tz, roll, pitch, yaw)

Plot the original pose

plot_pose(ax, original_pose, 'Original', 'blue')

Plot the transformed pose

plot_pose(ax, transformed_pose, 'Transformed', 'red')

Add legend

ax.legend()

Show the plot

plt.show()

`