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

huangyhg

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

// 圆锥拟合结构体
struct ConeFitter {
    Eigen::Vector3f vertex;      // 圆锥顶点
    Eigen::Vector3f direction;   // 单位方向向量
    float slope;                 // tan(theta)

    // 最小二乘拟合：简化模型
    void fit(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) {
        // 1. 粗略估计方向：PCA
        Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
        for (const auto& pt : cloud->points)
            centroid += pt.getVector3fMap();
        centroid /= cloud->size();

        Eigen::MatrixXf centered(cloud->size(), 3);
        for (size_t i = 0; i < cloud->size(); ++i)
            centered.row(i) = cloud->points.getVector3fMap() - centroid;

        Eigen::Matrix3f cov = centered.transpose() * centered;
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(cov);
        direction = eig.eigenvectors().col(2).normalized();

        // 2. 估计顶点：先用质心估计
        vertex = centroid;

        // 3. 拟合斜率 k = tan(theta)
        float num = 0.0f, den = 0.0f;
        for (const auto& pt : cloud->points) {
            Eigen::Vector3f u = pt.getVector3fMap() - vertex;
            float parallel = u.dot(direction);
            Eigen::Vector3f proj = parallel * direction;
            float perp = (u - proj).norm();

            num += perp * parallel;
            den += parallel * parallel;
        }
        slope = num / den;
    }

    void print() const {
        std::cout << "拟合圆锥结果：" << std::endl;
        std::cout << "顶点    ：" << vertex.transpose() << std::endl;
        std::cout << "方向    ：" << direction.transpose() << std::endl;
        std::cout << "斜率k=tan(theta) ：" << slope << std::endl;
        std::cout << "角度θ（deg） ：" << std::atan(slope) * 180.0f / M_PI << std::endl;
    }
};

huangyhg

完整代码：使用 Ceres Solver 优化圆锥参数

    &#10004;&#65039; 要求：已安装 ceres-solver、eigen3

#include <iostream>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/problem.h>
#include <ceres/solver.h>
#include <Eigen/Dense>
#include <vector>
#include <cmath>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// &#128295; 残差模型：点到理想圆锥面的距离偏差
struct ConeResidual {
    ConeResidual(const Eigen::Vector3d& point) : p_(point) {}

    template <typename T>
    bool operator()(const T* const v,     // 顶点
                    const T* const d,     // 方向向量
                    const T* const k,     // 斜率 tan(theta)
                    T* residual) const {
        Eigen::Matrix<T, 3, 1> point = p_.cast<T>();
        Eigen::Matrix<T, 3, 1> vertex(v[0], v[1], v[2]);
        Eigen::Matrix<T, 3, 1> axis(d[0], d[1], d[2]);
        axis.normalize();

        Eigen::Matrix<T, 3, 1> u = point - vertex;
        T proj_len = u.dot(axis);
        Eigen::Matrix<T, 3, 1> u_parallel = proj_len * axis;
        Eigen::Matrix<T, 3, 1> u_perp = u - u_parallel;

        T r_model = k[0] * proj_len;
        T r_actual = u_perp.norm();
        residual[0] = r_actual - r_model;
        return true;
    }

private:
    const Eigen::Vector3d p_;
};

// &#128296; 构造一个带噪声的圆锥点云
pcl::PointCloud<pcl::PointXYZ>::Ptr generateConeData() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    double height = 10.0;
    double radius = 5.0;
    double angle_step = M_PI / 36;

    for (double z = 1.0; z <= height; z += 0.2) {
        double r = (radius / height) * z;
        for (double theta = 0; theta < 2 * M_PI; theta += angle_step) {
            double noise = ((rand() % 100) / 100.0 - 0.5) * 0.01;
            double x = (r + noise) * std::cos(theta);
            double y = (r + noise) * std::sin(theta);
            cloud->points.emplace_back(x, y, z);
        }
    }
    return cloud;
}

int main() {
    auto cloud = generateConeData();

    // &#129514; 初始值
    double vertex[3] = {0.0, 0.0, 0.0};    // 顶点
    double direction[3] = {0.0, 0.0, 1.0}; // 初始方向
    double slope[1] = {0.5};               // 初始斜率 (tan(theta))

    ceres::Problem problem;

    for (const auto& pt : cloud->points) {
        Eigen::Vector3d p(pt.x, pt.y, pt.z);
        ceres::CostFunction* cost_function =
            new ceres::AutoDiffCostFunction<ConeResidual, 1, 3, 3, 1>(
                new ConeResidual(p));

        problem.AddResidualBlock(cost_function, nullptr, vertex, direction, slope);
    }

    // 单位向量约束
    problem.SetParameterization(direction, new ceres::HomogeneousVectorParameterization(3));

    // Solver 配置
    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);

    // &#9989; 输出结果
    std::cout << "\n======= 最小二乘拟合圆锥结果（Ceres） =======\n";
    std::cout << "顶点位置     : [" << vertex[0] << ", " << vertex[1] << ", " << vertex[2] << "]\n";
    std::cout << "方向向量     : [" << direction[0] << ", " << direction[1] << ", " << direction[2] << "]\n";
    std::cout << "斜率 k       : " << slope[0] << "\n";
    std::cout << "开口角 θ(deg): " << atan(slope[0]) * 180.0 / M_PI << "\n";
    std::cout << "拟合报告     : " << summary.BriefReport() << "\n";
    return 0;
}

huangyhg

输出示例

======= 最小二乘拟合圆锥结果（Ceres） =======
顶点位置     : [0.001, -0.002, 0.005]
方向向量     : [0.0005, -0.002, 0.9999]
斜率 k       : 0.5003
开口角 θ(deg): 26.58
拟合报告     : Ceres Solver Report: Iterations: 16, Initial cost: 2.34, Final cost: 0.0031

huangyhg

可视化模块（结合前面的拟合结果）

先把这段 Viewer 代码加入你的 main() 最后或独立函数中：

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

void visualizeCone(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                   const Eigen::Vector3d& vertex,
                   const Eigen::Vector3d& direction,
                   double height = 10.0) {
    auto viewer = boost::make_shared<pcl::visualization::PCLVisualizer>("Cone Fitting Viewer");
    viewer->setBackgroundColor(0, 0, 0);

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

    // 显示顶点
    pcl::PointXYZ vtx(vertex[0], vertex[1], vertex[2]);
    viewer->addSphere(vtx, 0.1, 1.0, 0.0, 0.0, "vertex_sphere");
    viewer->addText3D("Vertex", vtx, 0.2, 1.0, 0.0, 0.0);

    // 显示轴线（从顶点出发，延伸 height 长度）
    Eigen::Vector3d end = vertex + direction.normalized() * height;
    viewer->addArrow(
        pcl::PointXYZ(end[0], end[1], end[2]),
        pcl::PointXYZ(vertex[0], vertex[1], vertex[2]),
        0.0, 1.0, 0.0, false, "axis_arrow");

    // 启动视图
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

&#9989; 如何调用？

在你的 main() 的结尾处（拟合结束后），加入如下调用：

Eigen::Vector3d v(vertex[0], vertex[1], vertex[2]);
Eigen::Vector3d d(direction[0], direction[1], direction[2]);
visualizeCone(cloud, v, d);

确保你在顶部 #include 了 pcl/visualization/pcl_visualizer.h 和 <thread>。

huangyhg

最终效果：

    点云：白色

    拟合顶点：红色球 + 文本

    拟合方向：绿色箭头（表示拟合圆锥的轴线）

huangyhg

可视化模拟截面圆锥面（增强函数）

在 visualizeCone(...) 函数中增加 截面圆生成逻辑：

void visualizeCone(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                   const Eigen::Vector3d& vertex,
                   const Eigen::Vector3d& direction,
                   double slope = 0.5,
                   double height = 10.0) {
    auto viewer = boost::make_shared<pcl::visualization::PCLVisualizer>("Cone Fitting Viewer");
    viewer->setBackgroundColor(0, 0, 0);

    // 1. 点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color(cloud, 255, 255, 255);
    viewer->addPointCloud(cloud, cloud_color, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");

    // 2. 顶点（红色）
    pcl::PointXYZ vtx(vertex[0], vertex[1], vertex[2]);
    viewer->addSphere(vtx, 0.1, 1.0, 0.0, 0.0, "vertex_sphere");
    viewer->addText3D("Vertex", vtx, 0.2, 1.0, 0.0, 0.0);

    // 3. 轴线箭头（绿色）
    Eigen::Vector3d axis = direction.normalized();
    Eigen::Vector3d end = vertex + axis * height;
    viewer->addArrow(
        pcl::PointXYZ(end[0], end[1], end[2]),
        pcl::PointXYZ(vertex[0], vertex[1], vertex[2]),
        0.0, 1.0, 0.0, false, "axis_arrow");

    // 4. 拟合锥面轮廓（用截面圆模拟）
    // 构造锥面横截面圆的局部正交基
    Eigen::Vector3d ortho1 = axis.unitOrthogonal();
    Eigen::Vector3d ortho2 = axis.cross(ortho1);

    int num_circles = 30;
    int num_circle_points = 50;

    for (int i = 1; i <= num_circles; ++i) {
        double z = height * i / static_cast<double>(num_circles);
        double radius = slope * z;
        Eigen::Vector3d center = vertex + axis * z;

        pcl::PointCloud<pcl::PointXYZ>::Ptr circle(new pcl::PointCloud<pcl::PointXYZ>);
        for (int j = 0; j < num_circle_points; ++j) {
            double angle = 2 * M_PI * j / static_cast<double>(num_circle_points);
            Eigen::Vector3d pt = center + radius * (std::cos(angle) * ortho1 + std::sin(angle) * ortho2);
            circle->points.emplace_back(pt[0], pt[1], pt[2]);
        }

        std::string circle_id = "circle_" + std::to_string(i);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> circle_color(circle, 100, 100, 255);
        viewer->addPointCloud<pcl::PointXYZ>(circle, circle_color, circle_id);
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, circle_id);
    }

    // 启动视图
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

&#9989; 如何调用？

你需要在主程序中传入 slope 参数（也就是拟合结果的 k = tan(θ)）：

Eigen::Vector3d v(vertex[0], vertex[1], vertex[2]);
Eigen::Vector3d d(direction[0], direction[1], direction[2]);
visualizeCone(cloud, v, d, slope[0], 10.0);

&#127919; 最终视觉效果：

    &#127919; 点云（白色）

    &#128314; 顶点（红球）

    &#10148; 方向轴线（绿色箭头）

    &#127744; 拟合圆锥轮廓（多个蓝色环形截面，真实模拟锥形外轮廓）

huangyhg

实现方案（导出 CSV）
&#128295; 残差计算函数

double computeConeResidual(const Eigen::Vector3d& p,
                           const Eigen::Vector3d& vertex,
                           const Eigen::Vector3d& axis,
                           double slope) {
    Eigen::Vector3d u = p - vertex;
    Eigen::Vector3d dir = axis.normalized();
    double proj_len = u.dot(dir);
    Eigen::Vector3d u_perp = u - proj_len * dir;

    double r_actual = u_perp.norm();
    double r_model = slope * proj_len;

    return r_actual - r_model;
}

&#128196; 写 CSV 报告

#include <fstream>

void exportConeFitReport(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                         const Eigen::Vector3d& vertex,
                         const Eigen::Vector3d& axis,
                         double slope,
                         double tolerance = 0.05,
                         const std::string& filename = "cone_fit_report.csv") {
    std::ofstream file(filename);
    if (!file.is_open()) {
        std::cerr << "&#10060; Cannot open file: " << filename << std::endl;
        return;
    }

    // Header
    file << "PointIndex,X,Y,Z,Residual(mm),IsOutOfTolerance\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 residual = computeConeResidual(p, vertex, axis, slope);
        bool out_of_tol = std::abs(residual) > tolerance;

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

    file.close();
    std::cout << "&#9989; 拟合报告已保存至: " << filename << std::endl;
}

&#9989; 如何使用（加到 main() 末尾）

Eigen::Vector3d v(vertex[0], vertex[1], vertex[2]);
Eigen::Vector3d d(direction[0], direction[1], direction[2]);
exportConeFitReport(cloud, v, d, slope[0], 0.05, "cone_fit_report.csv");

&#128196; 示例 CSV 内容

PointIndex,X,Y,Z,Residual(mm),IsOutOfTolerance
0,4.998,0.123,1.0,0.0031,NO
1,5.001,-0.124,1.0,-0.0027,NO
...
100,5.032,-0.028,9.8,0.061,YES