aruco 码识别

1124675264

我使用aruco_ros获取到aruco码的位姿和opencv2.aruco里计算得到的旋转矩阵不一样，不知道哪里出问题了，希望有大佬能帮帮忙看看

import cv2.aruco as aruco
import math
import numpy as np
import cv2


def getImagePos(image, mark_id, mark_size, show=False):
    fx = 913.118
    fy = 913.105
    cx = 654.698
    cy = 370.966

    # 加载相机参数（必须事先校准相机）
    camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])  # 替换为你的相机参数
    dist_coeffs = np.array([0, 0, 0, 0, 0])  # 替换为你的畸变系数

    # 定义ArUco字典
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)
    # 初始化检测器参数
    parameters = aruco.DetectorParameters()
    # 创建检测器对象
    detector = aruco.ArucoDetector(aruco_dict, parameters)

    # 读取图像
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # 检测标记
    corners, ids, rejected = detector.detectMarkers(gray)

    # 定义标记实际尺寸（单位：米，需与实际打印尺寸一致）
    marker_size = 0.1  # 例如：5cm边长的标记

    # 计算每个检测到的标记的位姿
    rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
        corners, marker_size, camera_matrix, dist_coeffs
    )

    # 绘制检测结果
    i = 0
    # 绘制标记边框和ID
    image = aruco.drawDetectedMarkers(image, corners, ids)

    # 绘制位姿坐标系（X:红色, Y:绿色, Z:蓝色）
    cv2.drawFrameAxes(image, camera_matrix, dist_coeffs,
                      rvecs[i], tvecs[i], marker_size / 2)


    if show:
        cv2.imshow('tt', image)
        cv2.waitKey()

    # 打印位姿信息
    print(f"Marker ID: {ids[i][0]}")
    print(f"旋转向量: \n{rvecs[i]}")
    print(f"平移向量: \n{tvecs[i]}\n")

    t = tvecs[i][0]
    dis = math.sqrt(t[0]**2 + t[1]**2 + t[2]**2)

    print(f"相对距离: \n{dis}")

    # 旋转向量转欧拉角度
    euler = [math.degrees(e) for e in rvecs[i][0]]
    print(f"旋转: \n{euler}")



image = cv2.imread("Combined Image_screenshot_08.03.2025.png")

getImagePos(image, 700, 0.1)

# Marker ID: 700
# 旋转向量: 
# [[ 0.27634649 -3.11669144 -0.07238494]]
# 平移向量: 
# [[ 0.07038931 -0.02667739  0.3864855 ]]
# 
# 相对距离: 
# 0.39374786279149726
# 旋转: 
# [15.833487609229413, -178.57326528556658, -4.1473514458683685]

原图
Combined Image_screenshot_08.03.2025.png
aruco_ros识别到的数据

机器人小伊

在你的问题中，你提到使用 OpenCV 和 aruco_ros 获取到的 ArUco 码的旋转矩阵不一致。这通常可能由以下几个原因引起：

相机校准参数差异：确保 OpenCV 和 aruco_ros 使用的相机内参（焦距、光心）和畸变系数完全一致。
标记尺寸不一致：在 OpenCV 和 aruco_ros 中使用的 ArUco 标记的实际尺寸（marker_size）必须相同。
旋转矩阵表示方法：OpenCV 和 ROS 可能使用不同的旋转表示方法（如旋转向量、旋转矩阵、四元数等）。你需要确保在比较这些值时，它们被转换为相同的表示形式。
坐标系方向：检查 ROS 和 OpenCV 的坐标系方向是否一致。例如，OpenCV 默认使用右手坐标系，而 ROS 的坐标系可能根据具体实现有所不同。
数值精度：由于浮点运算的精度问题，即使理论上相同的计算也可能产生略微不同的结果。

首先，我们来修正你的 OpenCV 代码中的一些小错误，并确保我们能够正确地输出旋转矩阵：

修正 OpenCV 代码

修正相机矩阵的定义：

camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])

修正旋转向量转欧拉角的代码：

rvec_matrix, _ = cv2.Rodrigues(rvecs[i][0])
euler_angles = cv2.decomposeProjectionMatrix(np.hstack((rvec_matrix, tvecs[i][:, np.newaxis], np.eye(3, 1))))[6]  # 注意：这里可能需要进一步处理来获取正确的欧拉角
# 或者使用更简单的方法（但可能不精确）:
theta_x, theta_y, theta_z = rvecs[i][0]
roll = np.degrees(theta_x)
pitch = np.degrees(theta_y)
yaw = np.degrees(theta_z)
print(f"欧拉角: Roll={roll}, Pitch={pitch}, Yaw={yaw}")

检查 ROS 数据

对于 aruco_ros，你需要确保：

使用的相机校准文件（通常是 YAML 格式）与 OpenCV 中使用的一致。
检查 ROS 节点发布的 /aruco_pose 或类似话题中的位姿数据，确保你正确解析了这些消息。

比较旋转矩阵

由于 OpenCV 使用旋转向量，而 ROS 可能使用旋转矩阵或四元数，你需要将它们转换为相同的表示形式进行比较。例如，你可以将 OpenCV 的旋转向量转换为旋转矩阵，然后与 ROS 的旋转矩阵进行比较。

总结

确保所有参数（相机内参、畸变系数、标记尺寸）在两个系统中完全一致，并正确转换和比较旋转表示。如果问题仍然存在，你可能需要更深入地检查 ROS 节点的实现，或者考虑数值精度问题。

以上为生成回答，仅供参考～

1124675264

好像我对旋转向量的理解弄错了

# 转旋转矩阵
 rotation_matrix, _ = cv2.Rodrigues(rvecs[i])
 print("旋转矩阵:\n", rotation_matrix)

 te = tfs.euler.mat2euler(rotation_matrix)
 euler = [math.degrees(e) for e in te]

 print(f"旋转: \n{euler}")

# 旋转矩阵:
#  [[-0.98433795 -0.17557593 -0.01587109]
#  [-0.17612344  0.98333823  0.04501615]
#  [ 0.0077029   0.04710638 -0.99886018]]
# 旋转:
# [177.2999241238412, -0.44134783830292884, -169.8556541200813]

这么操作后计算的手眼标定结果和ros_aruco一样，但角度还是不太一样

x= -0.3206890278648447
y= 0.9433653592637539
z= 0.01836774839137613
w= 0.08296367980809832

q = [w, x, y, z]
m = tfs.quaternions.quat2mat(q)
print(m)

e = tfs.euler.mat2euler(m)
print(e)

d = [math.degrees(i) for  i in e]
print(d)

# [[-0.78055115 -0.60810155  0.14474945]
#  [-0.60200613  0.79364235  0.08786608]
#  [-0.16831079 -0.01855609 -0.98555931]]
# (-3.1227669003410075, 0.16911576446501436, -2.4846248148191847)
# [-178.92136379268987, 9.689619552973825, -142.358515562613]