全自动手眼标定c++代码

下面是一个简单的全自动手眼标定的C++代码示例：

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

using namespace std;
using namespace cv;

int main() {
    // 读取标定板图像
    vector<String> filenames;
    glob("calibration_images/*.jpg", filenames);
    vector<vector<Point2f>> image_points;
    Size chessboard_size(9, 6);
    vector<Point3f> object_points;
    for (size_t i = 0; i < filenames.size(); ++i) {
        Mat img = imread(filenames[i]);
        Mat gray;
        cvtColor(img, gray, COLOR_BGR2GRAY);
        vector<Point2f> corners;
        bool found = findChessboardCorners(gray, chessboard_size, corners,
                                            CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
        if (found) {
            image_points.push_back(corners);
            object_points.push_back(Point3f(i / chessboard_size.width, i % chessboard_size.width, 0));
            drawChessboardCorners(img, chessboard_size, corners, found);
            imshow("img", img);
            waitKey(500);
        }
    }
    destroyAllWindows();

    // 相机标定
    Mat camera_matrix, dist_coeffs;
    vector<Mat> rvecs, tvecs;
    calibrateCamera(object_points, image_points, Size(640, 480), camera_matrix, dist_coeffs, rvecs, tvecs);

    // 手眼标定
    Mat H;
    int n = rvecs.size();
    for (int i = 0; i < n; ++i) {
        Mat R;
        Rodrigues(rvecs[i], R);
        Mat rvec, tvec;
        rvec = R.t();
        tvec = -R.t() * tvecs[i];
        hconcat(rvec, tvec, H);
    }
    Mat A, B;
    A = Mat::zeros(3 * n, 3, CV_64FC1);
    B = Mat::zeros(3 * n, 1, CV_64FC1);
    for (int i = 0; i < n; ++i) {
        Mat H_i = H.rowRange(3 * i, 3 * i + 3);
        Mat A_i = A.rowRange(3 * i, 3 * i + 3);
        Mat B_i = B.rowRange(3 * i, 3 * i + 3);
        A_i = H_i.colRange(0, 3);
        B_i = -H_i.col(3);
    }
    Mat X;
    solve(A, B, X);

    // 输出结果
    Mat T = X.rowRange(0, 3);
    Mat R = X.rowRange(3, 6);
    cout << "T = " << T << endl;
    cout << "R = " << R << endl;
    return 0;
}

这段代码首先读取标定板图像，然后使用 `findChessboardCorners` 函数提取图像中的角点，将角点坐标和标定板上的物理坐标对应起来，最后使用 `calibrateCamera` 函数计算相机的内部参数矩阵和畸变系数。接着，使用 `Rodrigues` 函数将旋转向量转换为旋转矩阵，并使用 `hconcat` 函数将旋转矩阵和平移向量拼接成 $3\times4$ 的变换矩阵。然后，将变换矩阵按行排列成 $3n\times3$ 的矩阵 $A$，将平移向量按行排列成 $3n\times1$ 的矩阵 $B$，并使用 `solve` 函数求解 $AX=B$ 得到手眼标定结果。最后，输出平移向量和旋转矩阵。