（二）手眼标定——概述+原理+常用方法汇总+代码实战（C++）

一、手眼标定简述

手眼标定的目的：让机械臂和相机关联，相机充当机械臂的”眼睛“，最终实现指哪打哪

相机的使用前提首先需要进行相机标定，可以参考博文：（一）相机标定——四大坐标系的介绍、对于转换、畸变原理以及OpenCV完整代码实战（C++版）

当通过相机内参和畸变纠正之后再进行后续的处理

1，坐标系

2，分类

手眼标定分为眼在手上和眼在手外两种

3，原理

以眼在手外为例进行分析

首先，我们先列举出有用的已知条件
Ⅰ. $base$下的$end$是已知的，可以通过示教器读取，$T_{end}^{base}$
Ⅱ. $end$和$obj$是一个整体，相对静止关系，是个恒等式可作为桥梁，$T_{obj}^{end}$
Ⅲ. 相机可以拍摄$obj$，故$T_{obj}^{cam}$是已知的

为了形成闭合回路，各个坐标系之间都可以相互转换，需要求解$T_{cam}^{base}$，也就是让机械臂知道相机，或者相机知道机械臂

$$T_{bas{e}_1}^{en{d}_1}\cdot T_{ca{m}_1}^{bas{e}_1}\cdot T_{ob{j}_1}^{ca{m}_1}=T_{bas{e}_2}^{en{d}_2}\cdot T_{ca{m}_2}^{bas{e}_2}\cdot T_{ob{j}_2}^{ca{m}_2}$$

$$(T_{bas{e}_2}^{en{d}_2}{)}^{-1}\cdot T_{bas{e}_1}^{en{d}_1}\cdot T_{ca{m}_1}^{bas{e}_1}=T_{ca{m}_2}^{bas{e}_2}\cdot T_{ob{j}_2}^{ca{m}_2}\cdot T_{ca{m}_1}^{ob{j}_1}$$

$$[(T_{bas{e}_2}^{en{d}_2}{)}^{-1}\cdot T_{bas{e}_1}^{en{d}_1}]\cdot T_{ca{m}_1}^{bas{e}_1}=T_{ca{m}_2}^{bas{e}_2}\cdot [(T_{ca{m}_2}^{ob{j}_2}{)}^{-1}\cdot T_{ca{m}_1}^{ob{j}_1}]$$

$$AX=XB$$
其中A和B是已知的，目标是求解X
所有的手眼标定都是为了求解AX=XB，只不过求解的方法和思路不同，比较出名的是Tsai-Lenz算法

同理眼在手上是将$T_{base}^{obj}$作为中间桥梁，因为$base$和$obj$是相对静止关系

二、手眼标定方法汇总

因为相机分为两类，2D相机和3D相机，故手眼标定方法又可以进行细分2D和3D类别

1，2D相机手眼标定

2D相机进行手眼标定的本质是真实的世界坐标系到相机的像素坐标系的映射，本质就是仿射变换
比如：一张图片上的某个像素坐标通过变换矩阵进行映射到实际的世界坐标系下，前期是不考虑Z轴，机械臂的运作均在同一个水平面
最常用的方法是九点标定法

2，3D相机手眼标定

3D相机拍摄的数据为点云，故可以通过ICP进行点云匹配求解得到变换矩阵
也可以通过OpenCV的solvePnP求解变换矩阵

三、代码实战

1，2D相机手眼标定——九点标定法

①操作流程

Ⅰ. 制作一张含有9个圆点的标定板于相机正下方（多少个点都行，9个点只是为了提高精度而已，我们用3*5=15个点为例进行演示）

Ⅱ. 相机固定，拍一张标定板照片

Ⅲ. 通过圆查找相关算法，依次识别出标定板中的9个圆点的像素坐标$(x_{im{g}_i},y_{im{g}_i})$

Ⅳ. 机械臂保持同一水平面，依次去触碰标定板的圆心，并依次记录这9个机械臂位姿$(x_{robo{t}_i},y_{robo{t}_i})$

9点标定必须要求机械臂所在同一水平面（Z是固定值），无法得到机械臂的姿态

这里我是用的是15个点进行手眼标定测试，以下面图片为例进行实战演示

②具体实现

Ⅰ. 找圆点函数——findCirclesGrid

相机拍摄得到上图数据，通过OpenCV的findCirclesGrid函数找圆点

接下来详细介绍一下该函数的用法，源码是这样的：

CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize,
                                   OutputArray centers, int flags = CALIB_CB_SYMMETRIC_GRID,
                                   const Ptr<FeatureDetector> &blobDetector = SimpleBlobDetector::create());

举例：

int w = 3;
int h = 5;

// 读取图像
cv::Mat image = cv::imread("beyondyanyu.jpg");

// 设置 SimpleBlobDetector 参数
cv::SimpleBlobDetector::Params params;
params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;

// 创建 SimpleBlobDetector 对象
cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(params);

// 存储角点
std::vector<cv::Point2f> corners;
bool found = cv::findCirclesGrid(gray, cv::Size(w, h), corners, cv::CALIB_CB_ASYMMETRIC_GRID, blobDetector);

Ⅱ. 斑点检测器参数设置——cv::SimpleBlobDetector

findCirclesGrid函数的核心在于斑点检测器参数的设置，也就是cv::SimpleBlobDetector::Params params;，源码是这样的：

struct CV_EXPORTS_W_SIMPLE Params
{
    CV_WRAP Params();
    CV_PROP_RW float thresholdStep;
    CV_PROP_RW float minThreshold;
    CV_PROP_RW float maxThreshold;
    CV_PROP_RW size_t minRepeatability;
    CV_PROP_RW float minDistBetweenBlobs;

    CV_PROP_RW bool filterByColor;
    CV_PROP_RW uchar blobColor;

    CV_PROP_RW bool filterByArea;
    CV_PROP_RW float minArea, maxArea;

    CV_PROP_RW bool filterByCircularity;
    CV_PROP_RW float minCircularity, maxCircularity;

    CV_PROP_RW bool filterByInertia;
    CV_PROP_RW float minInertiaRatio, maxInertiaRatio;

    CV_PROP_RW bool filterByConvexity;
    CV_PROP_RW float minConvexity, maxConvexity;

    CV_PROP_RW bool collectContours;

    void read( const FileNode& fn );
    void write( FileStorage& fs ) const;
};

举例：

// 设置 SimpleBlobDetector 参数
cv::SimpleBlobDetector::Params params;

params.maxArea = std::numeric_limits<float>::max();
params.minArea = 2;
params.minDistBetweenBlobs = 1;
params.filterByColor = true;
params.blobColor = 255;

Ⅲ. 需要改动的地方

①你的图片宽高实际多少个圆点

int w = 3;
int h = 5;

②你的圆点标定板的路径

cv::Mat image = cv::imread("35_1.jpg");

③你的圆点标定板是否是对称的
对称：cv::CALIB_CB_SYMMETRIC_GRID
非对称：cv::CALIB_CB_ASYMMETRIC_GRID

bool found = cv::findCirclesGrid(gray, cv::Size(w, h), corners, cv::CALIB_CB_SYMMETRIC_GRID, blobDetector);

④你的机械臂依次指向对应的圆点坐标时机械臂的位置，因为我这边标定板是3*5的对称圆点标定板，故需要得到机械臂的15个位置

    // 机器人坐标系中的点的坐标
    std::vector<Point2D> robotPoints = {
        {5, 5}, {10, 5}, {15, 5},
        {5, 10}, {10, 10}, {15, 10},
        {5, 15}, {10, 15}, {15, 15},
        {5, 20}, {10, 20}, {15, 20},
        {5, 25}, {10, 25}, {15, 25}
    };

③. 完整代码

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

// 定义一个结构体来存储点的坐标
struct Point2D {
    float x;
    float y;
};

// 计算仿射变换矩阵
cv::Mat calculateAffineTransform(const std::vector<Point2D>& imagePoints, const std::vector<Point2D>& robotPoints,int points_num) {
    cv::Mat src(points_num, 2, CV_64F);
    cv::Mat dst(points_num, 2, CV_64F);
    for (int i = 0; i < points_num; ++i) {
        src.at<double>(i, 0) = imagePoints[i].x;
        src.at<double>(i, 1) = imagePoints[i].y;
        dst.at<double>(i, 0) = robotPoints[i].x;
        dst.at<double>(i, 1) = robotPoints[i].y;
    }
    cv::Mat transformMatrix = cv::estimateAffine2D(src, dst);
    return transformMatrix;
}

// 将图像坐标点通过仿射变换矩阵转换为机器人坐标点
// 机械臂坐标 = 仿射变换矩阵 * 图像坐标
Point2D transformPoint(const cv::Mat& transformMatrix, const Point2D& imagePoint) {
    cv::Mat src(3, 1, CV_64F);
    src.at<double>(0, 0) = imagePoint.x;
    src.at<double>(1, 0) = imagePoint.y;
    src.at<double>(2, 0) = 1.0;
    cv::Mat dst = transformMatrix * src;
    Point2D robotPoint;
    robotPoint.x = dst.at<double>(0, 0);
    robotPoint.y = dst.at<double>(1, 0);
    return robotPoint;
}


int main() {
    int w = 3;
    int h = 5;
    cv::TermCriteria criteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001);

    std::vector<Point2D> imagePoints;

    // 读取图像
    cv::Mat image = cv::imread("35_1.jpg");
    if (image.empty()) {
        std::cerr << "Could not open or find the image" << std::endl;
        return -1;
    }

    // 将图像转换为灰度图
    cv::Mat gray;
    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

    // 设置 SimpleBlobDetector 参数
    cv::SimpleBlobDetector::Params params;
    params.maxArea = std::numeric_limits<float>::max();
    params.minArea = 2;
    params.minDistBetweenBlobs = 1;

    // 创建 SimpleBlobDetector 对象
    cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(params);

    // 存储角点
    std::vector<cv::Point2f> corners;
    bool found = cv::findCirclesGrid(gray, cv::Size(w, h), corners, cv::CALIB_CB_SYMMETRIC_GRID, blobDetector);

    if (found) {
        imagePoints.resize(corners.size());

        for (int i = 0; i < corners.size(); i++)
        {
            std::cout << "corner: " << corners[i] << std::endl;
            imagePoints.at(i).x = corners[i].x;
            imagePoints.at(i).y = corners[i].y;
        }
        std::cout << corners.size() << std::endl;
        // 优化角点位置
        cv::cornerSubPix(gray, corners, cv::Size(w, h), cv::Size(-1, -1), criteria);
        // 绘制角点
        cv::drawChessboardCorners(image, cv::Size(w, h), cv::Mat(corners), found);
        cv::namedWindow("findCorners", cv::WINDOW_NORMAL);
        cv::imshow("findCorners", image);
        cv::waitKey(0);
        cv::destroyAllWindows();
    }
    else {
        std::cerr << "Could not find the circle grid" << std::endl;
    }

    for (auto point : imagePoints)
    {
        std::cout << "x: " << point.x << " y: " << point.y << std::endl;
    }

    // 机器人坐标系中的点的坐标
    std::vector<Point2D> robotPoints = {
        {5, 5}, {10, 5}, {15, 5},
        {5, 10}, {10, 10}, {15, 10},
        {5, 15}, {10, 15}, {15, 15},
        {5, 20}, {10, 20}, {15, 20},
        {5, 25}, {10, 25}, {15, 25}
    };


    // 计算仿射变换矩阵
    cv::Mat affineMatrix = calculateAffineTransform(imagePoints, robotPoints, w * h);
    std::cout << "Affine Transformation Matrix: " << std::endl << affineMatrix << std::endl;

    // 测试一个图像坐标点的变换
    std::cout << "Testing a testImagePoint transformation to robot point..." << std::endl;
    //Point2D testImagePoint = { 250, 250 };
    Point2D testImagePoint1 = { 590.203, 885.33 };
    Point2D testRobotPoint1 = transformPoint(affineMatrix, testImagePoint1);
    std::cout << "Image Point1 (" << testImagePoint1.x << ", " << testImagePoint1.y << ") transforms to Robot2 Point ("
        << testRobotPoint1.x << ", " << testRobotPoint1.y << ")" << std::endl;

    /*
    //变换矩阵affineMatrix会存在不可逆情况
    Point2D testRobotPoint2 = { 10, 10 };
    Point2D testImagePoint2 = transformPoint(affineMatrix.inv(), testRobotPoint2);
    std::cout << "Robot Point2 (" << testRobotPoint2.x << ", " << testRobotPoint2.y << ") transforms to Image2 Point ("
        << testImagePoint2.x << ", " << testImagePoint2.y << ")" << std::endl;
    */

    return 0;
}

④. 运行效果

得到仿射变换矩阵$R$为
$$R=\left[\begin{array}{ccc}0.02821525305435079& -5.038386495271226e-08& -1.652816243402001\\ 3.693734340049677e-08& 0.02822203196237374& 0.01414373627149304\end{array}\right]$$
图像中的点$(590.203,885.33)$，也就是最后一个点，对应我们输入的机械臂的最后一个位置，可以映射到机械臂位置为$(15,25)$，可以看到最终实际的求解效果为$(14.9999,25)$，还可以

$$\{\begin{array}{l}Poin{t}_{robot}=R\cdot Poin{t}_{img}\\ Poin{t}_{img}=R^{-1}\cdot Poin{t}_{robot}\end{array}$$

2，3D相机手眼标定——OpenCV

2D相机手眼标定常用九点标定法，其只能得到X和Y的位置关系，无法的机械臂姿态信息，毕竟2D相机和3D相机的成本有很大差距

2D相机手眼标定只需要拍照一张照片+机械臂的9个点位即可，但只能得到X和Y信息，Z轴信息是固定的
而3D相机的手眼标定可以求解机械臂的姿态，但需要多组拍摄标定板进行组成AX=XB多组方程求解

OpenCV提供了求解方法，核心函数是solvePnP和cv::calibrateHandEye

①操作流程

Ⅰ.准备工作

1.1 标定板——使用棋盘格（如 9x6 内角点），尺寸需已知（单位：米或毫米
1.2 相机内参——提前完成相机标定，获取 camera_matrix 和 dist_coeffs
具体代码之前写的博文有，可参考：（一）相机标定——四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

Ⅱ.数据采集

2.1 固定标定板——将标定板固定在工作区域内，确保机械臂运动时相机始终可观测到它
2.2 移动机械臂——控制机械臂移动至 不同位姿（需包含旋转和平移），每个位姿下：

• 记录机械臂末端的 XYZWPR（基座坐标系下）
• 拍摄标定板图像。

2.3 数据量——至少 10组 数据，位姿需多样化（建议20组以上）

Ⅲ.数据预处理

3.1 机械臂位姿转换——将 XYZWPR 转换为 旋转矩阵R_base2gripper和 平移向量t_base2gripper
3.2 标定板检测——对每张图像检测棋盘格角点，使用 solvePnP计算相机到标定板的位姿R_target2cam和t_target2cam

Ⅳ.眼标定计算

调用OpenCV的calibrateHandEye 函数，输入机械臂和相机的位姿数据，求解变换矩阵X

②具体实现

Ⅰ.准备标定板参数，这里使用opencv自带的棋盘格

路径为opencv\sources\samples\data下

Ⅱ.获取相机的内参以及畸变系数

具体代码之前写的博文有，可参考：（一）相机标定——四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

求解得到相机的内参矩阵camera_matrix和透镜畸变系数dist_coeffs

Ⅲ.采集机械臂和相机的数据

①收集机械臂姿态数据（xyzwpr）
机械臂夹取标定板在相机视野下运动9个位姿，相机拍摄9组数据，当然数据越多，切姿态越丰富标定效果越好
机械臂的位姿通过示教器读取并转换为矩阵形式
例如fanuc示教器上的读数是：xyzwpr，w表示Rx，p表示Ry，r表示Rz
Fanuc 机器人示教器上的 XYZWPR是基于固定轴的位姿表示，其中：
X, Y, Z：末端执行器在基座坐标系中的平移分量（单位：毫米或米）。
W, P, R：分别表示绕 X、Y、Z 轴 的旋转角度（单位：度），即 Roll-Pitch-Yaw 顺序（但按 Z-Y-X 轴顺序 组合旋转）
假设我们以及移动了9组位置并进行记录机械臂位置存放到std::vector<std::vector<double>> xyzwpr_data中
函数convertXYZWPRToMat化将xyzwpr转化为RT矩阵

②收集相机拍摄机械臂所夹持的棋盘格图像
为例方便演示，这里我采用的是opencv自带的棋盘格图像数据
路径为opencv\sources\samples\data下

Ⅳ.检测棋盘格位姿

之前第一篇博文以及介绍，这里就不再赘述，涉及的方法都是一样的
（一）相机标定——四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）
findChessboardCorners找标定板的角点
cornerSubPix亚像素优化角点
solvePnP计算相机到标定板的位姿

Ⅴ.手眼标定

手眼标定核心函数calibrateHandEye，输入机械臂和相机的位姿数据，输出旋转X_rot矩阵和平移X_trans向量，然后通过copyTo拼接成变换矩阵（4x4）即可
calibrateHandEye 参数：
R_base2gripper, t_base2gripper：机械臂末端在基座坐标系下的位姿。
R_target2cam,t_target2cam：标定板在相机坐标系下的位姿。
X_rot, X_trans：输出的相机到基座的旋转矩阵和平移向量。
flags：标定算法选择（推荐 CALIB_HAND_EYE_TSAI）

③完整代码

需要修改的地方：

Ⅰ.实际你使用的棋盘格单个尺寸和内角点数量

const float square_size = 0.025f; // 棋盘格单格尺寸(m)
const cv::Size board_size(9, 6); // 棋盘格内角点数量

Ⅱ.相机内参和畸变系数

cv::Mat camera_matrix, dist_coeffs;
可以参考博文：（一）相机标定——四大坐标系的介绍、对应转换、畸变原理以及OpenCV完整代码实战（C++版）

Ⅲ.多组从示教器读取的机械臂位姿

std::vector<std::vector<double>> xyzwpr_data

Ⅳ.机械臂运动时，相机所拍摄的标定板图片路径

std::string image_path = "D:/opencv_4.7/opencv/sources/samples/data/right0" + std::to_string(i+1) + ".jpg";

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>
#include <vector>
#include <cmath>
#include <Windows.h>
#include <iostream>
// 将 Fanuc 的 XYZWPR 转换为旋转矩阵 R 和平移向量 t
void convertXYZWPRToMat(double X, double Y, double Z, double W, double P, double R,
    cv::Mat& R_base2grip, cv::Mat& t_base2grip) {
    // 平移向量
    t_base2grip = (cv::Mat_<double>(3, 1) << X, Y, Z);

    // 角度转弧度
    W *= CV_PI / 180.0;
    P *= CV_PI / 180.0;
    R *= CV_PI / 180.0;

    // 绕Z轴的旋转矩阵
    cv::Mat Rz = (cv::Mat_<double>(3, 3) <<
        cos(W), -sin(W), 0,
        sin(W), cos(W), 0,
        0, 0, 1);

    // 绕Y轴的旋转矩阵
    cv::Mat Ry = (cv::Mat_<double>(3, 3) <<
        cos(P), 0, sin(P),
        0, 1, 0,
        -sin(P), 0, cos(P));

    // 绕X轴的旋转矩阵
    cv::Mat Rx = (cv::Mat_<double>(3, 3) <<
        1, 0, 0,
        0, cos(R), -sin(R),
        0, sin(R), cos(R));

    // 组合旋转矩阵
    R_base2grip = Rz * Ry * Rx;
}

int main() {
    // --------------- 1. 准备标定板参数 ---------------
    const float square_size = 0.025f; // 棋盘格单格尺寸(m)
    const cv::Size board_size(9, 6);  // 棋盘格内角点数量
    std::vector<cv::Point3f> object_points;
    for (int i = 0; i < board_size.height; ++i) {
        for (int j = 0; j < board_size.width; ++j) {
            object_points.emplace_back(j * square_size, i * square_size, 0);
        }
    }

    // --------------- 2. 读取相机内参和畸变系数 ---------------
    cv::Mat camera_matrix, dist_coeffs;
    // 假设已通过相机标定获得参数
    camera_matrix = (cv::Mat_<double>(3, 3) <<
        542.3547430629291, 0, 328.3241889680722,
        0, 541.614996071113, 246.9472922233208,
        0, 0, 1);

    dist_coeffs = (cv::Mat_<double>(5, 1) << -0.2805430825289494, 0.1043237314547243, -0.0005582136242986065, 0.001303557702627711, -0.02372163114377487);

    // --------------- 3. 采集数据 ---------------
    // 示例数据：每组数据是一个包含6个元素的 std::vector<double>
    std::vector<std::vector<double>> xyzwpr_data = {
        {100.0, 200.0, 300.0, 45.0, 30.0, 15.0},
        {150.0, 250.0, 350.0, 60.0, 45.0, 30.0},
        {200.0, 300.0, 400.0, 75.0, 60.0, 45.0},
        {250.0, 350.0, 450.0, 90.0, 75.0, 60.0},
        {300.0, 400.0, 500.0, 105.0, 90.0, 75.0},
        {350.0, 450.0, 550.0, 120.0, 105.0, 90.0},
        {400.0, 500.0, 600.0, 135.0, 120.0, 105.0},
        {450.0, 550.0, 650.0, 150.0, 135.0, 120.0},
        {500.0, 600.0, 700.0, 165.0, 150.0, 135.0}
    };

    std::vector<cv::Mat> R_base2gripper, t_base2gripper; // 机械臂位姿
    std::vector<cv::Mat> R_target2cam, t_target2cam;     // 相机检测标定板位姿

    // 模拟数据采集循环（实际需从机械臂和相机获取）
    for (int i = 0; i < 9; ++i) {
        // ------------------- 3.1 获取机械臂位姿 -------------------
        // 假设从Fanuc机器人读取基座到末端的变换矩阵
        cv::Mat R_base2grip = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat t_base2grip = (cv::Mat_<double>(3, 1) << 0.1 * i, 0, 0);

        convertXYZWPRToMat(xyzwpr_data[i].at(0), xyzwpr_data[i].at(1), xyzwpr_data[i].at(2), xyzwpr_data[i].at(3),
            xyzwpr_data[i].at(4), xyzwpr_data[i].at(5), R_base2grip, t_base2grip);

        R_base2gripper.push_back(R_base2grip.clone());
        t_base2gripper.push_back(t_base2grip.clone());

        // ------------------- 3.2 检测棋盘格位姿 -------------------
        // 假设从相机捕获图像
        std::string image_path = "D:/opencv_4.7/opencv/sources/samples/data/right0" + std::to_string(i+1) + ".jpg";
        cv::Mat image = cv::imread(image_path,-1);
        std::cout<<image.size()<<std::endl;

        std::vector<cv::Point2f> corners;
        bool found = cv::findChessboardCorners(image, board_size, corners);

        if (found) {
            // 亚像素优化角点
            cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1),
                cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));

            // 计算相机到标定板的位姿
            cv::Mat rvec, tvec;
            cv::solvePnP(object_points, corners, camera_matrix, dist_coeffs, rvec, tvec);

            // 转换为旋转矩阵
            cv::Mat R_target2cam_i;
            cv::Rodrigues(rvec, R_target2cam_i);

            R_target2cam.push_back(R_target2cam_i);
            t_target2cam.push_back(tvec);
        }
    }

    // --------------- 4. 手眼标定 ---------------
    cv::Mat X_rot, X_trans;
    cv::calibrateHandEye(
        R_base2gripper, t_base2gripper,
        R_target2cam, t_target2cam,
        X_rot, X_trans,
        cv::CALIB_HAND_EYE_TSAI
    );

    // --------------- 5. 输出结果 ---------------
    cv::Mat X = cv::Mat::eye(4, 4, CV_64F);
    X_rot.copyTo(X(cv::Rect(0, 0, 3, 3)));
    X_trans.copyTo(X(cv::Rect(3, 0, 1, 3)));

    std::cout << "相机到基座的变换矩阵 X = \n" << X << std::endl;

    return 0;
}

④运行效果

得到相机到基座的变换矩阵X，也就是AX = XB中的解