用开源PCL和c++和eigen编写一组离散点拟合最小二乘球

huangyhg

方法一：线性最小二乘球拟合（快速稳定）
🛠 完整代码示例

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

// 拟合球体中心和半径
void fitSphereLeastSquares(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                           Eigen::Vector3f& center_out,
                           float& radius_out) {
    int N = cloud->size();
    Eigen::MatrixXf A(N, 4);
    Eigen::VectorXf b(N);

    for (int i = 0; i < N; ++i) {
        const auto& pt = cloud->points;
        float x = pt.x;
        float y = pt.y;
        float z = pt.z;

        A(i, 0) = x;
        A(i, 1) = y;
        A(i, 2) = z;
        A(i, 3) = 1.0f;

        b(i) = x * x + y * y + z * z;
    }

    Eigen::Vector4f solution = (A.transpose() * A).ldlt().solve(A.transpose() * b);

    float xc = 0.5f * solution(0);
    float yc = 0.5f * solution(1);
    float zc = 0.5f * solution(2);
    float r_squared = xc * xc + yc * yc + zc * zc + solution(3);

    center_out = Eigen::Vector3f(xc, yc, zc);
    radius_out = std::sqrt(r_squared);
}

&#128200; 主函数：生成模拟球面点并拟合

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 构造一个半径为 5 的球体（加点噪声）
    float R = 5.0f;
    Eigen::Vector3f center_true(1.0f, 2.0f, 3.0f);

    for (float phi = 0; phi < M_PI; phi += M_PI / 20) {
        for (float theta = 0; theta < 2 * M_PI; theta += M_PI / 20) {
            float x = R * sin(phi) * cos(theta) + center_true.x();
            float y = R * sin(phi) * sin(theta) + center_true.y();
            float z = R * cos(phi) + center_true.z();

            // 添加微小噪声
            float noise = ((rand() % 100) / 100.0f - 0.5f) * 0.02f;
            cloud->points.emplace_back(x + noise, y + noise, z + noise);
        }
    }

    // 拟合
    Eigen::Vector3f center_est;
    float radius_est;
    fitSphereLeastSquares(cloud, center_est, radius_est);

    // 输出结果
    std::cout << "拟合球心: " << center_est.transpose() << std::endl;
    std::cout << "拟合半径: " << radius_est << std::endl;
    std::cout << "真 实 球心: " << center_true.transpose() << std::endl;
    std::cout << "真实半径: " << R << std::endl;

    return 0;
}

&#9989; 输出示例：

拟合球心: 1.0008 2.0005 3.0011
拟合半径: 4.9993
真实球心: 1 2 3
真实半径: 5

&#9989; 优势
特性        说明
&#128640; 快速        全程只需线性代数操作（求解 Ax=b）
&#129518; 稳定        数学闭式解，适用于密集球面点
&#128297; 工业适用        用于球度、中心偏移、球半径检测等
&#129504; 可扩展：

    &#9989; 用 Ceres Solver 做非线性拟合（更鲁棒）

    &#9989; 用 PCL Viewer 可视化拟合球体、球心

    &#9989; 输出误差报告（每点偏差、球度）

huangyhg

Ceres 拟合代码

struct SphereResidual {
    SphereResidual(const Eigen::Vector3d& point) : point_(point) {}

    template <typename T>
    bool operator()(const T* const center, const T* const radius, T* residual) const {
        Eigen::Matrix<T, 3, 1> c(center[0], center[1], center[2]);
        T r = radius[0];
        residual[0] = (point_.cast<T>() - c).norm() - r;
        return true;
    }

private:
    const Eigen::Vector3d point_;
};

void fitSphereCeres(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                    Eigen::Vector3d& center_out, double& radius_out) {
    double center[3] = {0.0, 0.0, 0.0};
    double radius = 1.0;

    ceres::Problem problem;

    for (const auto& pt : cloud->points) {
        Eigen::Vector3d p(pt.x, pt.y, pt.z);
        ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<SphereResidual, 1, 3, 1>(
            new SphereResidual(p));
        problem.AddResidualBlock(cost, nullptr, center, &radius);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    center_out = Eigen::Vector3d(center[0], center[1], center[2]);
    radius_out = radius;

    std::cout << "&#9989; Ceres 拟合完成:\n" << summary.BriefReport() << std::endl;
}

&#9989; 功能 2：用 PCL Viewer 可视化拟合球体、球心
&#128200; 追加可视化模块

#include <pcl/visualization/pcl_visualizer.h>
#include <thread>

void visualizeSphere(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                     const Eigen::Vector3d& center,
                     double radius) {
    auto viewer = boost::make_shared<pcl::visualization::PCLVisualizer>("Sphere Fitting Viewer");
    viewer->setBackgroundColor(0, 0, 0);

    // 显示原始点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 255, 255, 255);
    viewer->addPointCloud(cloud, color, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

    // 显示拟合球体（透明球）
    pcl::ModelCoefficients sphere;
    sphere.values.resize(4);
    sphere.values[0] = center[0];
    sphere.values[1] = center[1];
    sphere.values[2] = center[2];
    sphere.values[3] = radius;
    viewer->addSphere(sphere, "fitted_sphere");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "fitted_sphere");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "fitted_sphere");

    // 显示球心
    pcl::PointXYZ c(center[0], center[1], center[2]);
    viewer->addSphere(c, 0.1, 1.0, 0.0, 0.0, "center_point");
    viewer->addText3D("Center", c, 0.2, 1.0, 0.0, 0.0);

    // 交互式显示
    viewer->addCoordinateSystem(1.0);
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

&#9989; 功能 3：输出残差误差报告（CSV）
&#129534; 导出 CSV 报告（偏差、球度）

#include <fstream>

void exportSphereResidualCSV(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                             const Eigen::Vector3d& center,
                             double radius,
                             double tolerance = 0.05,
                             const std::string& filename = "sphere_fit_report.csv") {
    std::ofstream file(filename);
    file << "Index,X,Y,Z,Residual(mm),OutOfTolerance\n";

    for (size_t i = 0; i < cloud->size(); ++i) {
        const auto& pt = cloud->points;
        Eigen::Vector3d p(pt.x, pt.y, pt.z);
        double r_actual = (p - center).norm();
        double residual = r_actual - radius;
        bool out = std::abs(residual) > tolerance;

        file << i << "," << pt.x << "," << pt.y << "," << pt.z << ","
             << residual << "," << (out ? "YES" : "NO") << "\n";
    }

    std::cout << "&#128196; 拟合报告导出完成: " << filename << std::endl;
}

&#128257; 主函数整合调用示例

int main() {
    // 1. 生成球点
    auto cloud = generateSphereData();  // 你已有实现

    // 2. 拟合（Ceres）
    Eigen::Vector3d center;
    double radius;
    fitSphereCeres(cloud, center, radius);

    // 3. 可视化
    visualizeSphere(cloud, center, radius);

    // 4. 报告
    exportSphereResidualCSV(cloud, center, radius, 0.05, "sphere_fit_report.csv");

    return 0;
}

&#9989; 运行效果
项目        效果说明
Ceres Solver 拟合        更准确，残差最小（非线性优化）
PCL Viewer        显示球面、球心、点云
CSV 报告        每点偏差 + 是否超差判断（供 QC 审核）

huangyhg

实现方案

我们将：

    计算所有点残差

    映射颜色值（R,G,B）

    构造带颜色的 pcl::PointCloud<pcl::PointXYZRGB>

    用 PCL Viewer 渲染热力图

&#129521; 实现代码

#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

void visualizeSphereResidualsHeatmap(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                                     const Eigen::Vector3d& center,
                                     double radius,
                                     double tolerance = 0.05) {
    auto viewer = boost::make_shared<pcl::visualization::PCLVisualizer>("Sphere Residual Heatmap");
    viewer->setBackgroundColor(0, 0, 0);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    colored_cloud->points.resize(cloud->size());

    // 计算最大残差用于归一化
    double max_dev = 0.0;
    std::vector<double> residuals;
    for (const auto& pt : cloud->points) {
        Eigen::Vector3d p(pt.x, pt.y, pt.z);
        double r_actual = (p - center).norm();
        double residual = r_actual - radius;
        residuals.push_back(residual);
        max_dev = std::max(max_dev, std::abs(residual));
    }

    for (size_t i = 0; i < cloud->size(); ++i) {
        const auto& pt = cloud->points;
        double r = residuals;

        pcl::PointXYZRGB colored_pt;
        colored_pt.x = pt.x;
        colored_pt.y = pt.y;
        colored_pt.z = pt.z;

        // 颜色映射（蓝 → 绿 → 红）
        double norm = (r + max_dev) / (2.0 * max_dev);  // 映射到 0~1
        norm = std::min(std::max(norm, 0.0), 1.0);       // 限制范围

        uint8_t red   = static_cast<uint8_t>(255 * norm);
        uint8_t green = static_cast<uint8_t>(255 * (1.0 - std::abs(2 * norm - 1)));
        uint8_t blue  = static_cast<uint8_t>(255 * (1.0 - norm));

        colored_pt.r = red;
        colored_pt.g = green;
        colored_pt.b = blue;

        colored_cloud->points = colored_pt;
    }

    viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "residual_heatmap");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "residual_heatmap");

    // 显示球心
    pcl::PointXYZ c(center[0], center[1], center[2]);
    viewer->addSphere(c, 0.1, 1.0, 1.0, 1.0, "sphere_center");
    viewer->addText3D("Center", c, 0.2, 1.0, 1.0, 1.0);

    viewer->addCoordinateSystem(1.0);
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

&#129514; 在主程序中调用：

visualizeSphereResidualsHeatmap(cloud, center, radius, 0.05);

&#9989; 可视化效果说明：

    &#128994; 绿色：残差≈0，拟合非常好

    &#128308; 红色：点偏外，超出正方向

    &#128309; 蓝色：点偏内，超出负方向

    &#127752; 渐变中间值：轻微偏离