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

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

算法描述

计算机器人世界/手眼标定： w T b _{}^{w}\textrm{T}_b wTb 和 c T g _{}^{c}\textrm{T}_g cTg。

cv::calibrateRobotWorldHandEye 是 OpenCV 中用于机器人世界手眼标定的函数。该函数通过已知的世界坐标系（world）、相机坐标系（cam）、基座坐标系（base）和平板坐标系（gripper）的姿态来计算基座相对于世界的姿态以及末端执行器相对于相机的姿态。

函数原型

cpp 复制代码

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。

代码示例

cpp 复制代码

#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;
}

运行结果

bash 复制代码

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]