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

   1 #include <iostream>
   2 #include <opencv2/opencv_modules.hpp>
   3 #ifdef HAVE_OPENCV_ARUCO
   4 #include <opencv2/opencv.hpp>
   5 #include <opencv2/aruco.hpp>
   6
   7 using namespace std;
   8 using namespace cv;
   9
  10 namespace
  11 {
  12 enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
  13
  14 void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
  15 {
  16     corners.resize(0);
  17
  18     switch (patternType) {
  19     case CHESSBOARD:
  20     case CIRCLES_GRID:
  21         for( int i = 0; i < boardSize.height; i++ )
  22             for( int j = 0; j < boardSize.width; j++ )
  23                 corners.push_back(Point3f(float(j*squareSize),
  24                                           float(i*squareSize), 0));
  25         break;
  26
  27     case ASYMMETRIC_CIRCLES_GRID:
  28         for( int i = 0; i < boardSize.height; i++ )
  29             for( int j = 0; j < boardSize.width; j++ )
  30                 corners.push_back(Point3f(float((2*j + i % 2)*squareSize),
  31                                           float(i*squareSize), 0));
  32         break;
  33
  34     default:
  35         CV_Error(Error::StsBadArg, "Unknown pattern type\n");
  36     }
  37 }
  38
  39 Mat computeHomography(const Mat &R_1to2, const Mat &tvec_1to2, const double d_inv, const Mat &normal)
  40 {
  41     Mat homography = R_1to2 + d_inv * tvec_1to2*normal.t();
  42     return homography;
  43 }
  44
  45 void computeC2MC1(const Mat &R1, const Mat &tvec1, const Mat &R2, const Mat &tvec2,
  46                   Mat &R_1to2, Mat &tvec_1to2)
  47 {
  48     //c2Mc1 = c2Mo * oMc1 = c2Mo * c1Mo.inv()
  49     R_1to2 = R2 * R1.t();
  50     tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2;
  51 }
  52
  53 void decomposeHomography(const string &img1Path, const string &img2Path, const Size &patternSize,
  54                          const float squareSize, const string &intrinsicsPath)
  55 {
  56     Mat img1 = imread(img1Path);
  57     Mat img2 = imread(img2Path);
  58
  59     vector<Point2f> corners1, corners2;
  60     bool found1 = findChessboardCorners(img1, patternSize, corners1);
  61     bool found2 = findChessboardCorners(img2, patternSize, corners2);
  62
  63     if (!found1 || !found2)
  64     {
  65         cout << "Error, cannot find the chessboard corners in both images." << endl;
  66         return;
  67     }
  68
  69     //! [compute-poses]
  70     vector<Point3f> objectPoints;
  71     calcChessboardCorners(patternSize, squareSize, objectPoints);
  72
  73     FileStorage fs(intrinsicsPath, FileStorage::READ);
  74     Mat cameraMatrix, distCoeffs;
  75     fs["camera_matrix"] >> cameraMatrix;
  76     fs["distortion_coefficients"] >> distCoeffs;
  77
  78     Mat rvec1, tvec1;
  79     solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);
  80     Mat rvec2, tvec2;
  81     solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
  82     //! [compute-poses]
  83
  84     //! [compute-camera-displacement]
  85     Mat R1, R2;
  86     Rodrigues(rvec1, R1);
  87     Rodrigues(rvec2, R2);
  88
  89     Mat R_1to2, t_1to2;
  90     computeC2MC1(R1, tvec1, R2, tvec2, R_1to2, t_1to2);
  91     Mat rvec_1to2;
  92     Rodrigues(R_1to2, rvec_1to2);
  93     //! [compute-camera-displacement]
  94
  95     //! [compute-plane-normal-at-camera-pose-1]
  96     Mat normal = (Mat_<double>(3,1) << 0, 0, 1);
  97     Mat normal1 = R1*normal;
  98     //! [compute-plane-normal-at-camera-pose-1]
  99
 100     //! [compute-plane-distance-to-the-camera-frame-1]
 101     Mat origin(3, 1, CV_64F, Scalar(0));
 102     Mat origin1 = R1*origin + tvec1;
 103     double d_inv1 = 1.0 / normal1.dot(origin1);
 104     //! [compute-plane-distance-to-the-camera-frame-1]
 105
 106     //! [compute-homography-from-camera-displacement]
 107     Mat homography_euclidean = computeHomography(R_1to2, t_1to2, d_inv1, normal1);
 108     Mat homography = cameraMatrix * homography_euclidean * cameraMatrix.inv();
 109
 110     homography /= homography.at<double>(2,2);
 111     homography_euclidean /= homography_euclidean.at<double>(2,2);
 112     //! [compute-homography-from-camera-displacement]
 113
 114     //! [decompose-homography-from-camera-displacement]
 115     vector<Mat> Rs_decomp, ts_decomp, normals_decomp;
 116     int solutions = decomposeHomographyMat(homography, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
 117     cout << "Decompose homography matrix computed from the camera displacement:" << endl << endl;
 118     for (int i = 0; i < solutions; i++)
 119     {
 120       double factor_d1 = 1.0 / d_inv1;
 121       Mat rvec_decomp;
 122       Rodrigues(Rs_decomp[i], rvec_decomp);
 123       cout << "Solution " << i << ":" << endl;
 124       cout << "rvec from homography decomposition: " << rvec_decomp.t() << endl;
 125       cout << "rvec from camera displacement: " << rvec_1to2.t() << endl;
 126       cout << "tvec from homography decomposition: " << ts_decomp[i].t() << " and scaled by d: " << factor_d1 * ts_decomp[i].t() << endl;
 127       cout << "tvec from camera displacement: " << t_1to2.t() << endl;
 128       cout << "plane normal from homography decomposition: " << normals_decomp[i].t() << endl;
 129       cout << "plane normal at camera 1 pose: " << normal1.t() << endl << endl;
 130     }
 131     //! [decompose-homography-from-camera-displacement]
 132
 133     //! [estimate homography]
 134     Mat H = findHomography(corners1, corners2);
 135     //! [estimate homography]
 136
 137     //! [decompose-homography-estimated-by-findHomography]
 138     solutions = decomposeHomographyMat(H, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
 139     cout << "Decompose homography matrix estimated by findHomography():" << endl << endl;
 140     for (int i = 0; i < solutions; i++)
 141     {
 142       double factor_d1 = 1.0 / d_inv1;
 143       Mat rvec_decomp;
 144       Rodrigues(Rs_decomp[i], rvec_decomp);
 145       cout << "Solution " << i << ":" << endl;
 146       cout << "rvec from homography decomposition: " << rvec_decomp.t() << endl;
 147       cout << "rvec from camera displacement: " << rvec_1to2.t() << endl;
 148       cout << "tvec from homography decomposition: " << ts_decomp[i].t() << " and scaled by d: " << factor_d1 * ts_decomp[i].t() << endl;
 149       cout << "tvec from camera displacement: " << t_1to2.t() << endl;
 150       cout << "plane normal from homography decomposition: " << normals_decomp[i].t() << endl;
 151       cout << "plane normal at camera 1 pose: " << normal1.t() << endl << endl;
 152     }
 153     //! [decompose-homography-estimated-by-findHomography]
 154 }
 155
 156 const char* params
 157     = "{ help h         |       | print usage }"
 158       "{ image1         | ../data/left02.jpg | path to the source chessboard image }"
 159       "{ image2         | ../data/left01.jpg | path to the desired chessboard image }"
 160       "{ intrinsics     | ../data/left_intrinsics.yml | path to camera intrinsics }"
 161       "{ width bw       | 9     | chessboard width }"
 162       "{ height bh      | 6     | chessboard height }"
 163       "{ square_size    | 0.025 | chessboard square size }";
 164 }
 165
 166 int main(int argc, char *argv[])
 167 {
 168     CommandLineParser parser(argc, argv, params);
 169
 170     if ( parser.has("help") )
 171     {
 172         parser.about( "Code for homography tutorial.\n"
 173                       "Example 4: decompose the homography matrix.\n" );
 174         parser.printMessage();
 175         return 0;
 176     }
 177
 178     Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
 179     float squareSize = (float) parser.get<double>("square_size");
 180     decomposeHomography(parser.get<String>("image1"),
 181                         parser.get<String>("image2"),
 182                         patternSize, squareSize,
 183                         parser.get<String>("intrinsics"));
 184
 185     return 0;
 186 }
 187 #else
 188 int main()
 189 {
 190     std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
 191     return 0;
 192 }
 193 #endif