【opencv】教程代码 —features2D（3）Homography—分解单应性矩阵

decompose_homography.cpp 分解单应性矩阵

left01.jpg  boardSize：9x6  squareSize:0.025

left02.jpg

相机内参

#include <iostream> // 引入输入输出流库
#include <opencv2/core.hpp> // 引入OpenCV的核心功能头文件
#include <opencv2/highgui.hpp> // 引入OpenCV的GUI功能头文件
#include <opencv2/calib3d.hpp> // 引入OpenCV的相机标定和三维重建功能头文件


using namespace std; // 使用标准命名空间，例如std::vector可以省略std::
using namespace cv; // 使用opencv命名空间，例如cv::Mat可以省略cv::


namespace // 匿名命名空间
{
// 定义了几种标定板的模式的枚举类型
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };


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


    // 根据不同的标定板类型计算角点
    switch (patternType) {
    case CHESSBOARD:
    case CIRCLES_GRID:
        // 对于棋盘格和圆点网格，遍历每行每列生成角点
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f(float(j*squareSize),
                                          float(i*squareSize), 0));
        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),
                                          float(i*squareSize), 0));
        break;


    // 如果模式类型未知则报错
    default:
        CV_Error(Error::StsBadArg, "Unknown pattern type\n");
    }
}


// 计算单应性矩阵的函数，基于旋转矩阵、平移向量、平面到相机的逆距离和平面的法向量
Mat computeHomography(const Mat &R_1to2, const Mat &tvec_1to2, const double d_inv, const Mat &normal)
{
    Mat homography = R_1to2 + d_inv * tvec_1to2*normal.t(); // 计算单应性矩阵
    return homography; // 返回计算出的单应性矩阵
}


// 根据两次相机位姿计算两个相机之间的位姿关系
void computeC2MC1(const Mat &R1, const Mat &tvec1, const Mat &R2, const Mat &tvec2,
                  Mat &R_1to2, Mat &tvec_1to2)
{
    //c2Mc1 = c2Mo * oMc1 = c2Mo * c1Mo.inv()
    R_1to2 = R2 * R1.t(); // 计算旋转矩阵
    tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2; // 计算平移向量
}


// 主函数，使用两个图像、棋盘格大小、方块大小和内参路径进行标定板的单应性分解
void decomposeHomography(const string &img1Path, const string &img2Path, const Size &patternSize,
                         const float squareSize, const string &intrinsicsPath)
{
    // 根据路径读取两张图像
    Mat img1 = imread( samples::findFile( img1Path) );
    Mat img2 = imread( samples::findFile( img2Path) );


    // 查找两幅图像中的角点
    vector<Point2f> corners1, corners2;
    bool found1 = findChessboardCorners(img1, patternSize, corners1);
    bool found2 = findChessboardCorners(img2, patternSize, corners2);


    // 如果任一图像无法找到角点则返回错误
    if (!found1 || !found2)
    {
        cout << "Error, cannot find the chessboard corners in both images." << endl;
        return;
    }


    //! [compute-poses]
    vector<Point3f> objectPoints; // 存储物体在世界坐标系中的点
    // 计算棋盘格角点的世界坐标
    calcChessboardCorners(patternSize, squareSize, objectPoints); 


    // 读取相机的内参
    FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ);
    Mat cameraMatrix, distCoeffs;
    fs["camera_matrix"] >> cameraMatrix; // 相机矩阵
    fs["distortion_coefficients"] >> distCoeffs; // 畸变系数


    // 对两个图像使用solvePnP求解相机位姿
    // 使用solvePnP来估计两幅图片的相机外参即旋转向量和平移向量
    Mat rvec1, tvec1; // 旋转向量和平移向量
    solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);
    Mat rvec2, tvec2;
    solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
    //! [compute-poses]


    //! [compute-camera-displacement]
    Mat R1, R2;
    Rodrigues(rvec1, R1); // 旋转向量转换成旋转矩阵
    Rodrigues(rvec2, R2);


    // 计算两个相机位姿之间的相对旋转和平移
    Mat R_1to2, t_1to2;
    computeC2MC1(R1, tvec1, R2, tvec2, R_1to2, t_1to2);
    Mat rvec_1to2;
    Rodrigues(R_1to2, rvec_1to2); // 计算得到的旋转矩阵再转换成旋转向量
    //! [compute-camera-displacement]


    //! [compute-plane-normal-at-camera-pose-1]
    Mat normal = (Mat_<double>(3,1) << 0, 0, 1); // 定义一个垂直于棋盘格表面的法向量
    Mat normal1 = R1*normal; //相机1坐标轴z在世界坐标系的表示// 使用相机1的旋转矩阵变换法向量到相机坐标系下
    //! [compute-plane-normal-at-camera-pose-1]


    //! [compute-plane-distance-to-the-camera-frame-1]
    // 计算棋盘格平面到相机坐标系原点的距离
    Mat origin(3, 1, CV_64F, Scalar(0)); // 定义原点
    Mat origin1 = R1*origin + tvec1; // 相机坐标系1原点在世界坐标系下的表示
    //相机1坐标系原点到标定板平面的距离的逆
    double d_inv1 = 1.0 / normal1.dot(origin1); // 计算平面到相机1原点的逆距离
    //! [compute-plane-distance-to-the-camera-frame-1]


    //! [compute-homography-from-camera-displacement]
    // 根据相机位姿差异计算单应性矩阵
    Mat homography_euclidean = computeHomography(R_1to2, t_1to2, d_inv1, normal1);
    Mat homography =cameraMatrix * homography_euclidean * cameraMatrix.inv();
    // 根据欧几里得单应性矩阵计算实际的单应性矩阵
    // 内参解极约束
    homography /= homography.at<double>(2,2);
    homography_euclidean /= homography_euclidean.at<double>(2,2);
    //! [compute-homography-from-camera-displacement]


    //! [decompose-homography-from-camera-displacement]
    // 分解从相机位移计算得到的单应性矩阵
    vector<Mat> Rs_decomp, ts_decomp, normals_decomp;
    // 使用OpenCV函数decomposeHomographyMat进行分解
    int solutions = decomposeHomographyMat(homography, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
    // 显示分解得到的解决方案数和每个解决方案
    cout << "Decompose homography matrix computed from the camera displacement:" << endl << endl;
    for (int i = 0; i < solutions; i++)
    {
      double factor_d1 = 1.0 / d_inv1;
      Mat rvec_decomp;
      Rodrigues(Rs_decomp[i], rvec_decomp); // 将旋转矩阵转换成旋转向量
      cout << "Solution " << i << ":" << endl;
      cout << "rvec from homography decomposition: " << rvec_decomp.t() << endl;
      cout << "rvec from camera displacement: " << rvec_1to2.t() << endl;
      cout << "tvec from homography decomposition: " << ts_decomp[i].t() << " and scaled by d: " << factor_d1 * ts_decomp[i].t() << endl;
      cout << "tvec from camera displacement: " << t_1to2.t() << endl;
      cout << "plane normal from homography decomposition: " << normals_decomp[i].t() << endl;
      cout << "plane normal at camera 1 pose: " << normal1.t() << endl << endl;
    }
    //! [decompose-homography-from-camera-displacement]


    //! [estimate homography]
    // 使用findHomography()估计两幅图像间点的单应性矩阵
    Mat H = findHomography(corners1, corners2);
    //! [estimate homography]


    //! [decompose-homography-estimated-by-findHomography]
    // 分解由findHomography()函数估计得到的单应性矩阵
    solutions = decomposeHomographyMat(H, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
    cout << "Decompose homography matrix estimated by findHomography():" << endl << endl;
    for (int i = 0; i < solutions; i++)
    {
      double factor_d1 = 1.0 / d_inv1;
      Mat rvec_decomp;
      Rodrigues(Rs_decomp[i], rvec_decomp); // 将旋转矩阵转换成旋转向量
      cout << "Solution " << i << ":" << endl;
      cout << "rvec from homography decomposition: " << rvec_decomp.t() << endl;
      cout << "rvec from camera displacement: " << rvec_1to2.t() << endl;
      cout << "tvec from homography decomposition: " << ts_decomp[i].t() << " and scaled by d: " << factor_d1 * ts_decomp[i].t() << endl;
      cout << "tvec from camera displacement: " << t_1to2.t() << endl;
      cout << "plane normal from homography decomposition: " << normals_decomp[i].t() << endl;
      cout << "plane normal at camera 1 pose: " << normal1.t() << endl << endl;
    }
    //! [decompose-homography-estimated-by-findHomography]
}


// 命令行参数解析中用到的参数模板字符串
const char* params
    = "{ help h         |       | print usage }"
      "{ image1         | left02.jpg | path to the source chessboard image }"
      "{ image2         | left01.jpg | path to the desired 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 }";
}


// main函数 - 程序入口点
int main(int argc, char *argv[])
{
    // 解析命令行输入的参数
    CommandLineParser parser(argc, argv, params);


    // 如果参数中包含help，则打印程序的帮助信息
    if ( parser.has("help") )
    {
        parser.about("Code for homography tutorial.\n"
                     "Example 4: decompose the homography matrix.\n");
        parser.printMessage();
        return 0;
    }


    // 获取棋盘格尺寸参数、方块大小并调用decomposeHomography函数
    Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
    float squareSize = (float) parser.get<double>("square_size");
    decomposeHomography(parser.get<String>("image1"),
                        parser.get<String>("image2"),
                        patternSize, squareSize,
                        parser.get<String>("intrinsics"));


    return 0; // 程序正常结束
}

代码的主要功能是通过两张棋盘格图像来计算它们之间的单应性矩阵，并对该矩阵进行分解以获得相对旋转向量、平移向量和正常向量。这个过程通常用于计算机视觉中相机的位姿估计和校准。代码使用了OpenCV的相关函数完成图像处理和矩阵运算。

decomposeHomography用于执行相机位移下单应性矩阵的分解与计算。整个代码流程包括：

1. 读取两幅图像并在棋盘格中找到角点。2. 从外部文件读取相机内参。3. 使用solvePnP函数估计两幅图像相对于世界坐标系的姿态。4. 计算相机1到相机2的旋转和平移。5. 计算参考平面的法线向量和到相机1的距离。6. 根据相机间的位移计算出单应性矩阵。7. 使用decomposeHomographyMat函数分解由相机位移计算出的单应性矩阵。8. 通过findHomography函数估计两个视图间的单应性矩阵，并分解之。 代码的最终目的是从两幅图像计算出相机的位移，并从位移中分解出单应性矩阵，这通常用于场景的三维重构和姿态估计。

终端输出：

Decompose homography matrix computed from the camera displacement:


Solution 0:
rvec from homography decomposition: [-0.09198300601684746, -0.5372581107203847, 1.310868858823169]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [-0.7747960935447347, -0.02751122265362334, -0.6791979966698382] and scaled by d: [-0.1578091502009057, -0.005603439026252055, -0.1383378924669762]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [-0.1973513031094161, 0.6283452092297845, -0.7524857215914424]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 1:
rvec from homography decomposition: [-0.09198300601684746, -0.5372581107203847, 1.310868858823169]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [0.7747960935447347, 0.02751122265362334, 0.6791979966698382] and scaled by d: [0.1578091502009057, 0.005603439026252055, 0.1383378924669762]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [0.1973513031094161, -0.6283452092297845, 0.7524857215914424]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 2:
rvec from homography decomposition: [0.1053487844994422, -0.1561929302268701, 1.40135654775989]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [-0.4666552458868745, 0.105003306671971, -0.9130076449288979] and scaled by d: [-0.09504754657871854, 0.02138689486465782, -0.1859598438525694]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [-0.3131715302799941, 0.8421206250806385, -0.4390403687998199]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 3:
rvec from homography decomposition: [0.1053487844994422, -0.1561929302268701, 1.40135654775989]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [0.4666552458868745, -0.105003306671971, 0.9130076449288979] and scaled by d: [0.09504754657871854, -0.02138689486465782, 0.1859598438525694]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [0.3131715302799941, -0.8421206250806385, 0.4390403687998199]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Decompose homography matrix estimated by findHomography():


Solution 0:
rvec from homography decomposition: [0.1552207660862356, -0.1521327237748819, 1.323678685010914]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [-0.4482361641899351, 0.02485247517069353, -1.034409633494862] and scaled by d: [-0.09129597935439666, 0.005061909862158862, -0.2106867943469173]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [-0.1384902519298367, 0.9063331412907092, -0.3992251083267753]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 1:
rvec from homography decomposition: [0.1552207660862356, -0.1521327237748819, 1.323678685010914]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [0.4482361641899351, -0.02485247517069353, 1.034409633494862] and scaled by d: [0.09129597935439666, -0.005061909862158862, 0.2106867943469173]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [0.1384902519298367, -0.9063331412907092, 0.3992251083267753]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 2:
rvec from homography decomposition: [-0.2886605520980661, -0.5210498746612311, 1.381242036724107]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [-0.8705960844288273, 0.1353018264406045, -0.7037701811073137] and scaled by d: [-0.1773215293631533, 0.02755804582536839, -0.1433427131894416]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [-0.2284582164830118, 0.6009247378618285, -0.7659610321335505]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]


Solution 3:
rvec from homography decomposition: [-0.2886605520980661, -0.5210498746612311, 1.381242036724107]
rvec from camera displacement: [-0.09198300601684731, -0.5372581107203847, 1.310868858823169]
tvec from homography decomposition: [0.8705960844288273, -0.1353018264406045, 0.7037701811073137] and scaled by d: [0.1773215293631533, -0.02755804582536839, 0.1433427131894416]
tvec from camera displacement: [0.1578091502009057, 0.005603439026252086, 0.1383378924669763]
plane normal from homography decomposition: [0.2284582164830118, -0.6009247378618285, 0.7659610321335505]
plane normal at camera 1 pose: [0.1973513031094159, -0.6283452092297841, 0.7524857215914428]

如何计算相机间的转换矩阵？

如何使用solvePnP函数进行相机标定？

Mat homography_euclidean = computeHomography(R_1to2, t_1to2, d_inv1, normal1);

int solutions = decomposeHomographyMat(homography, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);