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; } } }