【C++】【初三知识我忘了。。】三维点最小二乘法拟合平面并求平面和轴的夹角

三维点最小二乘法拟合平面

这里是用三维点拟合三维平面　网上的代码乱七八糟　这个应该是最小二乘法拟合三维平面的正确答案　输入的点的个数要三个或多于三个哈

    Eigen::Vector4d PlaneFitting(const std::vector<Eigen::Vector3d>& plane_pts)
    {
        Eigen::Vector3d center = Eigen::Vector3d::Zero();
        for (const auto& pt : plane_pts)
            center += pt;
        center /= plane_pts.size();

        Eigen::MatrixXd A(plane_pts.size(), 3);
        for (size_t i = 0; i < plane_pts.size(); i++)
        {
            A(i, 0) = plane_pts[i][0] - center[0];
            A(i, 1) = plane_pts[i][1] - center[1];
            A(i, 2) = plane_pts[i][2] - center[2];
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);
        const float                       a = svd.matrixV()(0, 2);
        const float                       b = svd.matrixV()(1, 2);
        const float                       c = svd.matrixV()(2, 2);
        const float                       d = -(a * center[0] + b * center[1] + c * center[2]);
        return Eigen::Vector4d(a, b, c, d);
    }

求三维平面的法向量

\[Ax + By + Cz + D = 0 \]

法向量为：

\[(A, B, C) \]

求三维平面和xyz轴的夹角

平面的法向量和轴的夹角的余弦值等于平面和轴夹角的正弦值
xyz轴的方向向量分别为：

\[x: (1, 0, 0) \]

\[y: (0, 1, 0) \]

\[z:(0, 0, 1) \]

所以与x轴夹角为:

\[arcsin(\frac {(A, B, C)(1, 0, 0)}{\sqrt{A^2+B^2+C^2} \quad\sqrt{1^2+0^2+0^2} \quad}) \]

C++代码：
向量乘法等于对应项相乘最后求和

//开平方：
double result = sqrt(x);
//求arcsin:
double theta = asin(x)

得到的结果是弧度制
弧度制转角度：弧度乘以57.3