使用 python cv，获得旋转矩阵的 rpy

在Python中，如果你想要通过计算机视觉库（如OpenCV, cv2）获取一个旋转矩阵（Rotation Matrix），通常用于描述三维空间中的物体旋转，你需要先确定三个旋转参数，即绕X轴、Y轴和Z轴的角度，这被称为"Roll-Pitch-Yaw" (rpy)。

`rpy`分别代表：

- Roll: 绕Z轴的旋转角度
- Pitch: 绕X轴的旋转角度
- Yaw: 绕Y轴的旋转角度

转换`rpy`到旋转矩阵的过程可以分为几个步骤：

1. 导入必要的库：

import numpy as np
from cv2 import Rodrigues

2. 创建一个`rpy`数组，例如30度绕X轴，45度绕Y轴，60度绕Z轴：

rpy = [45, 30, 60]  # 度数转为弧度

注意：OpenCV一般使用弧度作为旋转参数。

3. 转换为旋转矩阵：

# 将rpy转换为四元素向量形式，前两个元素是绕x轴和y轴的旋转角的sin值，第三个元素是这两个旋转角的cos值，第四个元素是绕z轴的旋转角
rpy_rad = np.deg2rad(rpy)
tait_bryan_matrix = np.array([[np.cos(rpy_rad[0]) * np.cos(rpy_rad[1]), np.sin(rpy_rad[0]) * np.cos(rpy_rad[2]) - np.cos(rpy_rad[0]) * np.sin(rpy_rad[1]) * np.sin(rpy_rad[2]), np.sin(rpy_rad[0]) * np.sin(rpy_rad[2]) + np.cos(rpy_rad[0]) * np.sin(rpy_rad[1]) * np.cos(rpy_rad[2])],
                               [-np.sin(rpy_rad[1]) * np.cos(rpy_rad[2]), np.cos(rpy_rad[1]) * np.cos(rpy_rad[2]) + np.sin(rpy_rad[0]) * np.sin(rpy_rad[2]), -np.cos(rpy_rad[1]) * np.sin(rpy_rad[2])],
                               [np.sin(rpy_rad[2]), np.cos(rpy_rad[2]), np.cos(rpy_rad[2])]])

# OpenCV的Rodrigues函数将四元向量转换为旋转矩阵
rotation_matrix = Rodrigues(tait_bryan_matrix)[0]

`rotation_matrix`就是你要的旋转矩阵了。