pclpy Ransac平面分割算法输出的索引从点云中提取点云的子集

一、算法原理

1、Ransac介绍

RANSAC(RAndom SAmple Consensus,随机采样一致)算法是从一组含有"外点"(outliers)的数据中正确估计数学模型参数的迭代算法。"外点"一般指的是数据中的噪声，比如说匹配中的误匹配和估计曲线中的离群点。所以，RANSAC也是一种"外点"检测算法。RANSAC算法是一种不确定算法，它只能在一种概率下产生结果，并且这个概率会随着迭代次数的增加而加大（之后会解释为什么这个算法是这样的）。

RANSAC主要解决样本中的外点问题，最多可处理50%的外点情况。

范例

可以简单总结为以下步骤：

N：样本个数 K：求解模型需要的最少的点的个数（对于直线拟合来说就是两个点，对于计算Homography矩阵就是四个点）

随机采样K个点

对该K个点拟合模型

计算其他点到拟合模型的距离。如果小于一定阈值，该点被当作内点，统计内点个数

重复M次，选择内点数最多的模型

利用所有的内点重新估计模型（可选）

RANSAC用于拟合直线：

1.随机选取K = 2 ，2个点：

2.拟合一条直线：

3.统计内点个数，内点为绿色，此时的内点个数为9(小于一定阈值计算为内点)：

4.重复上述过程M次，找到内点数最大的模型（继续随机选点根据k=数目进行选点）：

5.利用所有的内点重新估计直线：

二、代码

python 复制代码

from pclpy import pcl

def compareCloudShow(cloud1, cloud2):
    """
    Args:在一个窗口生成2个窗口可视化点云
        cloud1: 点云数据1
        cloud2: 点云数据2
    """
    viewer = pcl.visualization.PCLVisualizer("viewer")  # 建立可刷窗口对象 窗口名 viewer
    v0 = 1  # 设置标签名（0, 1标记第一个窗口）
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v0)  # 创建一个可视化的窗口
    viewer.setBackgroundColor(0.0, 0.0, 0.0, v0)  # 设置窗口背景为黑色
    single_color = pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud1, 255.0, 0, 0.0)  # 将点云设置为红色
    viewer.addPointCloud(cloud1,          # 要添加到窗口的点云数据。
                         single_color,    # 指定点云的颜色
                         "sample cloud1",  # 添加的点云命名
                         v0)  # 点云添加到的视图

    v1 = 2  # 设置标签名（2代表第二个窗口）
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v1)  # 创建一个可视化的窗口
    viewer.setBackgroundColor(255.0, 255.0, 255.0, v1)  # 设置窗口背景为白色
    single_color = pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud2, 0.0, 255.0, 0.0)  # 将点云设置为绿色
    viewer.addPointCloud(cloud2,  # 要添加到窗口的点云数据。
                         single_color,  # 指定点云的颜色
                         "sample cloud2",  # 添加的点云命名
                         v1)  # 点云添加到的视图

    # 设置点云窗口（可移除对点云可视化没有影响）
    viewer.setPointCloudRenderingProperties(0,  # 设置点云点的大小
                                            1,  # 点云像素
                                            "sample cloud1",  # 识别特定点云
                                            v0)  # 在那个窗口可视化
    viewer.setPointCloudRenderingProperties(0,  # 设置点云点的大小
                                            1,  # 点云像素
                                            "sample cloud2",  # 识别特定点云
                                            v1)  # 在那个窗口可视化
    viewer.addCoordinateSystem(1.0)  # 设置坐标轴 坐标轴的长度为1.0
    # 窗口建立
    while not viewer.wasStopped():
        viewer.spinOnce(10)


if __name__ == '__main__':
    # 读取点云数据
    cloud = pcl.PointCloud.PointXYZ()
    reader = pcl.io.PCDReader()
    reader.read('res/table_scene_lms400.pcd', cloud)
    print('点云数目：', cloud.size())

    # 创建sor滤波器 参考 pclpy SOR去除异常值(统计滤波) pclpy专栏中
    cloud_filtered = pcl.PointCloud.PointXYZ()
    sor = pcl.filters.StatisticalOutlierRemoval.PointXYZ()  # 创建sor处理对象
    sor.setInputCloud(cloud)  # 将cloud处理
    sor.setMeanK(50)  # 每个点要分析的邻居数
    sor.setStddevMulThresh(1.0)  # 距离查询点的平均距离大于1个标准差的点都将被标记为离群值并删除
    sor.filter(cloud_filtered)  # sor处理后的点云保存在这里(内点)

    # 可视化滤波效果
    compareCloudShow(cloud, cloud_filtered)  # 参考 pclpy 可视化点云（多窗口可视化、单窗口多点云可视化） pclpy在专栏中

    coeffs = pcl.ModelCoefficients()  # 存储估计的平面参数
    inliers = pcl.PointIndices()  # 存储平面模型的内点索引
    # 创建分割object
    seg = pcl.segmentation.SACSegmentation.PointXYZ()
    # 可选项
    seg.setOptimizeCoefficients(True)
    # 设置
    seg.setModelType(0)  # 0平面模型
    seg.setMethodType(0)  # 表示 RANSAC 算法  open3d 平面分割（Ransac算法） 专栏open3d
    seg.setMaxIterations(1000)  # 设置 RANSAC 算法的最大迭代次数为 1000。
    seg.setDistanceThreshold(0.01)  # 设置平面模型的距离阈值为 0.01，用于判断点是否为内点（inliers）

    # 创建滤波object
    extract = pcl.filters.ExtractIndices.PointXYZ()
    nr_points = cloud_filtered.size()  # 获得点云数目
    while cloud_filtered.size() > nr_points * 0.3:
        # 从保留的点云中分割最大的平面成分
        seg.setInputCloud(cloud_filtered)  # 将滤波后的点云数据设置为分割器的输入
        seg.segment(inliers, coeffs)  # 分割后的内点索引保存在 inliers 中，将平面模型系数保存在 coeffs
        if len(inliers.indices) == 0:
            print('无法对给定数据集估计平面模型。')
            break

        # 提取内点(平面成分)
        extract.setInputCloud(cloud_filtered)  # 从点云中提取指定索引的点 和 open3d 中的select_index_by()一样
        extract.setIndices(inliers)  # 将计算索引进行装填
        extract.setNegative(False)  # 获得内点
        cloud_p = pcl.PointCloud.PointXYZ()
        extract.filter(cloud_p)
        # 可视化提取出来的平面
        compareCloudShow(cloud_filtered, cloud_p)
        print("点云数目：", cloud_p.size())

        # 再次滤波，提取外点（非平面成分）
        extract.setNegative(True)   # 获得外点
        cloud_f = pcl.PointCloud.PointXYZ()  
        extract.filter(cloud_f)
        cloud_filtered.swap(cloud_f)  # 等价于cloud_filtered = cloud_f