OpenCV相机标定与3D重建(10)眼标定函数calibrateHandEye()的使用

news2024/12/12 21:36:01

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

算法描述

计算手眼标定： $_{}^{g}\textrm{T}_c$
cv::calibrateHandEye 是 OpenCV 中用于手眼标定的函数。该函数通过已知的机器人末端执行器（gripper）相对于基座（base）和平板（target）相对于相机（cam）的姿态来计算相机相对于末端执行器的姿态。

该函数使用各种方法进行手眼标定。一种方法包括先估计旋转再估计平移（可分离解法），并且实现了以下方法：

R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration [269]
F. Park, B. Martin Robot Sensor Calibration: Solving AX = XB on the Euclidean Group [208]
R. Horaud, F. Dornaika Hand-Eye Calibration [124]
另一种方法包括同时估计旋转和平移（同时解法），并且实现了以下方法：
N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration [12]
K. Daniilidis Hand-Eye Calibration Using Dual Quaternions [63]
下图描述了手眼标定问题，其中需要估计安装在机器人末端执行器上的相机（“eye”）相对于末端执行器（“hand”）的变换。这种配置称为 eye-in-hand。

eye-to-hand 配置由一个静态相机观察安装在机器人末端执行器上的校准图案组成。然后可以通过输入合适的变换矩阵到函数中来估计从相机到机器人基座坐标系的变换，见下方说明。
在这里插入图片描述
标定过程如下：

使用静态校准图案来估计目标坐标系和相机坐标系之间的变换。
移动机器人末端执行器以获取多个姿态。
对于每个姿态，记录从末端执行器坐标系到机器人基座坐标系的齐次变换矩阵，例如使用机器人的运动学。
$\begin{bmatrix} X_b\\ Y_b\\ Z_b\\ 1 \end{bmatrix} = \begin{bmatrix} _{}^{b}\textrm{R}_g & _{}^{b}\textrm{t}_g \\ 0_{1 \times 3} & 1 \end{bmatrix} \begin{bmatrix} X_g\\ Y_g\\ Z_g\\ 1 \end{bmatrix}$
对于每个姿态，记录从校准目标坐标系到相机坐标系的齐次变换矩阵，例如使用基于2D-3D点对应关系的姿态估计方法（PnP）。
$\begin{bmatrix} X_c\\ Y_c\\ Z_c\\ 1 \end{bmatrix} = \begin{bmatrix} _{}^{c}\textrm{R}_t & _{}^{c}\textrm{t}_t \\ 0_{1 \times 3} & 1 \end{bmatrix} \begin{bmatrix} X_t\\ Y_t\\ Z_t\\ 1 \end{bmatrix}$
手眼标定过程返回以下齐次变换矩阵：
$\begin{bmatrix} X_g\\ Y_g\\ Z_g\\ 1 \end{bmatrix} = \begin{bmatrix} _{}^{g}\textrm{R}_c & _{}^{g}\textrm{t}_c \\ 0_{1 \times 3} & 1 \end{bmatrix} \begin{bmatrix} X_c\\ Y_c\\ Z_c\\ 1 \end{bmatrix}$
这个问题也被称为求解 AX=XB 方程：

对于 eye-in-hand 配置

对于 eye-to-hand 配置
在这里插入图片描述
注意
更多信息可以在该网站上找到。
至少需要两个具有非平行旋转轴的运动来确定手眼变换。因此，至少需要 3 个不同的姿态，但强烈建议使用更多的姿态。

函数原型

void cv::calibrateHandEye
(
	InputArrayOfArrays 	R_gripper2base,
	InputArrayOfArrays 	t_gripper2base,
	InputArrayOfArrays 	R_target2cam,
	InputArrayOfArrays 	t_target2cam,
	OutputArray 	R_cam2gripper,
	OutputArray 	t_cam2gripper,
	HandEyeCalibrationMethod 	method = CALIB_HAND_EYE_TSAI 
)

参数

参数[in] R_gripper2base: 从齐次矩阵中提取的旋转部分，该矩阵将一个用末端执行器坐标系表示的点变换到机器人基座坐标系 ( $_{}^{b}\textrm{T}_g$ )。这是一个包含所有从末端执行器坐标系到机器人基座坐标系变换的旋转矩阵（3x3）或旋转向量（3x1）的向量（vector）。
参数[in] t_gripper2base: 从齐次矩阵中提取的平移部分，该矩阵将一个用末端执行器坐标系表示的点变换到机器人基座坐标系 ( $_{}^{b}\textrm{T}_g$ )。这是一个包含所有从末端执行器坐标系到机器人基座坐标系变换的平移向量（3x1）的向量（vector）。
参数[in] R_target2cam: 从齐次矩阵中提取的旋转部分，该矩阵将一个用校准目标坐标系表示的点变换到相机坐标系 ( $_{}^{c}\textrm{T}_t$ )。这是一个包含所有从校准目标坐标系到相机坐标系变换的旋转矩阵（3x3）或旋转向量（3x1）的向量（vector）。
参数[in] t_target2cam: 从齐次矩阵中提取的平移部分，该矩阵将一个用校准目标坐标系表示的点变换到相机坐标系 ( $_{}^{c}\textrm{T}_t$ )。这是一个包含所有从校准目标坐标系到相机坐标系变换的平移向量（3x1）的向量（vector）。
参数[out] R_cam2gripper: 估计的从齐次矩阵中提取的旋转部分，该矩阵将一个用相机坐标系表示的点变换到末端执行器坐标系 ( $_{}^{g}\textrm{T}_c$ )。这是一个 3x3 的旋转矩阵。
参数[out] t_cam2gripper: 估计的从齐次矩阵中提取的平移部分，该矩阵将一个用相机坐标系表示的点变换到末端执行器坐标系 ( $_{}^{g}\textrm{T}_c$ )。这是一个 3x1 的平移向量。
参数[in] method: 实现的手眼标定方法之一，见 cv::HandEyeCalibrationMethod。

代码示例


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

using namespace cv;
using namespace std;

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

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

    // 从 target 到 cam 的旋转矩阵和位移向量
    vector< Mat > R_target2cam( num_poses );
    vector< Mat > t_target2cam( num_poses );

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

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

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

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

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

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

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

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

    // 输出变量
    Mat R_cam2gripper, t_cam2gripper;

    // 执行手眼标定
    calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, CALIB_HAND_EYE_TSAI );

    // 输出结果
    cout << "Rotation matrix from camera to gripper:\n" << R_cam2gripper << endl;
    cout << "Translation vector from camera to gripper:\n" << t_cam2gripper << endl;

    return 0;
}

运行结果

Rotation matrix from camera to gripper:
[0.7999999999999999, 0.6000000000000001, 0;
 -0.6000000000000001, 0.7999999999999999, 0;
 0, 0, 1]
Translation vector from camera to gripper:
[-0.4380000000000001;
 -0.6659999999999999;
 -0.63]