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

Posted 2023-04-08 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把旋转向量转化为旋转矩阵，而平移向量和平移矩阵一致，直接打印即可。

相机姿态估计：如何解释旋转和平移矩阵？

【中文标题】相机姿态估计：如何解释旋转和平移矩阵？

【英文标题】：Camera pose estimation: How do I interpret rotation and translation matrices?

【发布时间】：2015-07-16 06:49:13

【问题描述】：

假设我在两个图像之间有良好的对应关系，并尝试恢复它们之间的相机运动。 我可以为此使用 OpenCV 3 的新功能，如下所示：

 Mat E = findEssentialMat(imgpts1, imgpts2, focal, principalPoint, RANSAC, 0.999, 1, mask);

 int inliers = recoverPose(E, imgpts1, imgpts2, R, t, focal, principalPoint, mask);

 Mat mtxR, mtxQ;
 Mat Qx, Qy, Qz;
 Vec3d angles = RQDecomp3x3(R, mtxR, mtxQ, Qx, Qy, Qz);

 cout << "Translation: " << t.t() << endl;
 cout << "Euler angles [x y z] in degrees: " << angles.t() << endl;

现在，我很难理解 R 和 t 的实际含义。它们是否是将坐标从相机空间 1 映射到相机空间 2 所需的变换，如p_2 = R * p_1 + t？

考虑这个例子，手动标记对应的真实情况

我得到的输出是这样的：

Translation: [-0.9661243151855488, -0.04921320381132761, 0.253341406362796]
Euler angles [x y z] in degrees: [9.780449804801876, 46.49315494782735, 15.66510133665445]

我尝试将其与我在图像中看到的内容相匹配并提出解释，[-0.96,-0.04,0.25] 告诉我，我已向右移动，因为坐标沿负 x 轴移动，但它还会告诉我，我已经移动得更远了，因为坐标已经沿着正 z 轴移动。

我还围绕 y 轴旋转了相机（向左，我认为这将是围绕负 y 轴逆时针旋转，因为在 OpenCV 中，y 轴指向下方，不是吗？ )

问题：我的解释是否正确，如果不正确，正确的是什么？

【参考方案1】：

其实你的解释是正确的。

首先，关于 y 轴的方向，您是正确的。有关 OpenCV 的相机坐标系的说明，请参阅here。

您的代码会将第二个摄像头的 R 和 t 返回到第一个摄像头。这意味着如果 x1 是第一个图像中的一个点并且 x2 是第二个图像中的一个点，则以下等式成立 x1 = R*x2 + t。 现在，在您的情况下，右侧图像（前视图）来自摄像头 1，而汽车的左侧图像（侧视图）来自摄像头 2。

看看这个等式，我们首先看到的是应用了旋转。因此，图像您的相机当前拍摄左帧。现在您的 R 指定绕 y 轴旋转约 46 度。由于角度 alpha 的旋转点与将坐标轴反向旋转该角度相同，因此您的 R 告诉您向左旋转。正如您自己指出的那样，如果查看图片，这似乎是正确的。由于围绕其他轴的旋转很小且难以成像，因此我们在此省略它们。 因此，在应用旋转之后，您仍然站在拍摄左框架的相同位置，但您的相机或多或少指向汽车后部或汽车正后方的空间。

现在让我们看看平移向量。您对向右移动和更远移动的解释也是正确的。让我试着解释一下原因。想象一下，从您当前的位置开始，使用新的相机方向，您只能向右移动。您会直接撞到汽车或需要将相机放在发动机罩上方。因此，向右移动后，您还需要进一步移动才能到达您拍摄正确照片的位置。

我希望这个解释能帮助你想象你的 R 和 t 描述的运动。

【参考方案2】：

事实证明我的解释是正确的，关系p2 = R * p1 + t 确实成立。可以通过使用cv::triangulatePoints() 和cv::convertPointsFromHomogeneous 从对应点（相对于摄像机 1）获取 3D 坐标，然后应用上述等式来验证这一点。与相机 2 的相机矩阵相乘，然后得到 p2 图像坐标。

【参考方案3】：

你的解释听起来对我来说是正确的。我不是 100% 关于 OpenCV 中轴的方向，但我相信你对 Y 轴的看法是正确的。

输出也很有意义，不仅从代码的角度来看，而且如果您查看这两个图像，您可以大致想象完整 90 度旋转将指向的位置（它基本上是相同的角度，但在相反的一侧汽车）

这也是通过刚体运动机制对该概念的一个相当不错的解释： http://nghiaho.com/?page_id=671

【参考方案4】：

让我们看看。 OpenCV 相机坐标系为“X 朝向图像右侧，Y 朝向图像底部，Z = X x Y 朝向场景”。 Q=[R|t]是从camera2到camera1的坐标变换，所以t是以camera1为根，以camera2为顶点的向量，以camera1帧表示。因此，您的平移向量意味着camera2位于camera 1的左侧，根据您的图像，只有当汽车的侧视图在camera2中并且汽车的前视图在camera 1中时才有可能。这与正Z分量一致翻译，因为在侧视图中，汽车看起来离相机更远。

此标识也与您计算的欧拉角一致：它们在 OpenGL 约定中返回，从而表示从源到目标的旋转。在您的情况下，围绕camera1的垂直轴旋转46度，逆时针w.r.t。向下的 Y 轴为您带来您所拥有的侧视图。

【讨论】：

但是正 z 分量会告诉我 camera2 已经向汽车移动，但这不符合 camera2 是左侧图像的解释。我错过了什么？

重新阅读我写的内容：“t 是以 camera1 为根的向量，尖端在 camera2”。 t 的正 Z 分量仅表示 camera2 的中心在 camera1 的 XY 平面前面，即在 camera1 的中心前面。考虑到您的图像，这完全合理：只需想象在查看汽车前部的视图中扩展图像平面，然后决定侧视图的相机应该在该平面的哪一侧。另外，如果您觉得有用，请点赞/接受答案。

如果我这样做，在我看来，相机 2 的中心在相机 1 的后面，所以 z 应该是负数。但由于轮换，不是很清楚。