C#实现拟合平面计算平面与直线的夹角

using System;
using System.Collections.Generic;
using System.IO;
using System.Text;

class Program
{
    static void Main(string[] args)
    {
        List<List<double>> points = new List<List<double>>();

        // 读取文件
        using (StreamReader sr = new StreamReader("D:\\1.txt", Encoding.UTF8))
        {
            string line;
            while ((line = sr.ReadLine()) != null)
            {
                string[] strs = line.Split(',');
                List<double> point = new List<double>
                {
                    double.Parse(strs[0]),
                    double.Parse(strs[1]),
                    double.Parse(strs[2])
                };
                points.Add(point);
            }
        }

        // 构建矩阵A和b
        int n = points.Count;
        double[,] A = new double[n, 3];
        double[,] b = new double[n, 1];
        for (int i = 0; i < n; i++)
        {
            A[i, 0] = points[i][0];
            A[i, 1] = points[i][1];
            A[i, 2] = 1;
            b[i, 0] = points[i][2];
        }

        // 计算最小二乘解
        double[,] At = Transpose(A);
        double[,] AtA = Multiply(At, A);
        double[,] AtAInv = Invert(AtA);
        double[,] Atb = Multiply(At, b);
        double[,] a = Multiply(AtAInv, Atb);

        // 输出解向量a的三个元素
        Console.WriteLine($"a0,a1,a2: {a[0, 0].ToString("f6")}, {a[1, 0].ToString("f6")}, {a[2, 0].ToString("f6")}");

        // 计算夹角
        double[] normalVector = new double[] { a[0, 0], a[1, 0], -1 };
        double[] yAxisVector = new double[] { 1, 0, 0 };
        double cosTheta = CalculateCosTheta(normalVector, yAxisVector);
        double theta = Math.Asin(cosTheta);
        double angleInDegrees = theta * (180 / Math.PI);

        // 输出夹角
        Console.WriteLine($"The angle between the normal vector and the y-axis is: {angleInDegrees} degrees");
    }

    static double[,] Transpose(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[,] Multiply(double[,] a, double[,] b)
    {
        int aRows = a.GetLength(0);
        int aCols = a.GetLength(1);
        int bCols = b.GetLength(1);
        if (aCols != b.GetLength(0))
            throw new ArgumentException("Matrix dimensions do not match for multiplication");

        double[,] result = new double[aRows, bCols];
        for (int i = 0; i < aRows; i++)
        {
            for (int j = 0; j < bCols; j++)
            {
                for (int k = 0; k < aCols; k++)
                {
                    result[i, j] += a[i, k] * b[k, j];
                }
            }
        }
        return result;
    }

    static double[,] Invert(double[,] matrix)
    {
        int n = matrix.GetLength(0);
        if (n != matrix.GetLength(1) || n != 3)
            throw new ArgumentException("Matrix must be square and of dimension 3 for inversion");

        double det = matrix[0, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) -
                     matrix[0, 1] * (matrix[1, 0] * matrix[2, 2] - matrix[1, 2] * matrix[2, 0]) +
                     matrix[0, 2] * (matrix[1, 0] * matrix[2, 1] - matrix[1, 1] * matrix[2, 0]);

        if (det == 0)
            throw new InvalidOperationException("Matrix is singular and cannot be inverted");

        double[,] result = new double[n, n];
        result[0, 0] = (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) / det;
        result[0, 1] = (matrix[0, 2] * matrix[2, 1] - matrix[0, 1] * matrix[2, 2]) / det;
        result[0, 2] = (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1]) / det;
        result[1, 0] = (matrix[1, 2] * matrix[2, 0] - matrix[1, 0] * matrix[2, 2]) / det;
        result[1, 1] = (matrix[0, 0] * matrix[2, 2] - matrix[0, 2] * matrix[2, 0]) / det;
        result[1, 2] = (matrix[0, 2] * matrix[1, 0] - matrix[0, 0] * matrix[1, 2]) / det;
        result[2, 0] = (matrix[1, 0] * matrix[2, 1] - matrix[1, 1] * matrix[2, 0]) / det;
        result[2, 1] = (matrix[0, 1] * matrix[2, 0] - matrix[0, 0] * matrix[2, 1]) / det;
        result[2, 2] = (matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]) / det;

        return result;
    }

    static double CalculateCosTheta(double[] a, double[] b)
    {
        double magnitudeA = CalculateMagnitude(a);
        double magnitudeB = CalculateMagnitude(b);
        double dotProduct = CalculateDotProduct(a, b);
        double cosTheta = dotProduct / (magnitudeA * magnitudeB);
        return cosTheta;
    }

    static double CalculateMagnitude(double[] vector)
    {
        double sum = 0;
        foreach (double value in vector)
        {
            sum += value * value;
        }
        return Math.Sqrt(sum);
    }

    static double CalculateDotProduct(double[] a, double[] b)
    {
        double sum = 0;
        for (int i = 0; i < a.Length; i++)
        {
            sum += a[i] * b[i];
        }
        return sum;
    }
}

要注意的是求平面方程的时候输入的XY坐标需要是实际距离的坐标，不能是深度图中的像素坐标，不然求出来的平面方程不对。