OpenCVSharp 4.5 相机位移之单应性矩阵

用 OpenCVSharp 4.5 跑一遍 OpenCV 官方教程

原 OpenCV 官方教程链接：OpenCV: Basic concepts of the homography explained with code (Demo3)

using System;
using System.Collections.Generic;
using OpenCvSharp;
namespace ConsoleApp1
{
    class tutorial56 : ITutorial
    {
        enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };

        private void calcChessboardCorners(Size boardSize, float squareSize, out Point3f[] corners, Pattern patternType = Pattern.CHESSBOARD)
        {
            //corners.resize(0);

            List<Point3f> corner_list = new List<Point3f>();

            corners = new Point3f[boardSize.Width * boardSize.Height];
            switch (patternType)
            {
                case Pattern.CHESSBOARD:
                case Pattern.CIRCLES_GRID:
                    for (int i = 0; i < boardSize.Height; i++)
                        for (int j = 0; j < boardSize.Width; j++)
                            corner_list.Add(new Point3f((float)(j * squareSize), (float)(i * squareSize), 0));
                    break;

                case Pattern.ASYMMETRIC_CIRCLES_GRID:
                    for (int i = 0; i < boardSize.Height; i++)
                        for (int j = 0; j < boardSize.Width; j++)
                            corner_list.Add(new Point3f((float)((2 * j + i % 2) * squareSize), (float)(i * squareSize), 0));
                    break;

                default:
                    Console.WriteLine("Unknown pattern type\n");
                    break;
            }
            corners = corner_list.ToArray();
        }

        //! [compute-homography]
        private Mat computeHomography(Mat R_1to2, Mat tvec_1to2, double d_inv, Mat normal)
        {
            Mat homography = R_1to2 + d_inv * tvec_1to2 * normal.T();
            return homography;
        }
        //! [compute-homography]

        private Mat computeHomography(Mat R1, Mat tvec1, Mat R2, Mat tvec2, double d_inv, Mat normal)
        {
            Mat homography = R2 * R1.T() + d_inv * (-R2 * R1.T() * tvec1 + tvec2) * normal.T();
            return homography;
        }

        //! [compute-c2Mc1]
        private void computeC2MC1(Mat R1, Mat tvec1, Mat R2, Mat tvec2, out Mat R_1to2, out Mat tvec_1to2)
        {
            //c2Mc1 = c2Mo * oMc1 = c2Mo * c1Mo.inv()
            R_1to2 = R2 * R1.T();
            tvec_1to2 = R2 * (-R1.T() * tvec1) + tvec2;
        }
        //! [compute-c2Mc1]

        private void homographyFromCameraDisplacement(string img1Path, string img2Path, Size patternSize, float squareSize)
        {
            Mat img1 = Cv2.ImRead(img1Path);
            Mat img2 = Cv2.ImRead(img2Path);

            //! [compute-poses]
            Point2f[] corners1, corners2;
            bool found1 = Cv2.FindChessboardCorners(img1, patternSize, out corners1);
            bool found2 = Cv2.FindChessboardCorners(img2, patternSize, out corners2);

            if (!found1 || !found2)
            {
                Console.WriteLine("Error, cannot find the chessboard corners in both images.");
                return;
            }
            Console.WriteLine("{0}", corners1);
            Console.WriteLine("{0}", corners2);

            Point3f[] objectPoints;
            calcChessboardCorners(patternSize, squareSize, out objectPoints);

            //! [load-intrinsics]
            Mat cameraMatrix = new Mat(3, 3, MatType.CV_64F, new Double[9] {
                5.3591573396163199e+02, 0.0 , 3.4228315473308373e+02 ,
                0.0, 5.3591573396163199e+02, 2.3557082909788173e+02 ,
                 0.0,0.0, 1.0});

            Mat distCoeffs = new Mat(5, 1, MatType.CV_64F, new Double[5] { -2.6637260909660682e-01,
                -3.8588898922304653e-02,
                1.7831947042852964e-03,
                -2.8122100441115472e-04,
                2.3839153080878486e-01 });


            Mat rvec1 = new Mat(), tvec1 = new Mat();
            //SolvePnP(IEnumerable<Point3f> objectPoints, IEnumerable<Point2f> imagePoints, double[,] cameraMatrix, IEnumerable<double>? distCoeffs, ref double[] rvec, ref double[] tvec, bool useExtrinsicGuess = false, SolvePnPFlags flags = SolvePnPFlags.Iterative);
            Cv2.SolvePnP(InputArray.Create(objectPoints), InputArray.Create(corners1), cameraMatrix, distCoeffs, rvec1, tvec1);
            Mat rvec2 = new Mat(), tvec2 = new Mat();
            Cv2.SolvePnP(InputArray.Create(objectPoints), InputArray.Create(corners2), cameraMatrix, distCoeffs, rvec2, tvec2);
            //! [compute-poses]

            Mat img1_copy_pose = img1.Clone(), img2_copy_pose = img2.Clone();
            Mat img_draw_poses = new Mat();
            Cv2.DrawFrameAxes(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2 * squareSize);
            Cv2.DrawFrameAxes(img2_copy_pose, cameraMatrix, distCoeffs, rvec2, tvec2, 2 * squareSize);
            Cv2.HConcat(img1_copy_pose, img2_copy_pose, img_draw_poses);
            Cv2.ImShow("Chessboard poses", img_draw_poses);

            //! [compute-camera-displacement]
            Mat R1 = new Mat(), R2 = new Mat();
            Cv2.Rodrigues(rvec1, R1);
            Cv2.Rodrigues(rvec2, R2);

            Mat R_1to2 = new Mat(), t_1to2 = new Mat();
            computeC2MC1(R1, tvec1, R2, tvec2, out R_1to2, out t_1to2);
            Mat rvec_1to2 = new Mat();
            Cv2.Rodrigues(R_1to2, rvec_1to2);
            //! [compute-camera-displacement]

            //! [compute-plane-normal-at-camera-pose-1]
            Mat normal = new Mat(3, 1, MatType.CV_64F, new double[] { 0, 0, 1 });
            Mat normal1 = R1 * normal;
            //! [compute-plane-normal-at-camera-pose-1]

            //! [compute-plane-distance-to-the-camera-frame-1]
            Mat origin = Mat.Zeros(new Size(1, 3), MatType.CV_64F);
            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);

            Console.WriteLine("\nEuclidean Homography:\n{0}", Cv2.Format(homography_euclidean));
            Console.WriteLine("Euclidean Homography 2:\n{0}", Cv2.Format(homography_euclidean2));

            //! [estimate-homography]
            Mat H = Cv2.FindHomography(InputArray.Create(corners1), InputArray.Create(corners2));
            Console.WriteLine("\nfindHomography H:\n{0}", Cv2.Format(H));
            //! [estimate-homography]

            Console.WriteLine("homography from camera displacement:\n{0}", Cv2.Format(homography));
            Console.WriteLine("homography from absolute camera poses:\n{0}", Cv2.Format(homography2));

            //! [warp-chessboard]
            Mat img1_warp = new Mat();
            Cv2.WarpPerspective(img1, img1_warp, H, img1.Size());
            //! [warp-chessboard]

            Mat img1_warp_custom = new Mat();
            Cv2.WarpPerspective(img1, img1_warp_custom, homography, img1.Size());
            Cv2.ImShow("Warped image using homography computed from camera displacement", img1_warp_custom);

            Mat img_draw_compare = new Mat();
            Cv2.HConcat(img1_warp, img1_warp_custom, img_draw_compare);
            Cv2.ImShow("Warped images comparison", img_draw_compare);

            Mat img1_warp_custom2 = new Mat();
            Cv2.WarpPerspective(img1, img1_warp_custom2, homography2, img1.Size());
            Cv2.ImShow("Warped image using homography computed from absolute camera poses", img1_warp_custom2);

            Cv2.WaitKey();
        }

        public void Run()
        {
            string image1 = @"I:\csharp\images\left02.jpg";
            string image2 = @"I:\csharp\images\left01.jpg";
            int width = 9;
            int height = 6;
            Size patternSize = new Size(width, height);
            float squareSize = 0.025f;
            homographyFromCameraDisplacement(image1, image2, patternSize, squareSize);
        }

    }
}