手眼标定中如何正确平均多个ARUCO旋转向量？

你当前直接对旋转向量的三个分量做算术平均是错误的——旋转向量（轴角表示）属于非线性的旋转空间，不满足欧几里得空间的加法性质，直接平均会导致旋转结果偏离真实姿态，甚至出现你观察到的旋转轴闪烁问题。平移向量可以直接平均，因为它是欧几里得空间的线性向量，这也是你平移稳定的原因。

正确的做法是先将旋转向量转换为四元数或旋转矩阵（推荐四元数，计算更简洁稳定），在旋转的“线性近似空间”中完成平均，再转换回旋转向量。

方法1：基于四元数的平均（推荐） #

四元数是表示旋转的紧凑且数值稳定的方式，平均步骤如下：

1. 每帧检测到有效旋转向量后，将其转换为四元数；
2. 收集所有有效帧的四元数，计算算术平均后归一化（保证四元数的单位长度）；
3. 将平均后的四元数转换回旋转向量。

对应OpenCV代码修改 #

首先，你需要在代码中维护一个四元数的容器，替换原来的avgRvecs累加逻辑：

// 全局/类成员变量：收集所有有效四元数
std::vector<cv::Vec4d> quaternions;
int framesFound = 0;
cv::Vec3d avgTvecs = cv::Vec3d(0,0,0);

// 检测到标记时的逻辑
if (rvecs.size() == 1) {
    framesFound++;
    // 1. 将当前旋转向量转换为旋转矩阵
    cv::Mat rotMat;
    cv::Rodrigues(rvecs[0], rotMat);
    // 2. 旋转矩阵转四元数
    cv::Vec4d q;
    cv::Rodrigues(rotMat, q); // OpenCV中Rodrigues支持旋转矩阵转四元数（Vec4d格式：w,x,y,z）
    quaternions.push_back(q);
    // 平移向量直接累加
    avgTvecs += tvecs[0];
}

// 所有帧处理完成后计算平均
if (framesFound > 0) {
    // 计算四元数的算术平均
    cv::Vec4d avgQ(0,0,0,0);
    for (const auto& q : quaternions) {
        avgQ += q;
    }
    // 归一化四元数，保证其为单位四元数
    double norm = cv::norm(avgQ);
    avgQ /= norm;
    // 将平均后的四元数转换回旋转向量
    cv::Vec3d avgRvecs;
    cv::Rodrigues(avgQ, avgRvecs);
    // 平移向量取平均
    avgTvecs /= framesFound;

    // 绘制轴
    cv::drawFrameAxes(outputImage, camMatrix, distCoeffs, avgRvecs, avgTvecs, 0.1);
}

方法2：基于旋转矩阵的平均 #

如果更倾向于用旋转矩阵，需要注意旋转矩阵是正交矩阵，直接算术平均后不再是正交矩阵，需要通过SVD分解求最优的正交近似：

1. 每帧将旋转向量转旋转矩阵；
2. 计算所有旋转矩阵的算术平均；
3. 对平均矩阵做SVD分解，得到最接近的正交旋转矩阵；
4. 将该旋转矩阵转回旋转向量。

对应OpenCV代码示例 #

// 全局/类成员变量
std::vector<cv::Mat> rotMats;
int framesFound = 0;
cv::Vec3d avgTvecs = cv::Vec3d(0,0,0);

// 检测到标记时
if (rvecs.size() == 1) {
    framesFound++;
    cv::Mat rotMat;
    cv::Rodrigues(rvecs[0], rotMat);
    rotMats.push_back(rotMat);
    avgTvecs += tvecs[0];
}

// 计算平均
if (framesFound > 0) {
    // 初始化平均矩阵为零矩阵
    cv::Mat avgRotMat = cv::Mat::zeros(3,3,CV_64F);
    for (const auto& rm : rotMats) {
        avgRotMat += rm;
    }
    avgRotMat /= framesFound;
    // SVD分解求最优正交旋转矩阵
    cv::Mat U, S, Vt;
    cv::SVD::compute(avgRotMat, S, U, Vt);
    cv::Mat optimalRotMat = U * Vt;
    // 确保旋转矩阵的行列式为1（避免镜像翻转）
    if (cv::determinant(optimalRotMat) < 0) {
        Vt.row(2) *= -1;
        optimalRotMat = U * Vt;
    }
    // 转回旋转向量
    cv::Vec3d avgRvecs;
    cv::Rodrigues(optimalRotMat, avgRvecs);
    avgTvecs /= framesFound;

    cv::drawFrameAxes(outputImage, camMatrix, distCoeffs, avgRvecs, avgTvecs, 0.1);
}

额外注意点 #

你当前代码中有个笔误：avgRvecs[i] =+ rvecs[0][i]; 应该写成 avgRvecs[i] += rvecs[0][i];，否则每次都是赋值为正的当前分量，完全错误，这也是导致结果异常的原因之一。

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