smart-bow-infrared/Assets/InfraredProject/WebCamera/Script/ZIM/InfraredLocate/PerspectiveTransform.cs

using System;
using System.Diagnostics;
using o0.Geometry2D.Float;

// 其实这个类内部是用Double计算的
namespace ZIM
{
    // 来源 https://web.archive.org/web/20150222120106/xenia.media.mit.edu/~cwren/interpolator/
    public class ZIMPerspectiveTransform
    {
        // 变换矩阵
        double[,] H;
        double[,] InverseH;

        public ZIMPerspectiveTransform(OrdinalQuadrilateral src, OrdinalQuadrilateral dst)
        {
            // 该算法点序为 左下角开始 顺时针
            double[] X = new double[4] { src.A.x, src.C.x, src.D.x, src.B.x };
            double[] Y = new double[4] { src.A.y, src.C.y, src.D.y, src.B.y };

            double[] Xp = new double[4] { dst.A.x, dst.C.x, dst.D.x, dst.B.x };
            double[] Yp = new double[4] { dst.A.y, dst.C.y, dst.D.y, dst.B.y };

            TransformEstimate(X, Y, Xp, Yp);

            //UnityEngine.Debug.Log(H.ToJson(true));
            //UnityEngine.Debug.Log(H[0, 0]);
            //UnityEngine.Debug.Log(H[0, 1]);
            //UnityEngine.Debug.Log(H[0, 2]);
        }

        //public Vector Transform(Vector v)
        //{
        //    var result = Transform(v.x, v.y);
        //    return new Vector((float)result.x, (float)result.y);
        //}

        public (int x, int y) TransformRound(int x, int y)
        {
            var result = Transform((double)x, (double)y);
            return ((int)Math.Round(result.x), (int)Math.Round(result.y));
        }

        public (float x, float y) Transform(float x, float y)
        {
            var result = Transform((double)x, (double)y);
            return ((float)result.x, (float)result.y);
        }

        public (double x, double y) Transform(double x, double y)
        {
            double t1 = H[0, 0] * x + H[0, 1] * y + H[0, 2];
            double t2 = H[1, 0] * x + H[1, 1] * y + H[1, 2];
            double t3 = H[2, 0] * x + H[2, 1] * y + H[2, 2];
            return (t1 / t3, t2 / t3);
        }

        public (float x, float y) TransformInverse(float x, float y)
        {
            var result = TransformInverse((double)x, (double)y);
            return ((float)result.x, (float)result.y);
        }

        public (double x, double y) TransformInverse(double x, double y)
        {
            double t1 = InverseH[0, 0] * x + InverseH[0, 1] * y + InverseH[0, 2];
            double t2 = InverseH[1, 0] * x + InverseH[1, 1] * y + InverseH[1, 2];
            double t3 = InverseH[2, 0] * x + InverseH[2, 1] * y + InverseH[2, 2];
            return (t1 / t3, t2 / t3);
        }

        // Calculate matrix transform
        void TransformEstimate(double[] X, double[] Y, double[] Xp, double[] Yp)
        {
            //double[] Xp = { 0, 0, 1280, 1280 };
            //double[] Yp = { 0, 720, 720, 0 };
            double[,] B = new double[8, 8];
            double[,] D = new double[8, 1];

            for (int i = 0; i < 4; i++)
            {
                B[i, 0] = X[i];
                B[i, 1] = Y[i];
                B[i, 2] = 1;
                B[i, 6] = -X[i] * Xp[i];
                B[i, 7] = -Y[i] * Xp[i];

                B[i + 4, 3] = X[i];
                B[i + 4, 4] = Y[i];
                B[i + 4, 5] = 1;
                B[i + 4, 6] = -X[i] * Yp[i];
                B[i + 4, 7] = -Y[i] * Yp[i];
            }

            for (int i = 0; i < 4; i++)
            {
                D[i, 0] = Xp[i];
                D[i + 4, 0] = Yp[i];
            }

            double[,] BTranspose = TransposeMatrix(B);
            double[,] l = MatrixMultiply(MatrixMultiply(MatrixInverse(MatrixMultiply(BTranspose, B)), BTranspose), D);      // l 是8行1列的矩阵

            H = new double[3, 3] { { l[0, 0], l[1, 0], l[2, 0] }, { l[3, 0], l[4, 0], l[5, 0] }, { l[6, 0], l[7, 0], 1 } };
            InverseH = MatrixInverse(H);

            // 测试代码
            //var x1 = X[2];
            //var y1 = Y[2];
            //var x2 = X[0];
            //var y2 = Y[0];

            //// Plot the points and their transformed coordinates
            //UnityEngine.Debug.Log("Plotting the points and their transformed coordinates...");
            //for (double u = 0; u <= 1; u += 0.1)
            //{
            //    double x = u * x1 + (1 - u) * x2;
            //    double y = u * y1 + (1 - u) * y2;
            //    UnityEngine.Debug.Log($"Point ({x}, {y})");

            //    double t1 = H[0, 0] * x + H[0, 1] * y + H[0, 2];
            //    double t2 = H[1, 0] * x + H[1, 1] * y + H[1, 2];
            //    double t3 = H[2, 0] * x + H[2, 1] * y + H[2, 2];
            //    double tX = t1 / t3;
            //    double tY = t2 / t3;
            //    UnityEngine.Debug.Log($"Transformed point ({tX}, {tY})");
            //}
        }

        static double[,] TransposeMatrix(double[,] matrix)
        {
            int rows = matrix.GetLength(0);
            int cols = matrix.GetLength(1);
            double[,] result = new double[cols, rows];

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    result[j, i] = matrix[i, j];
                }
            }

            return result;
        }

        static double[,] ReshapeMatrix(double[,] matrix, int rows, int cols)
        {
            int originalRows = matrix.GetLength(0);
            int originalCols = matrix.GetLength(1);

            if (originalRows * originalCols != rows * cols)
            {
                throw new Exception("Invalid reshape dimensions");
            }

            double[,] result = new double[rows, cols];
            int rowIndex = 0;
            int colIndex = 0;

            for (int i = 0; i < originalRows; i++)
            {
                for (int j = 0; j < originalCols; j++)
                {
                    result[rowIndex, colIndex] = matrix[i, j];
                    colIndex++;

                    if (colIndex == cols)
                    {
                        colIndex = 0;
                        rowIndex++;
                    }
                }
            }

            return result;
        }

        static double[,] ReshapeMatrix(double[] array, int rows, int cols)
        {
            double[,] result = new double[rows, cols];
            int index = 0;

            for (int j = 0; j < cols; j++)
            {
                for (int i = 0; i < rows; i++)
                {
                    result[i, j] = array[index];
                    index++;
                }
            }

            return result;
        }

        static double[,] MatrixMultiply(double[,] matrix1, double[,] matrix2)
        {
            int rows1 = matrix1.GetLength(0);
            int cols1 = matrix1.GetLength(1);
            int rows2 = matrix2.GetLength(0);
            int cols2 = matrix2.GetLength(1);

            if (cols1 != rows2)
            {
                throw new Exception("Invalid matrix dimensions");
            }

            double[,] result = new double[rows1, cols2];

            for (int i = 0; i < rows1; i++)
            {
                for (int j = 0; j < cols2; j++)
                {
                    for (int k = 0; k < cols1; k++)
                    {
                        result[i, j] += matrix1[i, k] * matrix2[k, j];
                    }
                }
            }

            return result;
        }

        static double[,] MatrixInverse(double[,] matrix)
        {
            int n = matrix.GetLength(0);
            double[,] augmentedMatrix = new double[n, 2 * n];

            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    augmentedMatrix[i, j] = matrix[i, j];
                }

                augmentedMatrix[i, i + n] = 1;
            }

            for (int i = 0; i < n; i++)
            {
                double pivot = augmentedMatrix[i, i];

                for (int j = 0; j < 2 * n; j++)
                {
                    augmentedMatrix[i, j] /= pivot;
                }

                for (int k = 0; k < n; k++)
                {
                    if (k != i)
                    {
                        double factor = augmentedMatrix[k, i];

                        for (int j = 0; j < 2 * n; j++)
                        {
                            augmentedMatrix[k, j] -= factor * augmentedMatrix[i, j];
                        }
                    }
                }
            }

            double[,] inverseMatrix = new double[n, n];

            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    inverseMatrix[i, j] = augmentedMatrix[i, j + n];
                }
            }

            return inverseMatrix;
        }
    }
}