526互联

OpenCV获取相机旋转矩阵和平移矩阵

发布时间 2023-04-07 23:10:38作者: XnobodyT

想要求解旋转矩阵和平移矩阵，先要了解相机内参矩阵和畸变矩阵如何获取，不了解的可以先移步https://www.cnblogs.com/nobodyx/p/17297074.html

先上代码

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

int main() {
    // 使用 glob 库读取棋盘格图片
    std::vector<cv::String> filenames;
    cv::glob("left*.jpg", filenames, false);

    // 初始化棋盘格的行列数和尺寸（单位：mm）
    cv::Size board_size(9, 6);
    float square_size = 20.f;

    // 存储棋盘格图像上所有角点坐标
    std::vector<std::vector<cv::Point3f>> object_points;
    std::vector<std::vector<cv::Point2f>> image_points;

    // 遍历所有棋盘格图片，检测角点并存储角点坐标
    for (const auto &filename : filenames) {
        cv::Mat image = cv::imread(filename, cv::IMREAD_GRAYSCALE);
        std::vector<cv::Point2f> corners;
        bool ret = cv::findChessboardCorners(image, board_size, corners);
        if (ret) {
            cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
            std::vector<cv::Point3f> object_point;
            for (int i = 0; i < board_size.height; ++i) {
                for (int j = 0; j < board_size.width; ++j) {
                    object_point.emplace_back(float(j * square_size), float(i * square_size), 0);
                }
            }
            object_points.emplace_back(object_point);
            image_points.emplace_back(corners);
        } else {
            std::cerr << "Failed to detect corners in image: " << filename << std::endl;
        }
    }

    // 计算相机内参矩阵和畸变矩阵
    cv::Mat camera_matrix, dist_coeffs;
    std::vector<cv::Mat> rvecs, tvecs;
    double rms = cv::calibrateCamera(object_points, image_points, cv::Size(1920, 1080), camera_matrix, dist_coeffs, rvecs, tvecs);

    // 打印相机内参矩阵和畸变矩阵
    std::cout << "Camera matrix:" << std::endl << camera_matrix << std::endl;
    std::cout << "Distortion coefficients:" << std::endl << dist_coeffs << std::endl;

    // 计算相机的旋转矩阵和平移矩阵
    cv::Mat rotation_matrix, translation_matrix;
    cv::solvePnP(object_points[0], image_points[0], camera_matrix, dist_coeffs, rotation_matrix, translation_matrix);

    // 打印相机的旋转矩阵和平移矩阵
    std::cout << "Rotation matrix:" << std::endl << rotation_matrix << std::endl;
    std::cout << "Translation matrix:" << std::endl << translation_matrix << std::endl;

    std::cout << std::endl;

    return 0;
}

头一次没有使用using namespace, 有点费劲

这里有必要解释一下一个关键的函数--solvePnP

cv::solvePnP(object_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector, use_extrinsic_guess, flags);
  • object_points: std::vectorcv::Point3f 类型，存储 3D 物体坐标点的坐标值；
  • image_points: std::vectorcv::Point2f 类型，存储对应的 2D 图像坐标点的坐标值；
  • camera_matrix: cv::Mat 类型，相机的内参矩阵；
  • dist_coeffs: cv::Mat 类型，相机的畸变矩阵；
  • rotation_vector: cv::Mat 类型，函数返回的旋转向量，包含相机的旋转信息；
  • translation_vector: cv::Mat 类型，函数返回的平移向量，包含相机的平移信息；
  • use_extrinsic_guess: bool 类型，表示是否使用函数提供的外参估计结果作为初始值；
  • flags: int 类型，表示求解过程的选项，可选项包括 SOLVEPNP_ITERATIVE、SOLVEPNP_P3P、SOLVEPNP_EPNP 和 SOLVEPNP_DLS。

其中，rotation_vector 和 translation_vector 分别是 Rodrigues 变换和欧拉旋转的等效表示方式。可以通过 cv::Rodrigues(rotation_vector, rotation_matrix) 函数将旋转向量转换为旋转矩阵，或者通过 cv::composeRT(rotation_vector, translation_vector, camera_rotation, camera_translation, camera_rotation_matrix, camera_translation_matrix) 函数将旋转向量和平移向量组合成变换矩阵。

最后说一下思路

（1）求内参矩阵和畸变矩阵

// 使用 glob 库读取棋盘格图片
    std::vector<cv::String> filenames;
    cv::glob("left*.jpg", filenames, false);

    // 初始化棋盘格的行列数和尺寸（单位：mm）
    cv::Size board_size(9, 6);
    float square_size = 20.f;

    // 存储棋盘格图像上所有角点坐标
    std::vector<std::vector<cv::Point3f>> object_points;
    std::vector<std::vector<cv::Point2f>> image_points;

    // 遍历所有棋盘格图片，检测角点并存储角点坐标
    for (const auto &filename : filenames) {
        cv::Mat image = cv::imread(filename, cv::IMREAD_GRAYSCALE);
        std::vector<cv::Point2f> corners;
        bool ret = cv::findChessboardCorners(image, board_size, corners);
        if (ret) {
            cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
            std::vector<cv::Point3f> object_point;
            for (int i = 0; i < board_size.height; ++i) {
                for (int j = 0; j < board_size.width; ++j) {
                    object_point.emplace_back(float(j * square_size), float(i * square_size), 0);
                }
            }
            object_points.emplace_back(object_point);
            image_points.emplace_back(corners);
        } else {
            std::cerr << "Failed to detect corners in image: " << filename << std::endl;
        }
    }

    // 计算相机内参矩阵和畸变矩阵
    cv::Mat camera_matrix, dist_coeffs;
    std::vector<cv::Mat> rvecs, tvecs;
    double rms = cv::calibrateCamera(object_points, image_points, cv::Size(1920, 1080), camera_matrix, dist_coeffs, rvecs, tvecs);

    // 打印相机内参矩阵和畸变矩阵
    std::cout << "Camera matrix:" << std::endl << camera_matrix << std::endl;
    std::cout << "Distortion coefficients:" << std::endl << dist_coeffs << std::endl;

这个我不过多解释，不懂的可以看一下我上一篇博客

（2）求解旋转矩阵和平移矩阵

    // 计算相机的旋转矩阵和平移矩阵
    cv::Mat rotation_matrix, translation_matrix,R;
    cv::solvePnP(object_points[0], image_points[0], camera_matrix, dist_coeffs, R, translation_matrix );

    cv::Rodrigues(R, rotation_matrix);

    // 打印相机的旋转矩阵和平移矩阵
    std::cout << "Rotation matrix:" << std::endl << rotation_matrix << std::endl;
    std::cout << "Translation matrix:" << std::endl << translation_matrix << std::endl;

    std::cout << std::endl;

过程比较简单，就是用solvePnP获得相机旋转向量和平移向量，再利用Rodrigues把旋转向量转化为旋转矩阵，而平移向量和平移矩阵一致，直接打印即可。