### 将6D Pose转换为4x4 Transformation Matrix 在机器人学和计算机视觉领域，通常会遇到将6D姿态（位置向量`[x, y, z]`和旋转表示`[roll, pitch, yaw]`）转化为齐次变换矩阵的需求。以下是实现此功能的Python代码示例： ```python import numpy as np def euler_to_rotation_matrix(roll, pitch, yaw): """ Convert Euler angles (roll, pitch, yaw) to a rotation matrix. Parameters: roll (float): Rotation around the x-axis in radians. pitch (float): Rotation around the y-axis in radians. yaw (float): Rotation around the z-axis in radians. Returns: np.ndarray: A 3x3 rotation matrix. """ # Define trigonometric functions for each angle cy = np.cos(yaw) sy = np.sin(yaw) cp = np.cos(pitch) sp = np.sin(pitch) cr = np.cos(roll) sr = np.sin(roll) # Compute the rotation matrix using ZYX convention R_x = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) R_y = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) R_z = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) R = R_z @ R_y @ R_x return R def pose_to_transformation_matrix(x, y, z, roll, pitch, yaw): """ Convert a 6D pose (position and orientation) into a 4x4 homogeneous transformation matrix. Parameters: x (float): Translation along the x-axis. y (float): Translation along the y-axis. z (float): Translation along the z-axis. roll (float): Rotation around the x-axis in radians. pitch (float): Rotation around the y-axis in radians. yaw (float): Rotation around the z-axis in radians. Returns: np.ndarray: A 4x4 transformation matrix. """ # Create the translation vector t = np.array([x, y, z]) # Get the rotation matrix from Euler angles R = euler_to_rotation_matrix(roll, pitch, yaw) # Construct the full transformation matrix T = np.eye(4) T[:3, :3] = R T[:3, 3] = t return T # Example usage of the function if __name__ == "__main__": # Input values for position and orientation x, y, z = 1.0, 2.0, 3.0 roll, pitch, yaw = np.radians(30), np.radians(45), np.radians(60) # Generate the transformation matrix transformation_matrix = pose_to_transformation_matrix(x, y, z, roll, pitch, yaw) print("Transformation Matrix:") print(transformation_matrix) ``` 上述代码实现了从6D姿态到4x4齐次变换矩阵的转换过程[^1]。具体而言，函数`euler_to_rotation_matrix`负责将欧拉角（roll、pitch、yaw）转为旋转矩阵；而`pose_to_transformation_matrix`则进一步组合平移部分与旋转部分形成完整的变换矩阵。 #### 关键点说明 - **Euler Angles**: 使用Z-Y-X顺序来构建旋转矩阵，这是常见的约定之一。 - **Translation Vector**: 平移分量直接存储于齐次变换矩阵的最后一列前三个元素中。 - **Efficiency Consideration**: 所有的内部数据操作都采用NumPy数组形式完成，相比纯Python列表更加高效。