完善上述代码，把最后返回的 trans_matri 转为RPY角 并详细注释

在将`trans_matrix`转换为Roll-Pitch-Yaw（RPY）角之前，需要先了解一下如何从四元数到RPY。四元数包含了一个标量部分和三个旋转分量，而RPY是三个独立的角度分别描述绕X、Y、Z轴的旋转。对于opencv的坐标系（Z-up），转换过程通常是这样的：

1. 将四元数分割成旋转分量和标量
2. 使用公式提取各个轴的旋转角度
3. 检查是否需要调整顺序（因为Z轴的旋转可能会导致其他轴的逆序）

以下是一个完整的例子，添加了对RPY角的计算并做了相应的注释：

from math import atan2, degrees

def rotate_from_quaternion(quat):
    """
    Convert a quaternion into Roll-Pitch-Yaw angles.
    The input quaternion assumes the following form [w, x, y, z], with w being the scalar part.
    """
    w, x, y, z = quat
    
    # Calculate Euler angles from quaternion
    r = degrees(atan2(2 * (w * x + y * z), 1 - 2 * (x**2 + z**2)))  # Roll (around Z-axis)
    p = degrees(atan2(2 * (w * y - z * x), 1 - 2 * (y**2 + z**2)))  # Pitch (around Y-axis)
    y = 0  # Yaw is typically kept fixed since it's not used in this function

    # For cv2 coordinate system (Z-up), we don't need to adjust RPY order
    return r, p, y

def pose_estimation_3d3d(pts1, pts2):
    # ... (之前的代码)

    # 转换为OpenCV的四元数形式
    # 注意，OpenCV的四元数是wxyz格式
    quat = cv2.quaternionFromRotation(trans_matrix[:3, :3])

    # 转换为RPY角
    roll, pitch, yaw = rotate_from_quaternion(quat)

    # 返回四元数和RPY角
    return trans_matrix, roll, pitch, yaw

# 示例用法
...
rpy_angles = pose_estimation_3d3d(pts1, pts2)[1:]
print(f"Roll: {rpy_angles[0]}, Pitch: {rpy_angles[1]}, Yaw: {rpy_angles[2]}")

在这个实现中，我们使用`cv2.quaternionFromRotation()`获取四元数，然后利用自定义的`rotate_from_quaternion()`函数将其转换为RPY。请注意，这里假设yaw不变，因为原始代码中并没有涉及到yaw旋转。如果需要完整地提取所有三个旋转角，请查阅相关的资料或OpenCV文档。