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

decompose_homography.cpp 分解单应性矩阵

left01.jpg boardSize:9x6 squareSize:0.025

left02.jpg

相机内参

cpp 复制代码
#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函数估计两个视图间的单应性矩阵,并分解之。 代码的最终目的是从两幅图像计算出相机的位移,并从位移中分解出单应性矩阵,这通常用于场景的三维重构和姿态估计。

终端输出:

properties 复制代码
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);

相关推荐
封步宇AIGC20 分钟前
量化交易系统开发-实时行情自动化交易-Okex K线数据
人工智能·python·机器学习·数据挖掘
封步宇AIGC22 分钟前
量化交易系统开发-实时行情自动化交易-Okex交易数据
人工智能·python·机器学习·数据挖掘
z千鑫24 分钟前
【人工智能】利用大语言模型(LLM)实现机器学习模型选择与实验的自动化
人工智能·gpt·机器学习·语言模型·自然语言处理·自动化·codemoss
shelly聊AI26 分钟前
AI赋能财务管理,AI技术助力企业自动化处理财务数据
人工智能·财务管理
波点兔27 分钟前
【部署glm4】属性找不到、参数错误问题解决(思路:修改模型包版本)
人工智能·python·机器学习·本地部署大模型·chatglm4
佚明zj1 小时前
全卷积和全连接
人工智能·深度学习
程序小旭3 小时前
机器视觉基础—双目相机
计算机视觉·双目相机
qzhqbb4 小时前
基于统计方法的语言模型
人工智能·语言模型·easyui
冷眼看人间恩怨4 小时前
【话题讨论】AI大模型重塑软件开发:定义、应用、优势与挑战
人工智能·ai编程·软件开发
2401_883041084 小时前
新锐品牌电商代运营公司都有哪些?
大数据·人工智能