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

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

    Eigen::Vector4d PlaneFitting(const std::vector& 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 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);
    }

A x + B y + C z + D = 0 Ax + By + Cz + D = 0 Ax+By+Cz+D=0
法向量为：
( A , B , C ) (A, B, C) (A,B,C)

平面的法向量和轴的夹角的余弦值等于平面和轴夹角的正弦值
xyz轴的方向向量分别为：
x : ( 1 , 0 , 0 ) x: (1, 0, 0) x:(1,0,0)
y : ( 0 , 1 , 0 ) y: (0, 1, 0) y:(0,1,0)
z : ( 0 , 0 , 1 ) z:(0, 0, 1) z:(0,0,1)
所以与x轴夹角为:
a r c s i n ( ( A , B , C ) ( 1 , 0 , 0 ) A 2 + B 2 + C 2 1 2 + 0 2 + 0 2 ) arcsin(frac {(A, B, C)(1, 0, 0)}{sqrt{A^2+B^2+C^2} quadsqrt{1^2+0^2+0^2} quad}) arcsin(A2+B2+C2 ​12+02+02 ​(A,B,C)(1,0,0)​)
C++代码：
向量乘法等于对应项相乘最后求和

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

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