用 OpenCV 做相机标定与 ArUco 位姿估计

相机图像给出的是像素，机器人要的是射线和坐标。相机标定估计内参，把相机系三维点映射到像素，并建模镜头的畸变。知道这些值后，已知几何的标记就能给出相对相机的位姿。

ArUco 标记是带 ID 的方形二进制基准。检测出四个角就在图像里认出了标记；位姿估计把像素角点、真实标记尺寸和标定好的相机参数结合起来。

理解标定输出

相机矩阵含焦距和光心：

[ fx   0  cx ]
[  0  fy  cy ]
[  0   0   1 ]

fx 和 fy 以像素为单位。cx 和 cy 是主点位置。畸变系数建模径向桶形/枕形畸变和切向偏心等效应。

这些值属于某台相机、某个镜头、对焦位置和图像几何。裁剪或缩放改分辨率可能要调内参；换对焦或换镜头就要重标。

打印并量好标定板

棋盘格是实用的首选标定板，因为内角点好定义。格子边长要量准。位姿尺度继承这个单位：物点用米，平移就是米；物点用毫米，平移就是毫米。

标定板保持平直。卷曲的纸违反物点的平面几何假设。要精度就裱在硬板上。

定义内角点数，不是打印格数。10×7 格的板子内角点是 9×6。两者混了图像再清楚也检不出。

拍一组有用的照片

至少十张成功视图起步，之后加多样性而不是重复。让板子走遍整个画面，两个轴都倾斜，远近变化，图像边缘（畸变最强处）也要有角点。图案清晰完整。

二十张几乎一样的正对照片，不如几张分布好的位姿。扔掉运动模糊和角点不确定的帧。用运行时的分辨率和相机设置拍。

每张采用的图像，存检测到的图像角点和对应的平面物点 (列×格边长, 行×格边长, 0)。

用 OpenCV 标定

核心 Python 流程：

import cv2
import numpy as np

pattern = (9, 6)          # internal corners: columns, rows
square_size = 0.024       # metres

object_template = np.zeros((pattern[0] * pattern[1], 3), np.float32)
object_template[:, :2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1, 2)
object_template *= square_size

object_points, image_points = [], []

for gray in calibration_images:
    found, corners = cv2.findChessboardCorners(gray, pattern)
    if not found:
        continue
    refined = cv2.cornerSubPix(
        gray, corners, (11, 11), (-1, -1),
        (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001),
    )
    object_points.append(object_template.copy())
    image_points.append(refined)

rms, camera_matrix, distortion, rvecs, tvecs = cv2.calibrateCamera(
    object_points, image_points, gray.shape[::-1], None, None
)

采用图像太少先别调标定。把矩阵、畸变系数、图像宽高、标定板定义、标定日期存一起。光存一个数组文件以后容易误用。

别只看返回的 RMS

均方根（RMS）重投影误差汇总标定点投影位置和检测图像点差多远。越小一般越好，但一个数能藏住坏区域和弱图像集。

算每张图像的重投影误差，盯最差的几张。把重投影点画回图像上检查。去畸变后检查直的建筑边缘，尤其角落。再对已知距离的标定板估计位姿，把平移和实测比一比。

某张误差一家独大，只在有站得住的理由（如模糊、角点顺序错）时剔除。反复删高误差帧直到数字好看，会藏住差模型或差标定板。

检测 ArUco 标记

选一个预定义字典，标记也从同一字典生成。位网格越大 ID 越多，但图像尺寸固定时每位分到的像素越少。打印标记留安静的白边，量好位姿模型用的黑方边长。

dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
parameters = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, parameters)

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

老版本 OpenCV 的 API 构造不同，按所装版本的官方文档写。输出仍应是每个检出标记四个有序图像角点。

用已知标记尺寸估计位姿

在标记自身坐标系定义角点，解 PnP。对边长 s 的标记，OpenCV 的方形专用求解器要求角点顺序一致：

s = 0.080  # metres
object_corners = np.array([
    [-s/2,  s/2, 0],
    [ s/2,  s/2, 0],
    [ s/2, -s/2, 0],
    [-s/2, -s/2, 0],
], dtype=np.float32)

ok, rvec, tvec = cv2.solvePnP(
    object_corners,
    corners[0].reshape(4, 2),
    camera_matrix,
    distortion,
    flags=cv2.SOLVEPNP_IPPE_SQUARE,
)

tvec 是相机系下的标记原点，长度单位和 s 一致。rvec 是罗德里格斯旋转向量，不是欧拉角。用 cv2.Rodrigues() 转旋转矩阵，或经机器人坐标系变换。

画出投影坐标轴并实物验证。平移为正且量级可信不够——轴镜像或标记尺寸输错照样栽。

让位姿稳到机器人能用

标记像素少、角度太斜、糊了、贴图像边缘时位姿都发散。改善光照曝光、用大标记、走近，或允许时用多标记板。

只滤有效位姿并保留时间戳。低通滤波压抖动但加控制延迟。用重投影误差、距离界、运动限幅剔突变，别把每次检测无脑平均。

标记位姿是相对相机的。要指挥机械臂或底盘，经标定好的相机安装位变换到 base_link 或臂座。毫米/米在这里错了就是一千倍的错。

诊断常见故障

• **去畸变把直线掰弯：**标定视图覆盖不够、标定板不平，或图像几何变了。
• **距离尺度系统性错：**格子或标记尺寸量错或单位错。
• **位姿轴指向反了：**物点角点顺序或坐标系约定错。
• **标记 ID 闪：**标记太小、糊了、打印差，或字典不对。
• **正对视图附近位姿乱跳：**平面位姿二义性和弱像素几何，需要更好的视角、跟踪或多标记约束。
• **标定只在一个分辨率下work：**裁剪缩放后没调内参就复用。
• **机器人动作滞后标记：**平滑过度或时间戳过期藏住了当前几何。

用传感器模拟器探索噪声、偏置、滤波和延迟。机械臂模拟器展示相机几何对了目标位姿仍可能够不着。