【opencv】教程代码 —features2D（7）根据单应性矩阵估计相机坐标系下的物体位姿...

十年一梦实验室

于 2024-03-31 15:12:26 发布

阅读量713

点赞数 4

文章标签： opencv 矩阵数码相机人工智能计算机视觉

本文链接：https://blog.csdn.net/cxyhjl/article/details/137214833

版权

pose_from_homography.cpp从图像中找到棋盘角点并进行姿态估计

从图像中找到棋盘角点并显示

计算角点在世界坐标系中的位置

读取相机内参和畸变系数并校正图像中的角点

计算从3D点到2D点的单应性矩阵

通过奇异值分解(SVD)优化对旋转矩阵的估计

基于单应矩阵分解及其优化结果，估计姿态，绘制世界坐标系

#include <iostream>                         // 包含标准输入输出头文件
#include <opencv2/core.hpp>                 // 包含OpenCV核心功能的头文件
#include <opencv2/imgproc.hpp>              // 包含OpenCV图像处理功能的头文件
#include <opencv2/calib3d.hpp>              // 包含OpenCV摄像机标定和3D重建的头文件
#include <opencv2/highgui.hpp>              // 包含OpenCV图像显示功能的头文件


using namespace std;                        // 使用命名空间std，避免每次使用标准库时都要加std::
using namespace cv;                         // 使用命名空间cv，避免每次调用OpenCV函数时都要加cv::


namespace                                   // 匿名命名空间，内部的变量和函数仅在本文件内有效
{
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; // 枚举类型Pattern，代表不同的标定板类型


// 计算棋盘角点的函数
void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
{
    corners.resize(0);                      // 清空角点向量


    switch (patternType)                    // 根据不同的标定板类型进行计算
    {
    case CHESSBOARD:
    case CIRCLES_GRID:
        //! [compute-chessboard-object-points]
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float(j*squareSize), // 将每个角点的3D位置按行和列添加到corners向量中
                                          float(i*squareSize), 0));
        //! [compute-chessboard-object-points]
        break;


    case ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float((2*j + i % 2)*squareSize), // 对于非对称圆形网格，计算并存储3D位置
                                          float(i*squareSize), 0));
        break;


    default:
        CV_Error(Error::StsBadArg, "Unknown pattern type\n"); // 如果不是上述类型，默认抛出异常
    }
}


// 从共面点估计姿态
void poseEstimationFromCoplanarPoints(const string &imgPath, const string &intrinsicsPath, const Size &patternSize,
                                             const float squareSize)
{
    Mat img = imread( samples::findFile( imgPath) );         // 读取图像文件
    Mat img_corners = img.clone(), img_pose = img.clone();   // 克隆图像用于角点检测和姿态估计的显示


    //! [find-chessboard-corners]
    vector<Point2f> corners;                                 // 存储2D角点的容器
    bool found = findChessboardCorners(img, patternSize, corners); // 使用OpenCV函数查找棋盘角点
    //! [find-chessboard-corners]


    if (!found)                                              // 如果没有找到角点
    {
        cout << "Cannot find chessboard corners." << endl;   // 打印错误信息
        return;                                              // 直接返回
    }
    drawChessboardCorners(img_corners, patternSize, corners, found); // 在图像上绘制检测到的角点
    imshow("Chessboard corners detection", img_corners);     // 显示带有角点的图像


    //! [compute-object-points]
    vector<Point3f> objectPoints;                            // 存储3D角点的容器
     // 使用自定义函数计算3D角点的位置
    calcChessboardCorners(patternSize, squareSize, objectPoints);
    vector<Point2f> objectPointsPlanar; // 用于存储投影后的2D点                     
    for (size_t i = 0; i < objectPoints.size(); i++)
    {
        objectPointsPlanar.push_back(Point2f(objectPoints[i].x, objectPoints[i].y)); // 把3D点投影到2D平面
    }
    //! [compute-object-points]


    //! [load-intrinsics]
    FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ); // 打开摄像机内参数文件
    Mat cameraMatrix, distCoeffs;                           // 定义摄像机的内参数矩阵和畸变系数
    fs["camera_matrix"] >> cameraMatrix;                    // 从文件中读取摄像机矩阵
    fs["distortion_coefficients"] >> distCoeffs;            // 从文件中读取畸变系数
    //! [load-intrinsics]


    //! [compute-image-points]
    vector<Point2f> imagePoints;  // 存储畸变矫正后的2D点的容器
    undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs); // 畸变矫正
    //! [compute-image-points]


    //! // 计算从3D点到2D点的单应性矩阵 [estimate-homography]
    Mat H = findHomography(objectPointsPlanar, imagePoints);
    cout << "H:\n" << H << endl; // 打印出单应性矩阵
    //! [estimate-homography]


    //! [pose-from-homography]
    // 归一化确保||c1|| = 1 // 计算归一化的因子
    double norm = sqrt(H.at<double>(0,0)*H.at<double>(0,0) +  
                       H.at<double>(1,0)*H.at<double>(1,0) +
                       H.at<double>(2,0)*H.at<double>(2,0));


    H /= norm;    // 归一化单应性矩阵
    Mat c1  = H.col(0);   // 获取单应性矩阵的第一列
    Mat c2  = H.col(1);     // 获取单应性矩阵的第二列
    Mat c3 = c1.cross(c2);   // 计算第三个轴的方向


    Mat tvec = H.col(2);  // 获取平移向量
    Mat R(3, 3, CV_64F);    // 定义旋转矩阵


    for (int i = 0; i < 3; i++)
    {    // 分别设置旋转矩阵R的列
        R.at<double>(i,0) = c1.at<double>(i,0);            
        R.at<double>(i,1) = c2.at<double>(i,0);
        R.at<double>(i,2) = c3.at<double>(i,0);
    }
    //! [pose-from-homography]


    //!旋转矩阵的极分解 [polar-decomposition-of-the-rotation-matrix]
    cout << "R (before polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl; // 打印未分解的旋转矩阵和其行列式
    Mat_<double> W, U, Vt;    // 定义用于奇异值分解的矩阵
    SVDecomp(R, W, U, Vt);   // 奇异值分解
    R = U*Vt;       // 通过U和Vt的乘积获得旋转矩阵R
    double det = determinant(R);   // 计算R的行列式
    if (det < 0)    // 如果行列式小于0，调整Vt矩阵再重新计算R
    {
        Vt.at<double>(2,0) *= -1;
        Vt.at<double>(2,1) *= -1;
        Vt.at<double>(2,2) *= -1;


        R = U*Vt;
    }
    // 打印分解后的旋转矩阵和其行列式
    cout << "R (after polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl; 
    //! [polar-decomposition-of-the-rotation-matrix]


    //! [display-pose]
    Mat rvec;                                               // 定义旋转向量
    Rodrigues(R, rvec);                                     // 将旋转矩阵转换为旋转向量
    drawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize); // 在图像上绘制坐标轴
    imshow("Pose from coplanar points", img_pose);          // 显示带有姿态的图像
    waitKey();                                              // 等待用户按键
    //! [display-pose]
}


const char* params
    = "{ help h         |       | print usage }"             // 命令行参数的初始化和解析
      "{ image          | left04.jpg | path to a chessboard image }"
      "{ intrinsics     | left_intrinsics.yml | path to camera intrinsics }"
      "{ width bw       | 9     | chessboard width }"
      "{ height bh      | 6     | chessboard height }"
      "{ square_size    | 0.025 | chessboard square size }";
}


int main(int argc, char *argv[])                            // 主函数
{
    CommandLineParser parser(argc, argv, params);           // 初始化命令行解析器


    if (parser.has("help"))                                 // 如果有帮助信息请求
    {
        parser.about("Code for homography tutorial.\n"
            "Example 1: pose from homography with coplanar points.\n"); // 打印该程序的简介
        parser.printMessage();                              // 打印帮助信息
        return 0;
    }


    Size patternSize(parser.get<int>("width"), parser.get<int>("height")); // 获取棋盘的尺寸
    float squareSize = (float) parser.get<double>("square_size");          // 获取棋盘的方格大小
    poseEstimationFromCoplanarPoints(parser.get<String>("image"),          // 调用姿态估计函数
                                     parser.get<String>("intrinsics"),
                                     patternSize, squareSize);


    return 0;
}

此代码段的功能是利用OpenCV库从图像中找到棋盘角点并进行姿态估计。它包括以下主要部分：

使用摄像头拍摄的含有标定板（棋盘）的图像来找到角点；
使用棋盘角点，并结合已知的棋盘格尺寸来计算这些角点在空间中的位置；
根据摄像头的内参数文件来读取摄像头内参数并校正图像畸变；
计算2D图像点与3D空间点的单应性矩阵；
通过奇异值分解(SVD)优化对旋转矩阵的估计；
基于单应矩阵分解及其优化结果，估计姿态，并在图像上显示检测到的棋盘角点和计算出的姿态；

该程序适用于计算机视觉和机器人视觉领域中，特别是在相机校准和空间位置估计场景中。通过对摄像头捕获的物体进行准确的姿态估计，可以实现机器人的精准定位和路径规划。

终端输出：

H:
[2.933817142664291, -0.03262345741700864, -0.2971965462459956;
 -0.0447851314919854, 3.002917156419064, -0.203610190903598;
 -0.7132714521558404, -0.3361256089579286, 0.9999999999999999]
R (before polar decomposition):
[0.9715880815225828, -0.01080386433887985, 0.2365582243054619;
 -0.0148314287738872, 0.9944718355305832, 0.1107036064935512;
 -0.2362130998987821, -0.1113142434165035, 0.9660567460669759]
det(R): 1.00148
R (after polar decomposition):
[0.9715941332941491, -0.01130419642660607, 0.2363832804822285;
 -0.0153517308169317, 0.9937440091258498, 0.110621736957108;
 -0.2361549586803454, -0.1111083231339211, 0.9653423102822399]
det(R): 1

drawChessboardCorners(img_corners, patternSize, corners, found);

SVDecomp(R, W, U, Vt);

findHomography

H /= norm;

Mat tvec = H.col(2);

十年一梦实验室

关注

4
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
【opencv】教程代码 —features2D（7）根据单应性矩阵估计相机坐标系下的物体位姿...

pose_from_homography.cpp从图像中找到棋盘角点并进行姿态估计从图像中找到棋盘角点并显示计算角点在世界坐标系中的位置读取相机内参和畸变系数并校正图像中的角点计算从3D点到2D点的单应性矩阵通过奇异值分解(SVD)优化对旋转矩阵的估计基于单应矩阵分解及其优化结果，估计姿态，绘制世界坐标系#include <iostream> ...
复制链接

扫一扫