📚 课程目录第4章基于ArUco码的位姿估计

4. 基于ArUco码的位姿估计

任务目标

本案例实现基于OpenCV的ArUco码检测与位姿估计：输入一张包含ArUco码的图像（或实时相机帧），检测码的四个角点，结合相机内参与畸变系数，解算码坐标系相对于相机坐标系的旋转向量和平移向量，并在图像上绘制检测结果与坐标轴。

环境依赖

需要 opencv-python（≥4.7，包含 aruco 模块）与 numpy。若使用实时相机，还需 opencv-python 自带的VideoCapture支持。

关键完整代码

import cv2
import numpy as np

# ---------- 1. 相机参数（以真实标定结果替换） ----------
# 此处为示例内参，实际使用时必须替换为标定得到的值
CAMERA_MATRIX = np.array([[800, 0, 320],
                          [0, 800, 240],
                          [0, 0, 1]], dtype=np.float32)
DIST_COEFFS = np.array([0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32)

# ---------- 2. ArUco字典与检测参数 ----------
ARUCO_DICT = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
DETECTOR_PARAMS = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(ARUCO_DICT, DETECTOR_PARAMS)

# ---------- 3. 单张图像位姿估计 ----------
def estimate_pose_on_image(image_path):
    img = cv2.imread(image_path)
    if img is None:
        print(f"无法读取图像: {image_path}")
        return

    corners, ids, _ = detector.detectMarkers(img)

    if ids is None:
        print("未检测到ArUco码")
        return

    # 所有检测到的码统一估计位姿
    rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
        corners, marker_length=0.05, cameraMatrix=CAMERA_MATRIX,
        distCoeffs=DIST_COEFFS
    )

    # 绘制检测结果
    cv2.aruco.drawDetectedMarkers(img, corners, ids)

    for i in range(len(ids)):
        # 绘制每个码的坐标轴（长度0.03m）
        cv2.drawFrameAxes(img, CAMERA_MATRIX, DIST_COEFFS,
                          rvecs[i], tvecs[i], 0.03)
        # 打印位姿结果
        print(f"Marker ID: {ids[i][0]}")
        print(f"  旋转向量 rvec: {rvecs[i].flatten()}")
        print(f"  平移向量 tvec: {tvecs[i].flatten()}")

    cv2.imshow("ArUco Pose Estimation", img)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

# ---------- 4. 实时相机位姿估计 ----------
def estimate_pose_live(camera_index=0):
    cap = cv2.VideoCapture(camera_index)
    if not cap.isOpened():
        print("无法打开相机")
        return

    while True:
        ret, frame = cap.read()
        if not ret:
            break

        corners, ids, _ = detector.detectMarkers(frame)

        if ids is not None:
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                corners, marker_length=0.05,
                cameraMatrix=CAMERA_MATRIX, distCoeffs=DIST_COEFFS
            )
            cv2.aruco.drawDetectedMarkers(frame, corners, ids)
            for i in range(len(ids)):
                cv2.drawFrameAxes(frame, CAMERA_MATRIX, DIST_COEFFS,
                                  rvecs[i], tvecs[i], 0.03)

        cv2.imshow("Live ArUco Pose", frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    # 二选一运行：
    # estimate_pose_on_image("aruco_sample.jpg")   # 单张图像
    estimate_pose_live(0)                          # 实时相机

运行方式

安装依赖：pip install opencv-python numpy
单张图像模式：准备一张包含ArUco码的图片（可用 cv2.aruco.generateImageMarker 生成），将代码中 estimate_pose_on_image 的路径替换为实际文件路径并取消注释。
实时模式：直接运行代码，默认调用摄像头0，按 q 退出。

验证方法与判定标准

检测验证：运行后窗口显示图像/视频帧，ArUco码被绿色边框标记，且每个码上绘制了红-绿-蓝三色坐标轴（分别对应X-Y-Z方向）。
数值验证：控制台输出每个码的ID、旋转向量（3元素）和平移向量（3元素）。平移向量的单位与 marker_length 一致（本案例设为0.05，即米）。
正确性判定：将ArUco码正对相机并缓慢移动，观察 tvec 的Z值应随距离增大而增大；旋转码时 rvec 各分量应相应变化。若数值随物理运动单调合理变化，则位姿估计功能正常。

常见问题

检测不到码：确认ArUco字典类型（本案例为DICT_4X4_50）与码的生成字典一致；图像过暗或过曝时调整光照。
位姿跳变：相机内参和畸变系数必须使用标定值，示例值仅用于演示流程，不能用于实际测量。
坐标轴方向异常：检查 marker_length 是否与真实码的物理尺寸一致（单位：米），不一致会导致平移向量比例错误。