Hand-To-Eye标定求助：OpenCV函数调用报错及相机-基座位姿转换疑问

问题背景 #

需要通过OpenCV的calibrateRobotWorldHandEye函数完成**外部固定相机（Hand-To-Eye）**标定，获取相机相对机器人基座的位姿，但现有示例多为眼在手上（Hand-in-Eye）场景，调用函数时出现迭代收敛失败错误。

当前代码调用 #

cv::calibrateRobotWorldHandEye(
    r_marker_2_cams, t_marker_2_cams, 
    r_base_2_eefs,   t_base_2_eefs, 
    r_base_2_marker, t_base_2_marker, 
    r_eef_2_cam, t_eef_2_cam
);

输入数据示例 #

{"0" : {"eef_to_base" : {"rotation" : [[-0.095606111055037801,-0.51025118851622564,-0.85469479707478668],[-0.2832817770222838,-0.80917743832918987,0.51476529419348682],[-0.95425934961939407,0.29133416881098761,-0.067182555378477338]],"translation" : [0.90001126059008763,0.26805892340487769,0.98525793586232102]},"marker_to_camera" : {"rotation" : [[0.92253465633644494,-0.2152414766107825,0.32031377523392118],[-0.2152414766107825,-0.97590218043617782,-0.035861413334157398],[0.32031377523392118,-0.035861413334157398,-0.94663247590026744]],"translation" : [0.03402884930726869,0.014143468196645812,0.15751829284878796]}},"1" : {"eef_to_base" : {"rotation" : [[-0.20698015831412614,-0.61227580911613599,-0.76307112881791062],[-0.41541975402669973,-0.65115455185373627,0.6351568133654526],[-0.88576839073691027,0.44845967842308965,-0.11957539378987342]],"translation" : [0.96772548551068494,0.19639468634609225,0.89220657837415496]},"marker_to_camera" : {"rotation" : [[0.95787953058770325,-0.23867817485777876,0.15968573426465274],[-0.23867817485777876,-0.97090358713932667,-0.019466723569897548],[0.15968573426465274,-0.019466723569897548,-0.98697594344837614]],"translation" : [0.044783191711068503,0.023470920523726766,0.15747101277574513]}},"2" : {"eef_to_base" : {"rotation" : [[-0.12350759604901118,-0.20680972023673849,-0.97055428149784384],[-0.18232558826984002,-0.95666344874673281,0.22705159258209323],[-0.97545028247484278,0.20499947670082008,0.080448498880580921]],"translation" : [0.73249608671719035,0.66220009961098181,0.89221177149030129]},"marker_to_camera" : {"rotation" : [[0.78348037242671398,-0.11539983601306734,0.61060738930204139],[-0.11539983601306734,-0.99253307053011008,-0.039509261600644968],[0.61060738930204139,-0.039509261600644968,-0.79094730189660378]],"translation" : [-0.021747313435247093,0.026637357656369078,0.1910133070186012]}},"3" : {"eef_to_base" : {"rotation" : [[-0.14139277743876,-0.58163823962076944,-0.80106494162396458],[-0.066538421728242203,-0.80178083087220542,0.59390246478676123],[-0.98771489860286521,0.13727511594141062,0.074664728093018773]],"translation" : [0.91470660231510692,0.28704308433655318,0.9501287577881915]},"marker_to_camera" : {"rotation" : [[0.95967525014499111,-0.063827726844024352,0.27376894554546011],[-0.063827726844024352,-0.99792109498052117,-0.0089168087790869148],[0.27376894554546011,-0.0089168087790869148,-0.96175415516447016]],"translation" : [0.026701151449500173,0.023098944615863572,0.16560896248519821]}},"4" : {"eef_to_base" : {"rotation" : [[-0.15445547258729375,-0.78526027945890686,-0.59959136125527657],[-0.023678306619458522,-0.6037576161025463,0.79681621393757118],[-0.98771597374117148,0.13726993298712675,0.074660034250187302]],"translation" : [0.95856056973329129,0.015659241202002405,0.95013413396721758]},"marker_to_camera" : {"rotation" : [[0.99915164331289708,-0.033625485075008531,-0.02377646774876508],[-0.033625485075008531,-0.99943442347142009,0.00039991726695511552],[-0.02377646774876508,0.00039991726695511552,-0.99971721984147655]],"translation" : [0.052542209909467959,0.026759669926727965,0.14180080527811537]}}}

报错信息 #

lib/opencv-4.5.2/modules/calib3d/src/calibration_handeye.cpp:524: 
error: (-7:Iterations do not converge) Rotation normalization issue: determinant(R) is null in function 'normalizeRotation

问题分析与解决办法 #

1. 参数方向错误（核心问题） #

calibrateRobotWorldHandEye函数要求的第二组参数是末端执行器到机器人基座的位姿（R_gripper2base、t_gripper2base），但你传入的r_base_2_eefs、t_base_2_eefs是基座到末端执行器的位姿，方向完全相反，这会导致旋转矩阵的行列式异常，触发收敛失败。

修正方法：

将基座到末端的位姿转换为末端到基座的位姿：

• 旋转矩阵：正交矩阵的逆等于其转置，直接对r_base_2_eefs取转置即可。
• 平移向量：用转置后的旋转矩阵乘以原平移向量的负值。

示例代码：

// 遍历所有位姿样本
for (size_t i = 0; i < r_base_2_eefs.size(); ++i) {
    cv::Mat base_to_eef_R = r_base_2_eefs[i];
    cv::Mat base_to_eef_t = t_base_2_eefs[i];
    
    // 转换为末端到基座的旋转与平移
    cv::Mat eef_to_base_R = base_to_eef_R.t();
    cv::Mat eef_to_base_t = -eef_to_base_R * base_to_eef_t;
    
    // 替换原参数
    r_base_2_eefs[i] = eef_to_base_R;
    t_base_2_eefs[i] = eef_to_base_t;
}

2. 旋转矩阵正交性验证与修正 #

报错提示旋转矩阵行列式为0，说明输入的旋转矩阵不是有效的正交矩阵（可能是采集或计算时的误差导致）。需要对每个旋转矩阵做正交化修正，确保其属于正交群（行列式为±1）。

正交化代码示例：

cv::Mat orthogonalizeRotation(const cv::Mat& R) {
    cv::Mat U, S, Vt;
    cv::SVD::compute(R, S, U, Vt);
    cv::Mat R_ortho = U * Vt;
    
    // 确保行列式为1（符合右手坐标系）
    if (cv::determinant(R_ortho) < 0) {
        Vt.row(2) *= -1;
        R_ortho = U * Vt;
    }
    return R_ortho;
}

// 对所有旋转矩阵应用正交化
for (auto& R : r_marker_2_cams) R = orthogonalizeRotation(R);
for (auto& R : r_base_2_eefs) R = orthogonalizeRotation(R);

3. 增加样本数量与位姿多样性 #

当前仅采集5组样本，数量过少且位姿变化可能不足，导致算法无法收敛。建议：

• 采集至少10组以上样本。
• 确保末端执行器的位姿覆盖不同的平移（前后、左右、上下）和旋转（俯仰、偏航、翻滚）角度，避免样本分布过于集中。

4. 坐标系一致性检查 #

确保所有坐标系（机器人基座、末端执行器、相机、标定板）都遵循右手定则，避免因坐标系方向不一致导致的位姿计算错误。

5. 获取相机到基座的位姿 #

完成标定后，你需要的r_cam_2_base、t_cam_2_base可以通过以下方式推导：

• 标定得到的r_eef_2_cam是末端到相机的旋转，t_eef_2_cam是末端到相机的平移。
• 相机到基座的位姿 = 末端到基座的位姿 × 相机到末端的位姿（即r_eef_2_cam的逆 × r_eef_2_base，平移同理）。

公式：

cv::Mat r_cam_2_eef = r_eef_2_cam.t();
cv::Mat t_cam_2_eef = -r_cam_2_eef * t_eef_2_cam;

cv::Mat r_cam_2_base = r_eef_2_base * r_cam_2_eef;
cv::Mat t_cam_2_base = r_eef_2_base * t_cam_2_eef + t_eef_2_base;

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