OpenCV相机标定与3D重建(11)机器人世界手眼标定函数calibrateRobotWorldHandEye()的使用

• 操作系统：ubuntu22.04
• OpenCV版本：OpenCV4.9
• IDE:Visual Studio Code
• 编程语言：C++11

算法描述

计算机器人世界/手眼标定：${}_{}^w{\text{T}}_b$ 和${}_{}^c{\text{T}}_g$。
cv::calibrateRobotWorldHandEye 是 OpenCV 中用于机器人世界手眼标定的函数。该函数通过已知的世界坐标系（world）、相机坐标系（cam）、基座坐标系（base）和平板坐标系（gripper）的姿态来计算基座相对于世界的姿态以及末端执行器相对于相机的姿态。

函数原型

void cv::calibrateRobotWorldHandEye
(
InputArrayOfArrays R_world2cam,
InputArrayOfArrays t_world2cam,
InputArrayOfArrays R_base2gripper,
InputArrayOfArrays t_base2gripper,
OutputArray R_base2world,
OutputArray t_base2world,
OutputArray R_gripper2cam,
OutputArray t_gripper2cam,
RobotWorldHandEyeCalibrationMethod method = CALIB_ROBOT_WORLD_HAND_EYE_SHAH 
)

参数

• 参数R_world2cam: 从世界坐标系（world）到相机坐标系（camera）的齐次矩阵中提取的旋转部分 (cTw)。这是一个包含所有从世界坐标系到相机坐标系变换的旋转矩阵 (3x3) 或旋转向量 (3x1) 的向量 (vector)。
• 参数t_world2cam: 从世界坐标系（world）到相机坐标系（camera）的齐次矩阵中提取的平移部分 (cTw)。这是一个包含所有从世界坐标系到相机坐标系变换的平移向量 (3x1) 的向量 (vector)。
• 参数R_base2gripper: 从机器人基座坐标系（base）到末端执行器坐标系（gripper）的齐次矩阵中提取的旋转部分 (gTb)。这是一个包含所有从机器人基座坐标系到末端执行器坐标系变换的旋转矩阵 (3x3) 或旋转向量 (3x1) 的向量 (vector)。
• 参数t_base2gripper: 从机器人基座坐标系（base）到末端执行器坐标系（gripper）的齐次矩阵中提取的平移部分 (gTb)。这是一个包含所有从机器人基座坐标系到末端执行器坐标系变换的平移向量 (3x1) 的向量 (vector)。
• 参数R_base2world: 估计的从机器人基座坐标系（base）到世界坐标系（world）的齐次矩阵中提取的旋转部分 (wTb)，即 (3x3) 旋转矩阵。
• 参数t_base2world: 估计的从机器人基座坐标系（base）到世界坐标系（world）的齐次矩阵中提取的平移部分 (wTb)，即 (3x1) 平移向量。
• 参数R_gripper2cam: 估计的从末端执行器坐标系（gripper）到相机坐标系（camera）的齐次矩阵中提取的旋转部分 (cTg)，即 (3x3) 旋转矩阵。
• 参数t_gripper2cam: 估计的从末端执行器坐标系（gripper）到相机坐标系（camera）的齐次矩阵中提取的平移部分 (cTg)，即 (3x1) 平移向量。
• 参数method: 实现的机器人世界/手眼标定方法之一，参见 cv::RobotWorldHandEyeCalibrationMethod。

代码示例

#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>

using namespace cv;
using namespace std;

int main()
{
    // 假设我们有四组数据，分别对应不同的抓取位置
    int num_poses = 4;

    // 从 world 到 cam 的旋转矩阵和位移向量
    vector< Mat > R_world2cam( num_poses );
    vector< Mat > t_world2cam( num_poses );

    // 从 base 到 gripper 的旋转矩阵和位移向量
    vector< Mat > R_base2gripper( num_poses );
    vector< Mat > t_base2gripper( num_poses );

    // 初始化示例数据
    R_world2cam[ 0 ] = ( Mat_< double >( 3, 3 ) << 1, 0, 0, 0, 1, 0, 0, 0, 1 );
    t_world2cam[ 0 ] = ( Mat_< double >( 3, 1 ) << 0.1, 0.2, 0.3 );

    R_world2cam[ 1 ] = ( Mat_< double >( 3, 3 ) << 0, -1, 0, 1, 0, 0, 0, 0, 1 );
    t_world2cam[ 1 ] = ( Mat_< double >( 3, 1 ) << 0.4, 0.5, 0.6 );

    R_world2cam[ 2 ] = ( Mat_< double >( 3, 3 ) << 0, 0, -1, 0, 1, 0, 1, 0, 0 );
    t_world2cam[ 2 ] = ( Mat_< double >( 3, 1 ) << 0.7, 0.8, 0.9 );

    R_world2cam[ 3 ] = ( Mat_< double >( 3, 3 ) << 0, 0, 1, 0, 1, 0, -1, 0, 0 );
    t_world2cam[ 3 ] = ( Mat_< double >( 3, 1 ) << 1.0, 1.1, 1.2 );

    R_base2gripper[ 0 ] = ( Mat_< double >( 3, 3 ) << 1, 0, 0, 0, 1, 0, 0, 0, 1 );
    t_base2gripper[ 0 ] = ( Mat_< double >( 3, 1 ) << 0.3, 0.4, 0.5 );

    R_base2gripper[ 1 ] = ( Mat_< double >( 3, 3 ) << 0, -1, 0, 1, 0, 0, 0, 0, 1 );
    t_base2gripper[ 1 ] = ( Mat_< double >( 3, 1 ) << 0.6, 0.7, 0.8 );

    R_base2gripper[ 2 ] = ( Mat_< double >( 3, 3 ) << 0, 0, -1, 0, 1, 0, 1, 0, 0 );
    t_base2gripper[ 2 ] = ( Mat_< double >( 3, 1 ) << 0.9, 1.0, 1.1 );

    R_base2gripper[ 3 ] = ( Mat_< double >( 3, 3 ) << 0, 0, 1, 0, 1, 0, -1, 0, 0 );
    t_base2gripper[ 3 ] = ( Mat_< double >( 3, 1 ) << 1.2, 1.3, 1.4 );

    // 输出变量
    Mat R_base2world, t_base2world;
    Mat R_gripper2cam, t_gripper2cam;

    // 执行机器人世界手眼标定
    calibrateRobotWorldHandEye( R_world2cam, t_world2cam, R_base2gripper, t_base2gripper, R_base2world, t_base2world, R_gripper2cam, t_gripper2cam, CALIB_ROBOT_WORLD_HAND_EYE_SHAH );

    // 输出结果
    cout << "Rotation matrix from base to world:\n" << R_base2world << endl;
    cout << "Translation vector from base to world:\n" << t_base2world << endl;
    cout << "Rotation matrix from gripper to camera:\n" << R_gripper2cam << endl;
    cout << "Translation vector from gripper to camera:\n" << t_gripper2cam << endl;

    return 0;
}

运行结果

Rotation matrix from base to world:
[1, 0, 0;
 0, 1, 0;
 0, 0, 1]
Translation vector from base to world:
[4.163336342344337e-17;
 9.71445146547012e-17;
 1.387778780781446e-17]
Rotation matrix from gripper to camera:
[1, 0, 0;
 0, 1, 0;
 0, 0, 1]
Translation vector from gripper to camera:
[-0.2;
 -0.1999999999999999;
 -0.2000000000000001]

原文地址：https://blog.csdn.net/jndingxin/article/details/144345209

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！