【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}\sqrt{1^2+0^2+0^2}})$$
C++代码：
向量乘法等于对应项相乘最后求和

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

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