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

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