OpenCV 4.8.1中ArUco标记solvePnP位姿估计偏移问题求助

问题描述 #

使用OpenCV 4.8.1.78版本进行ArUco标记位姿估计，因版本较新采用solvePnP方法替代旧版受限函数。标记检测准确，但输出的旋转向量（rvec）、平移向量（tvec）绘制出的轴未与标记对齐，反而固定在帧左下角。已排查相机校准流程，未发现问题，附上代码寻求解决思路。

from utils import ARUCO_DICT, aruco_display
import cv2
import numpy as np
import matplotlib.pyplot as plt
import csv


def detect_aruco(camera, video, type, output_video_path, marker_size):
    if camera:
        video_capture = cv2.VideoCapture(0)
        cv2.waitKey(2000)
    else:
        if video is None:
            print("[Error] Video file location is not provided")
            return

        video_capture = cv2.VideoCapture(video)

    if type not in ARUCO_DICT:
        print(f"ArUCo tag type '{type}' is not supported")
        return

    aruco_dict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[type])
    aruco_params = cv2.aruco.DetectorParameters()


    fourcc = cv2.VideoWriter.fourcc(*'mp4v')
    output_width = int(video_capture.get(cv2.CAP_PROP_FRAME_WIDTH))
    output_height = int(video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
    output_video = cv2.VideoWriter(output_video_path, fourcc, 20.0, (output_width, output_height))

    all_tvecs = []
    all_rvecs = []

    while True:
        ret, frame = video_capture.read()
        print(ret)

        if not ret:
            false_count += 1
            if false_count >= 2:
                break  # If 'ret' is False twice consecutively, exit the loop
            continue  # Skip to the next frame

        # Reset false_count if 'ret' is True
        false_count = 0

        h, w, _ = frame.shape
        width = 1000
        height = int(width * (h / w))
        frame = cv2.resize(frame, (width, height), interpolation=cv2.INTER_CUBIC)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        corners, ids, rejected = cv2.aruco.detectMarkers(gray, aruco_dict)
        detected_markers = aruco_display(corners, ids, rejected, frame)

        markerPoints = np.array([[0, 0, 0],
                                 [marker_size, 0, 0],
                                 [marker_size, marker_size, 0],
                                 [0, marker_size, 0]], dtype=np.float32)

        if np.all(ids is not None):
            for i in range(0, len(ids)):
                trash, rvec, tvec = cv2.solvePnP(markerPoints, corners[i], camera_matrix,
                                                 distortion_coefficients, False, cv2.SOLVEPNP_IPPE_SQUARE)


                all_tvecs.append(tvec)
                all_rvecs.append(rvec)

                cv2.aruco.drawDetectedMarkers(frame, corners)
                cv2.drawFrameAxes(frame, camera_matrix, distortion_coefficients, np.array(rvec), np.array(tvec), 0.1)

        cv2.imshow("Image", detected_markers)

        key = cv2.waitKey(1) & 0xFF
        if key == ord("q"):
            break

    cv2.destroyAllWindows()
    video_capture.release()
    output_video.release()

    return all_tvecs

核心问题排查与解决思路 #

1. 图像缩放导致相机参数不匹配 #

代码中对帧进行了resize操作（缩放到width=1000），但solvePnP和drawFrameAxes使用的camera_matrix是基于原始图像尺寸校准的，缩放后内参未同步更新，直接导致位姿计算完全错误。

解决方法：

• 方案一：移除帧resize操作，保持与校准图像尺寸一致
• 方案二：根据缩放比例重新计算相机内参：

  # 在resize后添加
  original_width = output_width
  scale = width / original_width
  # 生成适配缩放后图像的相机矩阵
  new_camera_matrix = camera_matrix.copy()
  new_camera_matrix[0, :] *= scale  # fx, cx按比例缩放
  new_camera_matrix[1, :] *= scale  # fy, cy按比例缩放
  new_camera_matrix[2, 2] = 1.0     # 保持主点z分量不变
  

  后续solvePnP和drawFrameAxes均使用new_camera_matrix。

2. 绘制对象不匹配 #

代码中将标记绘制到detected_markers，但轴绘制到原始frame，最后imshow的是detected_markers，导致轴无法显示在最终窗口的正确位置。

解决方法：
将轴和标记统一绘制到detected_markers上：

if np.all(ids is not None):
    for i in range(0, len(ids)):
        # ... 计算new_camera_matrix、rvec、tvec ...
        cv2.aruco.drawDetectedMarkers(detected_markers, corners)
        cv2.drawFrameAxes(detected_markers, new_camera_matrix, distortion_coefficients, rvec, tvec, marker_size/2)

3. 未初始化变量导致潜在异常 #

代码中false_count在首次使用前未初始化，会引发运行时错误，虽不直接导致位姿偏移，但需修复：

all_tvecs = []
all_rvecs = []
false_count = 0  # 初始化变量

while True:
    ret, frame = video_capture.read()
    # ... 后续代码 ...

4. 轴长度单位不匹配 #

drawFrameAxes的最后一个参数是轴长度，需与marker_size的单位一致（如均为米或毫米）。建议设置为marker_size/2，轴长度与标记尺寸成比例，显示更直观。

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