samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp

一个带有单应性分解的示例程序。

查看 相应的教程 了解更多详情。

#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
 
using namespace std;
using namespace 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, "未知的模式类型\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 << "错误，无法在两张图像中找到棋盘角点。" << endl;
        return;
    }
 
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;
 
    Mat rvec1, tvec1;
solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);
    Mat rvec2, tvec2;
solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
 
    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);
 
    Mat normal = (Mat_<double>(3,1) << 0, 0, 1);
    Mat normal1 = R1*normal;
 
    Mat origin(3, 1, CV_64F, Scalar(0));
    Mat origin1 = R1*origin + tvec1;
    double d_inv1 = 1.0 / normal1.dot(origin1);
 
    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);
 
vector<Mat> Rs_decomp, ts_decomp, normals_decomp;
    int solutions = decomposeHomographyMat(homography, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
cout << "分解从相机位移计算得到的单应性矩阵:" << 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 << "解 " << i << ":" << endl;
cout << "来自单应性分解的 rvec: " << rvec_decomp.t() << endl;
cout << "来自相机位移的 rvec: " << rvec_1to2.t() << endl;
cout << "来自单应性分解的 tvec: " << ts_decomp[i].t() << " 并按 d 缩放: " << factor_d1 * ts_decomp[i].t() << endl;
cout << "来自相机位移的 tvec: " << t_1to2.t() << endl;
cout << "来自单应性分解的平面法线: " << normals_decomp[i].t() << endl;
cout << "相机 1 姿态下的平面法线: " << normal1.t() << endl << endl;
    }
 
    Mat H = findHomography(corners1, corners2);
 
solutions = decomposeHomographyMat(H, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
cout << "分解由 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 << "解 " << i << ":" << endl;
cout << "来自单应性分解的 rvec: " << rvec_decomp.t() << endl;
cout << "来自相机位移的 rvec: " << rvec_1to2.t() << endl;
cout << "来自单应性分解的 tvec: " << ts_decomp[i].t() << " 并按 d 缩放: " << factor_d1 * ts_decomp[i].t() << endl;
cout << "来自相机位移的 tvec: " << t_1to2.t() << endl;
cout << "来自单应性分解的平面法线: " << normals_decomp[i].t() << endl;
cout << "相机 1 姿态下的平面法线: " << normal1.t() << endl << endl;
    }
}
 
const char* params
= "{ help h | | 打印用法 }"
      "{ image1 | left02.jpg | 源棋盘图像路径 }"
      "{ image2 | left01.jpg | 目标棋盘图像路径 }"
      "{ intrinsics | left_intrinsics.yml | 相机内参路径 }"
      "{ width bw | 9 | 棋盘宽度 }"
      "{ height bh | 6 | 棋盘高度 }"
      "{ square_size | 0.025 | 棋盘方格大小 }";
}
 
int main(int argc, char *argv[])
{
    CommandLineParser parser(argc, argv, params);
 
    if ( parser.has("help") )
    {
parser.about( "单应性教程代码。\n"
                      "示例 4: 分解单应性矩阵。\n" );
parser.printMessage();
        return 0;
    }
 
    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;
}