PCL模型滤波（model_outlier_removal）

1.原理

生成一个模型，通过点云各点和此模型之间的相似程度（通过点云点和模型之间的距离阈值判断相似程度：小于距离阈值的即为相似点，保留；大于距离阈值的即为非相似点，不保留），筛选同模型相似的点云点，生成结果点云。

（1）生成模型（PCL支持模型类型附在文末）。

（2）通过距离阈值判断模型到点云点之间的距离，筛选小于该用户设定的距离阈值的点云。

（3）将筛选出来的点云生成新点云。

原理介绍如下图所示（以平面模型滤波举例）：

想象平面A为我们设定好的平面模型，这些点为散列点云，距离阈值设定为m。

我们将所有的散列点计算得到该点到平面A的距离n，如果n<m，则保留，如果n>m则不保留。

2.使用场景

例如提取地面；还有其他裁剪某些有特殊形状的物体，诸如用圆柱体模型裁剪点云得到一棵树的点云等等。

3.注意事项

本示例代码中添加了三个点计算平面方程的C++实现，可自行取用。

// 计算三维平面公式Ax + By + Cz + D = 0中的各个系数（三点确定一个平面
double x1 = 143.01;    double y1 = 49.68;    double z1 = 24.53;
double x2 = 159.08;    double y2 = 44.23;    double z2 = 24.67;
double x3 = 161.39;    double y3 = 51.24;    double z3 = 24.5;
double A = (y3 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
double B = (x3 - x1) * (z2 - z1) - (x2 - x1) * (z3 - z1);
double C = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
double D = -(A * x1 + B * y1 + C * z1);

4.关键函数

（1）设置距离阈值：判断模型到点云点之间的距离，已确定是否保留该点云点。

setThreshold()

（2）设置模型类型（可设置的类型附在文末）

setModelType()

5.代码

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/model_outlier_removal.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main()
{
    /****************模型滤波********************/
    // 原始点云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::io::loadPCDFile("D:/code/csdn/data/JZDM.pcd", *cloud);   // 加载原始点云数据
    // 结果点云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);

    // 设置模型
    // 计算三维平面公式ax+by+cz+d = 0中的各个系数（三点确定一个平面）   测试示例使用，平常如果知道平面系数不用计算，如果不知道，取三个点用以下公式计算即可
    double x1 = 143.01;    double y1 = 49.68;    double z1 = 24.53;
    double x2 = 159.08;    double y2 = 44.23;    double z2 = 24.67;
    double x3 = 161.39;    double y3 = 51.24;    double z3 = 24.5;
    double A = (y3 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
    double B = (x3 - x1) * (z2 - z1) - (x2 - x1) * (z3 - z1);
    double C = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1);
    double D = -(A * x1 + B * y1 + C * z1);

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);           // 模型系数
    coefficients->values.resize(4);
    coefficients->values[0] = A;                                                    // 设置各系数
    coefficients->values[1] = B;
    coefficients->values[2] = C;
    coefficients->values[3] = D;

    // 模型滤波
    pcl::ModelOutlierRemoval<pcl::PointXYZRGB> filter;
    filter.setModelCoefficients(*coefficients);                                     // 设置模型系数
    filter.setThreshold(10.0);                                                      // 距离阈值（即实际点云点到模型面之间的距离）
    filter.setModelType(pcl::SACMODEL_PLANE);                                       // 设置SAC模型类型
    filter.setInputCloud(cloud);
    filter.filter(*cloud_filtered);

    /****************展示********************/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> view_raw(new pcl::visualization::PCLVisualizer("raw"));
    view_raw->addPointCloud<pcl::PointXYZRGB>(cloud, "raw cloud");
    view_raw->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw cloud");

    boost::shared_ptr<pcl::visualization::PCLVisualizer> view_filtered(new pcl::visualization::PCLVisualizer("filter"));
    view_filtered->addPointCloud<pcl::PointXYZRGB>(cloud_filtered, "filtered cloud");
    view_filtered->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "filtered cloud");

    while (!view_raw->wasStopped())
    {
        view_raw->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    while (!view_filtered->wasStopped())
    {
        view_filtered->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}

6.结果展示

7.附PCL支持模型类型

• SACMODEL_PLANE 平面模型
• SACMODEL_LINE 直线模型
• SACMODEL_CIRCLE2D 2d圆模型
• SACMODEL_CIRCLE3D 3d圆模型
• SACMODEL_SPHERE 球模型
• SACMODEL_CYLINDER 圆柱模型
• SACMODEL_CONE 圆锥模型
• SACMODEL_TORUS 圆环模型，目前还未实现
• SACMODEL_PARALLEL_LINE　和给定轴平行的直线
• SACMODEL_PERPENDICULAR_PLANE 和给定轴垂直的平面
• SACMODEL_NORMAL_PLANE　拟合一个平面，用于拟合的内点的法相量与最后输出的平面之间法相量的夹角必须小于某个阈值
• SACMODEL_NORMAL_SPHERE　拟合一个球，用于拟合的内点的法相量与最后输出的球面之间法相量的夹角必须小于某个阈值
• SACMODEL_PARALLEL_PLANE 拟合出和给定轴平行的平面
• SACMODEL_NORMAL_PARALLEL_PLANE 拟合一个平面，其约束条件为 SACMODEL_NORMAL_PLANE + SACMODEL_PERPENDICULAR_PLANE。
• SACMODEL_STICK　棒状模型