点云学习笔记22——给点云轮廓拟合成直线后给边界直线编号

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/pcl_visualizer.h>

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";

    // 1. 使用RANSAC算法拟合最佳平面
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

    // 设置RANSAC模型为平面模型
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.001);  // 阈值，控制点到平面的最大距离
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.size() == 0)
    {
        PCL_ERROR("Couldn't estimate a planar model for the given dataset.\n");
        return (-1);
    }

    std::cout << "Plane coefficients: "
        << coefficients->values[0] << " "
        << coefficients->values[1] << " "
        << coefficients->values[2] << " "
        << coefficients->values[3] << std::endl;

    // 2. 将点云投影到拟合的平面上
    pcl::PointCloud<pcl::PointXYZ>::Ptr projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    proj.setInputCloud(cloud);
    proj.setModelCoefficients(coefficients);
    proj.filter(*projected_cloud);

    std::cout << "Projected cloud size: " << projected_cloud->points.size() << std::endl;

    // 3. 使用凸包算法提取二维点云的凸多边形边界
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull_points(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ConvexHull<pcl::PointXYZ> chull;
    chull.setInputCloud(projected_cloud);
    chull.reconstruct(*hull_points);

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

    // 可视化凸包边界
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(hull_points, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(hull_points, 255, 0, 0), "hull cloud");

    // 连接凸包的点形成线条并添加编号
    pcl::PointCloud<pcl::PointXYZ>::Ptr hull_lines(new pcl::PointCloud<pcl::PointXYZ>());
    for (size_t i = 0; i < hull_points->points.size(); ++i)
    {
        hull_lines->points.push_back(hull_points->points[i]);
        // 在每个点上添加编号标签
        viewer->addText3D(std::to_string(i + 1), hull_points->points[i], 10, 1.0, 1.0, 1.0, "label" + std::to_string(i + 1));
    }

    viewer->addPolygon<pcl::PointXYZ>(hull_lines, "convex hull");

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }

    return 0;
}