OpenCVSharp 4.5 单应性矩阵(Homography)估计相机姿态

用 OpenCVSharp 4.5 跑一遍 OpenCV 官方教程。

原 OpenCV 官方 教程链接：（今天链接失效了？）Demo 1

using System;
using System.Collections.Generic;
using OpenCvSharp;

namespace ConsoleApp1
{
    class tutorial52 : ITutorial
    {
        enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };

        private void calcChessboardCorners(Size boardSize, float squareSize, out Vec3f[] corners, Pattern patternType = Pattern.CHESSBOARD)
        {
            List<Vec3f> corners_list = new List<Vec3f>();
            //corners.resize(0);

            switch (patternType)
            {
                case Pattern.CHESSBOARD:
                case Pattern.CIRCLES_GRID:
                    //! [compute-chessboard-object-points]
                    for (int i = 0; i < boardSize.Height; i++)
                        for (int j = 0; j < boardSize.Width; j++)
                            corners_list.Add(new Point3f((float)(j * squareSize), (float)(i * squareSize), 0));
                    //! [compute-chessboard-object-points]
                    break;

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

                default:

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

        private void poseEstimationFromCoplanarPoints(string imgPath, string intrinsicsPath, Size patternSize, float squareSize)
        {
            Mat img = Cv2.ImRead(imgPath);
            Mat img_corners = img.Clone(), img_pose = img.Clone();

            //! [find-chessboard-corners]
            Point2f[] corners;
            bool found = Cv2.FindChessboardCorners(img, patternSize, out corners);
            //! [find-chessboard-corners]

            if (found == false)
            {
                Console.WriteLine("Cannot find chessboard corners.");
                return;
            }
            Cv2.DrawChessboardCorners(img_corners, patternSize, corners, found);
            Cv2.ImShow("Chessboard corners detection", img_corners);

            //! [compute-object-points]
            Vec3f[] objectPoints;
            calcChessboardCorners(patternSize, squareSize, out objectPoints);
            Point2d[] objectPointsPlanar = new Point2d[objectPoints.Length];
            for (int i = 0; i < objectPoints.Length; i++)
            {
                objectPointsPlanar[i] = (new Point2d(objectPoints[i].Item0, objectPoints[i].Item1));
            }
            //! [compute-object-points]

            //! [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 });

            //! [load-intrinsics]

            //! [compute-image-points]
            Mat imagePoints = new Mat();
            Mat corners_mat = Mat.FromArray<Point2f>(corners);
            Cv2.UndistortPoints(corners_mat, imagePoints, cameraMatrix, distCoeffs);
            //! [compute-image-points]

            //! [estimate-homography]
            Mat objectPointsPlannar_mat = Mat.FromArray<Point2d>(objectPointsPlanar);
            Mat H = Cv2.FindHomography(objectPointsPlannar_mat, imagePoints, HomographyMethods.None);
            Console.WriteLine("H:\n{0}", Cv2.Format(H));
            //! [estimate-homography]

            //! [pose-from-homography]
            // Normalization to ensure that ||c1|| = 1
            double norm = Math.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 = new Mat(3, 3, MatType.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]

            //! [polar-decomposition-of-the-rotation-matrix]
            Console.WriteLine("R (before polar decomposition):\n{0}\ndet(R):{1}", Cv2.Format(R), Cv2.Format(Cv2.Determinant(R)));
            Mat W = new Mat();
            Mat U = new Mat();
            Mat Vt = new Mat();
            Cv2.SVDecomp(R, W, U, Vt);
            R = U * Vt;
            Console.WriteLine("R (after polar decomposition):\n{0}\ndet(R):{1}", Cv2.Format(R), Cv2.Format(Cv2.Determinant(R)));
            //! [polar-decomposition-of-the-rotation-matrix]

            //! [display-pose]
            Mat rvec = new Mat();
            Cv2.Rodrigues(R, rvec);
            Cv2.DrawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2 * squareSize);
            Cv2.ImShow("Pose from coplanar points", img_pose);
            Cv2.WaitKey();
            //! [display-pose]
        }

        public void Run()
        {
            int width = 9;
            int height = 6;
            float squareSize = 0.025f;
            string imagePath = @"I:\csharp\images\left04.jpg";
            string intrinsicsPath = @"\I:\chsarp\images\left_intrinsics.yml";
            Size patternSize = new Size(width, height);
            poseEstimationFromCoplanarPoints(imagePath, intrinsicsPath, patternSize, squareSize);

            return;
        }
    }
}