点云学习笔记19——RANSAC拟合点云轮廓的外接圆

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <vector>
#include <cmath>

// 定义一个结构体 Circle 表示圆，包含圆心坐标 (x, y) 和半径 r
struct Circle 
{
    // 圆心 (x, y) 和半径 r
    float x, y, r;  
};

// 计算点到圆的距离
float distanceToCircle(float x, float y, Circle& circle)
{
    // 使用勾股定理计算点到圆心的距离，并减去半径,欧几里得距离公式tt
    return std::sqrt(std::pow(x - circle.x, 2) + std::pow(y - circle.y, 2)) - circle.r;
}

// 最小二乘法拟合圆
Circle fitCircle(const std::vector<pcl::PointXYZ>& points) 
{
    // 初始化用于计算的变量
    float A = 0, B = 0, C = 0, D = 0, E = 0, F = 0;

    // 构造矩阵方程 Ax + By + C = 0 和 x^2 + y^2的方程
    for (const auto& pt : points) 
    {
        float x = pt.x;
        float y = pt.y;
        A += x;
        B += y;
        C += 1;
        D += x * x;
        E += x * y;
        F += y * y;
    }

    // 计算分母，避免分母为零的情况
    float denom = points.size() * D - A * A;
    if (denom == 0)
    {
        std::cerr << "Denominator is zero; cannot fit a circle." << std::endl;
        return { 0, 0, 0 };// 返回空的圆
    }

    // 计算圆心坐标
    float cx = (B * D - A * E) / denom;
    float cy = (A * E - B * D) / denom;

    // 计算半径
    float r = 0;
    for (const auto& pt : points) 
    {
        r += std::sqrt(std::pow(pt.x - cx, 2) + std::pow(pt.y - cy, 2));
    }
    r /= points.size();// 求平均半径

    return { cx, cy, r };// 返回拟合的圆
}

int main(int argc, char** argv) 
{
    // 读取点云文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::string file_path = "F:\\convex.pcd";  // 替换为你的点云文件路径

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) 
    {
        PCL_ERROR("Couldn't read PCD file \n");
        return (-1);
    }

    std::cout << "Loaded point cloud with " << cloud->points.size() << " points.\n";

    //  使用凸包算法提取二维点云的凸多边形边界
    pcl::ConvexHull<pcl::PointXYZ> chull;// 创建凸包对象
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull_points(new pcl::PointCloud<pcl::PointXYZ>);// 用于存储凸包点云
    chull.setInputCloud(cloud);// 设置输入点云
    chull.reconstruct(*hull_points);// 计算凸包并保存结果

    std::cout << "Convex hull points: " << hull_points->points.size() << std::endl;

    // 提取二维点云坐标（x, y）
    std::vector<pcl::PointXYZ> points_2d;
    for (const auto& pt : hull_points->points) 
    {
        points_2d.push_back(pt);// 将凸包点存入二维坐标向量
    }

    // 2. 拟合外接圆
    Circle circle = fitCircle(points_2d);// 拟合外接圆
    std::cout << "Fitted Circle: Center = (" << circle.x << ", " << circle.y << "), Radius = " << circle.r << std::endl;

    // 3. 可视化凸包点云和拟合的外接圆
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Convex Hull Viewer"));

    // 添加拟合圆的可视化（通过画多个圆周上的点来表示）
    pcl::PointCloud<pcl::PointXYZ>::Ptr circle_points(new pcl::PointCloud<pcl::PointXYZ>);
    int num_points = 100;
    for (int i = 0; i < num_points; ++i) 
    {
        float theta = 2 * M_PI * i / num_points;// 均匀分布的角度
        pcl::PointXYZ pt;
        pt.x = circle.x + circle.r * std::cos(theta);// 计算圆周点的 x 坐标
        pt.y = circle.y + circle.r * std::sin(theta);// 计算圆周点的 y 坐标
        pt.z = 0;
        circle_points->points.push_back(pt);// 添加圆周点到点云
    }
    viewer->addPointCloud<pcl::PointXYZ>(circle_points, "fitted circle");// 添加圆的点云到可视化器

    // 4. 保存拟合圆的点云到PCD文件
    //std::string output_file = "F:\\fitted_circle_points.pcd";  // 输出文件路径
    //if (pcl::io::savePCDFileASCII(output_file, *circle_points) == -1) {
    //    std::cerr << "Failed to save fitted circle points." << std::endl;
    //    return (-1);
    //}
    //std::cout << "Saved fitted circle points to '" << output_file << "'." << std::endl;

    // 启动可视化器
    while (!viewer->wasStopped()) 
    {
        viewer->spinOnce(100);
    }

    return 0;
}