手眼标定求解标定板到相机的平移和旋转向量（C++）

目录

（1）OpenCV的solvePnP代码使用参数说明：

（2）代码运行效果演示：

（3）（C++代码）

---

（1）OpenCV的solvePnP代码使用参数说明：

本次代码可以直接求出标定板到相机的转换矩阵,并且会将结果分别保存在一个txt文件中，求出结果搭配好我前期发的手眼标定代码，即可直接快速精确的求解出手眼矩阵。

参数：
objectPoints：世界坐标系（上图中OwXwYwZw）下的3D点坐标数组
imagePoints：图像（上图中ouv）中对应3D点的成像点坐标数组
cameraMatrix：相机内参矩阵，3×3
distCoeffs：相机畸变系数数组，可以为NULL，此时视为无畸变（此处通常为1X4的矩阵）。
rvec和tvec：计算结果输出，rvec为旋转向量，tvec为平移向量，两者合并表达的是物体整体（即世界坐标系）在相机坐标系中的位姿

（2）代码运行效果演示：

（3）（C++代码）

#include <iostream>
#include <vector>
//  
/// COPYRIGHT NOTICE  
/// Copyright (c) 2023, 合肥工业大学Robotic Group  （版权声明）  
/// All rights reserved.  
///   
/// @file    SolvePnP.cpp    
/// @brief   矩阵转换类的实现文件  
///  
/// 本文件为求解标定板到相机平移向量和旋转向量的实现代码  
///  
/// @version 1.1     
/// @author  Yahooo!!!    
/// @E-mail：  
/// @date      
///   
///  
///  修订说明:修改部分BUG
// 
#include <filesystem>
#include <opencv2/opencv.hpp>
#include <fstream>

namespace fs = std::filesystem;
// 函数：将Mat对象保存到文本文件
void saveMatToFile(const cv::Mat& mat, const std::string& filename) {
    std::ofstream file(filename, std::ios::app); // 以追加模式打开
    if (file.is_open()) {
        file << filename << ": ";
        for (int r = 0; r < mat.rows; ++r) {
            for (int c = 0; c < mat.cols; ++c) {
                file << mat.at<double>(r, c) << (c == mat.cols - 1 ? "\n" : ", ");
            }
        }
        file.close();
    }
    else {
        std::cerr << "无法打开文件 " << filename << std::endl;
    }
}

int main() {
    std::string folderPath = "D:\\11.28"; // 替换为您的图像文件夹路径
    cv::Mat cameraMatrix; // 替换为你的相机内参矩阵

    cameraMatrix = (cv::Mat_<double>(3, 3) << 1307.66080795052,	0	,619.463669957549,
        0	,1307.72738908587,	533.245905117153,
        0,	0,	1);
    cv::Mat distCoeffs = (cv::Mat_<double>(1, 4) << -0.0559872733031574,	0.0643028456963296, 0,0 );
    std::vector<std::string> fileNames;
    // 棋盘格参数
    int board_width = 11;  // 替换为你的棋盘格宽度上的角点数
    int board_height = 8; // 替换为你的棋盘格高度上的角点数
    float square_size = 10.0f; // 替换为你的棋盘格每个方格的大小

    cv::Size boardSize(board_width, board_height);
    std::vector<cv::Point3f> objp;
    for (int i = 0; i < board_height; ++i)
        for (int j = 0; j < board_width; ++j)
            objp.push_back(cv::Point3f(j * square_size, i * square_size, 0));

    for (const auto& entry : fs::directory_iterator(folderPath)) {
        if (entry.is_regular_file()) {
            std::string filePath = entry.path().string();
            cv::Mat image = cv::imread(filePath);
            if (!image.empty()) {
                std::vector<cv::Point2f> imagePoints;
                bool found = cv::findChessboardCorners(image, boardSize, imagePoints);
                if (found) {
                    
                    cv::Mat imageCopy = image.clone();
                    cv::drawChessboardCorners(imageCopy, boardSize, imagePoints, found);
                    std::cout << "Processing image: " << filePath << std::endl;
                    cv::imshow("Chessboard Corners", imageCopy);
                    cv::waitKey(500); // 等待500毫秒

                    cv::Mat grayImage;
                    cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY);
                    cv::cornerSubPix(grayImage, imagePoints, 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(objp, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
                    std::cout << "Image: " << filePath << std::endl;
                    std::cout << "Rotation Vector:\n" << rvec << std::endl;
                    std::cout << "Translation Vector:\n" << tvec << std::endl;

                    // 保存结果到文件
                    saveMatToFile(rvec, "rotation_vectors.txt");
                    saveMatToFile(tvec, "translation_vectors.txt");
                }
            }
        }
    }

    cv::destroyAllWindows();
    return 0;
}