三维点云使用pcl实现RANSAC平面分割

小白每日一练！

点云分割

分割是将点云划分为多个部分的过程，每个部分代表不同的物体或表面。在这里，我们使用RANSAC算法来识别和分离平面。（以ModelNet40为例）

完整代码放在最后面啦！！测试好了可以直接使用！！

RANSAC算法

RANSAC算法是一种用于从一组包含异常数据的观测数据中估计数学模型的迭代方法。它通过随机选择数据点来估计模型参数，然后计算符合该模型的数据点的数量，以此来评估模型的准确性。

这样说可能很难理解，我们把它应用到实际场景。想象一下，RANSAC算法就像一个挑选好豆子的过程，你有一堆各种各样的豆子，这些豆子代表数据点。在这些豆子中，大部分是好的，但也有一些坏豆子（异常值）。你的任务是找出所有好豆子，并将它们分类。

1. 随机抓取：首先，你闭上眼睛，随机从豆子堆里抓一把豆子（随机选择样本）。

2. 检查形状：然后，你检查这些豆子的形状是否大致相同（估计模型参数）。如果它们都是圆形的，那么你可能认为这是一把好豆子。

3. 挑选相似豆子：接下来，你把这把豆子放在一边，然后一颗一颗地检查剩下的豆子。如果其他豆子看起来和这把豆子很像（误差小于某个阈值），你也把它们放到一边（共识集）。

4. 重复尝试：你可能需要重复这个过程多次，因为有时候你随机抓到的第一把豆子可能恰好都是坏豆子。每次你都会尝试找到更多的好豆子（迭代）。

5. 找到最多好豆子的那一堆：最终，你会找到一堆看起来最像好豆子的豆子（具有最多内点的模型）。这堆豆子就是你的答案。

6. 结束：当你尝试了很多次，或者觉得已经找到了足够多的好豆子时，你就停止这个过程。

所以说，RANSAC的关键在于它能够容忍一些坏豆子（异常值），并且通过不断的尝试和挑选，最终找到大多数好豆子（数据集的真实模型）。这个过程可能需要一些时间和耐心，但最终能够有效地从混乱的数据中找出有用的信息。

实现步骤

创建平面分割对象

这里创建了一个SACSegmentation对象用于平面分割，inliers用于存储平面内点的索引，coefficients用于存储平面模型的系数。

pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

设置分割参数

• setOptimizeCoefficients(true): 优化平面模型系数。
• setModelType(pcl::SACMODEL_PLANE): 指定模型类型为平面。
• setMethodType(pcl::SAC_RANSAC): 使用RANSAC算法进行分割。
• setMaxIterations(1000): 设置最大迭代次数。
• setDistanceThreshold(0.01): 设置距离阈值，用于判断点是否属于平面。

seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);

创建提取索引对象

这个对象用于从点云中提取或移除索引对应的点。

pcl::ExtractIndices<pcl::PointXYZ> extract;

创建可视化对象

这里创建了一个PCLVisualizer对象用于3D可视化。

pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);

 预定义一些颜色

这里定义了一个颜色列表，用于给不同的平面着色。

std::vector<std::tuple<int, int, int>> colors = { /* ... */ };

平面分割循环

这个循环不断执行平面分割，直到剩余的点云大小小于原始点云的10%。

int i = 0, nr_points = static_cast<int>(cloud->points.size());
while (cloud->points.size() > 0.1 * nr_points) {
    // 分割平面
    // ...
}

分割平面

设置输入点云并执行分割，将平面内点的索引存储在inliers中，平面模型的系数存储在coefficients中。

seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

提取平面内的点

使用ExtractIndices对象提取平面内的点。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_plane);

可视化平面

将提取的平面添加到可视化器中，并设置颜色和大小。

保存平面

将平面保存为PCD文件。

std::stringstream ss;
ss << "plane_" << i << ".pcd";
pcl::io::savePCDFileASCII(ss.str(), *cloud_plane);

移除平面内的点

设置ExtractIndices对象的setNegative参数为true，这样它就会从输入点云中移除平面内点的索引对应的点，从而保留非平面点。

extract.setNegative(true);
extract.filter(*cloud);

 显示剩余点云

这段代码将剩余的点云（即非平面点）以白色显示在可视化器中，并设置点的大小。

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> remaining_color(cloud, 255, 255, 255); // White for remaining points
viewer->addPointCloud<pcl::PointXYZ>(cloud, remaining_color, "remaining_cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "remaining_cloud");

进入可视化主循环

这个循环使可视化器持续运行，直到用户关闭窗口。

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

示例代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vector>

int main(int argc, char** argv) {
    // 创建点云对象并加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("modelnet40_desk_0003.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read the point cloud file\n");
        return -1;
    }

    // 创建点云平面分割对象
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

    // 设置分割参数
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.01);  // 距离阈值，适用于点云数据的尺度

    pcl::ExtractIndices<pcl::PointXYZ> extract;

    // 创建可视化对象
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);

    // 预定义一些颜色
    std::vector<std::tuple<int, int, int>> colors = {
        std::make_tuple(255, 0, 0),   // Red
        std::make_tuple(0, 255, 0),   // Green
        std::make_tuple(0, 0, 255),   // Blue
        std::make_tuple(255, 255, 0), // Yellow
        std::make_tuple(255, 0, 255), // Magenta
        std::make_tuple(0, 255, 255), // Cyan
        std::make_tuple(255, 165, 0), // Orange
        std::make_tuple(128, 0, 128)  // Purple
    };

    int i = 0, nr_points = static_cast<int>(cloud->points.size());
    while (cloud->points.size() > 0.1 * nr_points) {
        // 分割平面
        seg.setInputCloud(cloud);
        seg.segment(*inliers, *coefficients);
        if (inliers->indices.size() == 0) {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

         //输出平面系数
        std::cout << "Plane coefficients: " << coefficients->values[0] << " "
            << coefficients->values[1] << " "
            << coefficients->values[2] << " "
            << coefficients->values[3] << std::endl;

        // 提取平面内的点
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
        extract.setInputCloud(cloud);
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*cloud_plane);
        std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

        // 从颜色列表中获取颜色
        int r, g, b;
        std::tie(r, g, b) = colors[i % colors.size()];
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_plane, r, g, b);
        viewer->addPointCloud<pcl::PointXYZ>(cloud_plane, single_color, "plane_" + std::to_string(i));
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane_" + std::to_string(i));

        // 保存提取到的平面
        std::stringstream ss;
        ss << "plane_" << i << ".pcd";
        pcl::io::savePCDFileASCII(ss.str(), *cloud_plane);

        // 移除平面内的点，保留其余点
        extract.setNegative(true);
        extract.filter(*cloud);
        i++;
    }

    // 显示剩余点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> remaining_color(cloud, 255, 255, 255); // White for remaining points
    viewer->addPointCloud<pcl::PointXYZ>(cloud, remaining_color, "remaining_cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "remaining_cloud");

    // 进入可视化主循环
    while (!viewer->wasStopped()) {
        viewer->spinOnce(100);
    }

    return 0;
}

当然了，如果不需要保存分割后的平面，可以删掉这一段。

        // 保存提取到的平面
        std::stringstream ss;
        ss << "plane_" << i << ".pcd";
        pcl::io::savePCDFileASCII(ss.str(), *cloud_plane);