基于OpenCV/ArUco将位姿参数转换至指定标签坐标系的技术问询

基础变换逻辑 #

明确坐标系变换关系：

• 相机坐标系下，标签0的旋转向量为rvecs[0]、平移向量为tvecs[0]；任意标签i的旋转向量为rvecs[i]、平移向量为tvecs[i]。
• 目标是将所有标签位姿从相机坐标系转换到标签0的局部坐标系。

关键变换公式 #

1. 旋转向量转旋转矩阵：

   • 标签0相对相机的旋转矩阵R_c0：通过cv.Rodrigues(rvecs[0])转换得到
   • 任意标签i相对相机的旋转矩阵R_ci：通过cv.Rodrigues(rvecs[i])转换得到

2. 计算标签0到相机的逆变换：

   • 旋转矩阵逆变换：R_0c = R_c0.T（旋转矩阵的逆等于自身转置）
   • 平移向量逆变换：t_0c = -R_0c @ tvecs[0]（矩阵乘法，注意维度匹配）

3. 计算标签i在标签0坐标系下的位姿：

   • 旋转矩阵：R_0i = R_0c @ R_ci
   • 平移向量：t_0i = R_0c @ tvecs[i] + t_0c
   • 若需转回旋转向量，再次调用cv.Rodrigues(R_0i)即可得到标签0坐标系下的rvec_0i。

OpenCV代码示例（Python） #

import cv2 as cv
import numpy as np

# 假设已通过ArUco检测得到rvecs、tvecs（均为列表，元素为(3,1)数组）
# 处理标签0的基础变换
rvec_0 = rvecs[0]
tvec_0 = tvecs[0]

# 旋转向量转旋转矩阵
R_c0, _ = cv.Rodrigues(rvec_0)
# 计算标签0到相机的逆变换矩阵
R_0c = R_c0.T
t_0c = -R_0c @ tvec_0

# 遍历所有标签，转换位姿到标签0坐标系
for i in range(len(rvecs)):
    rvec_i = rvecs[i]
    tvec_i = tvecs[i]
    
    # 标签i相对相机的旋转矩阵
    R_ci, _ = cv.Rodrigues(rvec_i)
    
    # 计算标签i在标签0坐标系下的位姿
    R_0i = R_0c @ R_ci
    t_0i = R_0c @ tvec_i + t_0c
    
    # 可选：转回旋转向量格式
    rvec_0i, _ = cv.Rodrigues(R_0i)
    
    # 输出结果
    print(f"标签{i}在标签0坐标系下的rvec:\n{rvec_0i}")
    print(f"标签{i}在标签0坐标系下的tvec(米):\n{t_0i}")

注意事项 #

• 确保向量与矩阵维度匹配：OpenCV的tvecs默认是(3,1)列向量，矩阵乘法需用@（Python3.5+）或np.matmul，避免用*（元素级乘法）。
• 验证正确性：标签0转换后的t_00应接近(0,0,0)，R_00应为单位矩阵。

内容的提问来源于stack exchange，提问作者SuperMouette