CGAL散乱点拟合最小二乘平面（3D平面拟合，基于Eigen）

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

Kernel::Point_3 compute_centroid(const std::vector<Kernel::Point_3>& points) {
    double x = 0.0, y = 0.0, z = 0.0;
    for (const Kernel::Point_3& p : points) {
        x += p.x();
        y += p.y();
        z += p.z();
    }
    size_t n = points.size();
    return Kernel::Point_3(x / n, y / n, z / n);
}

Kernel::Plane_3 fit_plane(std::vector<Kernel::Point_3>& points, Kernel::Point_3& centroid)
{
    double x0x0 = 0, x0y1 = 0, x0z2 = 0, x0_3 = 0;
    double x1y0 = 0, y1y1 = 0, y1z2 = 0, y1_3 = 0;
    double x2z0 = 0, y2z1 = 0, z2z2 = 0, z2_3 = 0;
    double x3_0 = 0, y3_1 = 0, z3_2 = 0, n_3_3 = 0;

    for (auto&& point : points) {
        x0x0 += point.x() * point.x();
        x0y1 += point.x() * point.y();
        x0z2 += point.x() * point.z();
        x0_3 += point.x();
        x1y0 += point.x() * point.y();
        y1y1 += point.y() * point.y();
        y1z2 += point.y() * point.z();
        y1_3 += point.y();
        x2z0 += point.x() * point.z();
        y2z1 += point.y() * point.z();
        z2z2 += point.z() * point.z();
        z2_3 += point.z();
        x3_0 += point.x();
        y3_1 += point.y();
        z3_2 += point.z();
    }
    n_3_3 = points.size();
    // 构造误差矩阵
    Eigen::Matrix4d A1;
    A1 << x0x0, x0y1, x0z2, x0_3, x1y0, y1y1, y1z2, y1_3, x2z0, y2z1, z2z2, z2_3, x3_0, y3_1, z3_2, n_3_3;
    // A1 * [A, B, C, D].T = [0, 0, 0, 0].T, 求解[A, B, C, D].T的最优值,约束条件||[A, B, C, D].T||=1, min||A1 * [A, B, C, D].T||^2
    Eigen::JacobiSVD<Eigen::Matrix4d> svd(A1, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix4d V = svd.matrixV();
    Eigen::Vector4d result = V.col(3);// 归一化
    result.normalize();

    //使用最小二乘法求得系数向量  
    Vector normal_vector(result.x(), result.y(), result.z());
    centroid = compute_centroid(points);
    return Kernel::Plane_3(centroid, normal_vector);
}