Merge pull request #10304 from catree:add_tutorial_homography

doc/opencv.bib

@@ -554,6 +554,13 @@
   pages = {135--144},
   publisher = {Springer}
 }
+@book{Ma:2003:IVI,
+ author = {Ma, Yi and Soatto, Stefano and Kosecka, Jana and Sastry, S. Shankar},
+ title = {An Invitation to 3-D Vision: From Images to Geometric Models},
+ year = {2003},
+ isbn = {0387008934},
+ publisher = {SpringerVerlag},
+}
 @ARTICLE{Malis,
   author = {Malis, Ezio and Vargas, Manuel and others},
   title = {Deeper understanding of the homography decomposition for vision-based control},

doc/tutorials/features2d/table_of_content_features2d.markdown

@@ -93,3 +93,10 @@ OpenCV.
     *Author:* Fedor Morozov
 
     Using *AKAZE* and *ORB* for planar object tracking.
+
+-   @subpage tutorial_homography
+
+    *Compatibility:* \> OpenCV 3.0
+
+    This tutorial will explain the basic concepts of the homography with some
+    demonstration codes.

samples/cpp/tutorial_code/features2D/Homography/left_intrinsics.yml

+%YAML:1.0
+---
+image_width: 640
+image_height: 480
+board_width: 9
+board_height: 6
+square_size: 1.
+aspectRatio: 1.
+flags: 2
+camera_matrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 5.3591575307485539e+02, 0., 3.4228314953752817e+02, 0.,
+       5.3591575307485539e+02, 2.3557082321320789e+02, 0., 0., 1. ]
+distortion_coefficients: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [ -2.6637290673868386e-01, -3.8586722644459073e-02,
+       1.7831841406179300e-03, -2.8122035403651473e-04,
+       2.3838760574917545e-01 ]
+avg_reprojection_error: 3.9259109564815858e-01

samples/cpp/tutorial_code/features2D/Homography/tutorial_homography_ex1_pose_from_homography.cpp

+#include <iostream>
+#include <opencv2/opencv_modules.hpp>
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.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:
+        //! [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),
+                                          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),
+                                          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(imgPath);
+    Mat img_corners = img.clone(), img_pose = img.clone();
+
+    //! [find-chessboard-corners]
+    vector<Point2f> corners;
+    bool found = findChessboardCorners(img, patternSize, corners);
+    //! [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;
+    calcChessboardCorners(patternSize, squareSize, objectPoints);
+    vector<Point2f> objectPointsPlanar;
+    for (size_t i = 0; i < objectPoints.size(); i++)
+    {
+        objectPointsPlanar.push_back(Point2f(objectPoints[i].x, objectPoints[i].y));
+    }
+    //! [compute-object-points]
+
+    //! [load-intrinsics]
+    FileStorage fs(intrinsicsPath, FileStorage::READ);
+    Mat cameraMatrix, distCoeffs;
+    fs["camera_matrix"] >> cameraMatrix;
+    fs["distortion_coefficients"] >> distCoeffs;
+    //! [load-intrinsics]
+
+    //! [compute-image-points]
+    vector<Point2f> imagePoints;
+    undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs);
+    //! [compute-image-points]
+
+    //! [estimate-homography]
+    Mat H = findHomography(objectPointsPlanar, imagePoints);
+    cout << "H:\n" << H << endl;
+    //! [estimate-homography]
+
+    //! [pose-from-homography]
+    // Normalization to ensure that ||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.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]
+
+    //! [display-pose]
+    Mat rvec;
+    Rodrigues(R, rvec);
+    aruco::drawAxis(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
+    imshow("Pose from coplanar points", img_pose);
+    waitKey();
+    //! [display-pose]
+}
+
+const char* about = "Code for homography tutorial.\n"
+                    "Example 1: pose from homography with coplanar points.\n";
+
+const char* params
+    = "{ h help         | false | print usage }"
+      "{ image          |       | path to a chessboard image (left04.jpg) }"
+      "{ intrinsics     |       | path to camera intrinsics (left_intrinsics.yml) }"
+      "{ width w        | 9     | chessboard width }"
+      "{ height h       | 6     | chessboard height }"
+      "{ square_size    | 0.025 | chessboard square size }";
+}
+
+int main(int argc, char *argv[])
+{
+    CommandLineParser parser(argc, argv, params);
+
+    if (parser.get<bool>("help"))
+    {
+        cout << about << endl;
+        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;
+}
+#else
+int main()
+{
+    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
+    return 0;
+}
+#endif

samples/cpp/tutorial_code/features2D/Homography/tutorial_homography_ex2_perspective_correction.cpp

+#include <iostream>
+#include <opencv2/opencv_modules.hpp>
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+using namespace std;
+using namespace cv;
+
+namespace
+{
+enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
+
+Scalar randomColor( RNG& rng )
+{
+  int icolor = (unsigned int) rng;
+  return Scalar( icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255 );
+}
+
+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");
+    }
+}
+
+void perspectiveCorrection(const string &img1Path, const string &img2Path, const Size &patternSize, RNG &rng)
+{
+    Mat img1 = imread(img1Path);
+    Mat img2 = imread(img2Path);
+
+    //! [find-corners]
+    vector<Point2f> corners1, corners2;
+    bool found1 = findChessboardCorners(img1, patternSize, corners1);
+    bool found2 = findChessboardCorners(img2, patternSize, corners2);
+    //! [find-corners]
+
+    if (!found1 || !found2)
+    {
+        cout << "Error, cannot find the chessboard corners in both images." << endl;
+        return;
+    }
+
+    //! [estimate-homography]
+    Mat H = findHomography(corners1, corners2);
+    cout << "H:\n" << H << endl;
+    //! [estimate-homography]
+
+    //! [warp-chessboard]
+    Mat img1_warp;
+    warpPerspective(img1, img1_warp, H, img1.size());
+    //! [warp-chessboard]
+
+    Mat img_draw_warp;
+    hconcat(img2, img1_warp, img_draw_warp);
+    imshow("Desired chessboard view / Warped source chessboard view", img_draw_warp);
+
+    //! [compute-transformed-corners]
+    Mat img_draw_matches;
+    hconcat(img1, img2, img_draw_matches);
+    for (size_t i = 0; i < corners1.size(); i++)
+    {
+        Mat pt1 = (Mat_<double>(3,1) << corners1[i].x, corners1[i].y, 1);
+        Mat pt2 = H * pt1;
+        pt2 /= pt2.at<double>(2);
+
+        Point end( (int) (img1.cols + pt2.at<double>(0)), (int) pt2.at<double>(1) );
+        line(img_draw_matches, corners1[i], end, randomColor(rng), 2);
+    }
+
+    imshow("Draw matches", img_draw_matches);
+    waitKey();
+    //! [compute-transformed-corners]
+}
+
+const char* about = "Code for homography tutorial.\n"
+                    "Example 2: perspective correction.\n";
+
+const char* params
+    = "{ h help         | false | print usage }"
+      "{ image1         |       | path to the source chessboard image (left02.jpg) }"
+      "{ image2         |       | path to the desired chessboard image (left01.jpg) }"
+      "{ width w        | 9     | chessboard width }"
+      "{ height h       | 6     | chessboard height }";
+}
+
+int main(int argc, char *argv[])
+{
+    cv::RNG rng( 0xFFFFFFFF );
+    CommandLineParser parser(argc, argv, params);
+
+    if (parser.get<bool>("help"))
+    {
+        cout << about << endl;
+        parser.printMessage();
+        return 0;
+    }
+
+    Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
+    perspectiveCorrection(parser.get<string>("image1"),
+                          parser.get<string>("image2"),
+                          patternSize, rng);
+
+    return 0;
+}
+#else
+int main()
+{
+    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
+    return 0;
+}
+#endif

samples/cpp/tutorial_code/features2D/Homography/tutorial_homography_ex3_homography_from_camera_displacement.cpp

+#include <iostream>
+#include <opencv2/opencv_modules.hpp>
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.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, "Unknown pattern type\n");
+    }
+}
+
+//! [compute-homography]
+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;
+}
+//! [compute-homography]
+
+Mat computeHomography(const Mat &R1, const Mat &tvec1, const Mat &R2, const Mat &tvec2,
+                      const double d_inv, const Mat &normal)
+{
+    Mat homography = R2 * R1.t() + d_inv * (-R2 * R1.t() * tvec1 + tvec2) * normal.t();
+    return homography;
+}
+
+//! [compute-c2Mc1]
+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;
+}
+//! [compute-c2Mc1]
+
+void homographyFromCameraDisplacement(const string &img1Path, const string &img2Path, const Size &patternSize,
+                                      const float squareSize, const string &intrinsicsPath)
+{
+    Mat img1 = imread(img1Path);
+    Mat img2 = imread(img2Path);
+
+    //! [compute-poses]
+    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;
+    }
+
+    vector<Point3f> objectPoints;
+    calcChessboardCorners(patternSize, squareSize, objectPoints);
+
+    FileStorage fs(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);
+    //! [compute-poses]
+
+    Mat img1_copy_pose = img1.clone(), img2_copy_pose = img2.clone();
+    Mat img_draw_poses;
+    aruco::drawAxis(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2*squareSize);
+    aruco::drawAxis(img2_copy_pose, cameraMatrix, distCoeffs, rvec2, tvec2, 2*squareSize);
+    hconcat(img1_copy_pose, img2_copy_pose, img_draw_poses);
+    imshow("Chessboard poses", img_draw_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;
+    //! [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;
+    double d_inv1 = 1.0 / normal1.dot(origin1);
+    //! [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]
+
+    //Same but using absolute camera poses instead of camera displacement, just for check
+    Mat homography_euclidean2 = computeHomography(R1, tvec1, R2, tvec2, d_inv1, normal1);
+    Mat homography2 = cameraMatrix * homography_euclidean2 * cameraMatrix.inv();
+
+    homography_euclidean2 /= homography_euclidean2.at<double>(2,2);
+    homography2 /= homography2.at<double>(2,2);
+
+    cout << "\nEuclidean Homography:\n" << homography_euclidean << endl;
+    cout << "Euclidean Homography 2:\n" << homography_euclidean2 << endl << endl;
+
+    //! [estimate-homography]
+    Mat H = findHomography(corners1, corners2);
+    cout << "\nfindHomography H:\n" << H << endl;
+    //! [estimate-homography]
+
+    cout << "homography from camera displacement:\n" << homography << endl;
+    cout << "homography from absolute camera poses:\n" << homography2 << endl << endl;
+
+    //! [warp-chessboard]
+    Mat img1_warp;
+    warpPerspective(img1, img1_warp, H, img1.size());
+    //! [warp-chessboard]
+
+    Mat img1_warp_custom;
+    warpPerspective(img1, img1_warp_custom, homography, img1.size());
+    imshow("Warped image using homography computed from camera displacement", img1_warp_custom);
+
+    Mat img_draw_compare;
+    hconcat(img1_warp, img1_warp_custom, img_draw_compare);
+    imshow("Warped images comparison", img_draw_compare);
+
+    Mat img1_warp_custom2;
+    warpPerspective(img1, img1_warp_custom2, homography2, img1.size());
+    imshow("Warped image using homography computed from absolute camera poses", img1_warp_custom2);
+
+    waitKey();
+}
+
+const char* about = "Code for homography tutorial.\n"
+                    "Example 3: homography from the camera displacement.\n";
+
+const char* params
+    = "{ h help         | false | print usage }"
+      "{ image1         |       | path to the source chessboard image (left02.jpg) }"
+      "{ image2         |       | path to the desired chessboard image (left01.jpg) }"
+      "{ intrinsics     |       | path to camera intrinsics (left_intrinsics.yml) }"
+      "{ width w        | 9     | chessboard width }"
+      "{ height h       | 6     | chessboard height }"
+      "{ square_size    | 0.025 | chessboard square size }";
+}
+
+int main(int argc, char *argv[])
+{
+    CommandLineParser parser(argc, argv, params);
+
+    if (parser.get<bool>("help"))
+    {
+        cout << about << endl;
+        parser.printMessage();
+        return 0;
+    }
+
+    Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
+    float squareSize = (float) parser.get<double>("square_size");
+    homographyFromCameraDisplacement(parser.get<string>("image1"),
+                                     parser.get<string>("image2"),
+                                     patternSize, squareSize,
+                                     parser.get<string>("intrinsics"));
+
+    return 0;
+}
+#else
+int main()
+{
+    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
+    return 0;
+}
+#endif

samples/cpp/tutorial_code/features2D/Homography/tutorial_homography_ex4_decompose_homography.cpp

+#include <iostream>
+#include <opencv2/opencv_modules.hpp>
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.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, "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(img1Path);
+    Mat img2 = imread(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(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);
+    //! [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;
+    //! [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;
+    double d_inv1 = 1.0 / normal1.dot(origin1);
+    //! [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;
+    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]
+    Mat H = findHomography(corners1, corners2);
+    //! [estimate homography]
+
+    //! [decompose-homography-estimated-by-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* about = "Code for homography tutorial.\n"
+                    "Example 4: decompose the homography matrix.\n";
+
+const char* params
+    = "{ h help         | false | print usage }"
+      "{ image1         |       | path to the source chessboard image (left02.jpg) }"
+      "{ image2         |       | path to the desired chessboard image (left01.jpg) }"
+      "{ intrinsics     |       | path to camera intrinsics (left_intrinsics.yml) }"
+      "{ width w        | 9     | chessboard width }"
+      "{ height h       | 6     | chessboard height }"
+      "{ square_size    | 0.025 | chessboard square size }";
+}
+
+int main(int argc, char *argv[])
+{
+    CommandLineParser parser(argc, argv, params);
+
+    if (parser.get<bool>("help"))
+    {
+        cout << about << endl;
+        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;
+}
+#else
+int main()
+{
+    std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
+    return 0;
+}
+#endif