PCL基于6D协方差的点云采样（covariance_sampling）

PCL专栏目录及须知-CSDN博客

1.原理

协方差矩阵

（1）计算点云法线。

（2）通过带有法线的点云，计算当前点云中每个点的6D协方差矩阵。

（3）对每次计算出的协方差矩阵进行特征分解，得到点云中每个点的6D主方向和形状信息。

（4）基于这些主方向和形状信息，选择具有代表性的点作为采样点。

（5）所有稳定性强、具有代表性的采样点组合成新点云，以方便进行ICP配准。

如何理解是否具有代表性：

邻近的几个点云的主方向、位姿不相似，那么它们不能被代表；

而如果邻近的几个点云的主方向、位姿均相似，那么选取其中一个代表其他点云就可以。

如下图（所有点均位置非常接近）：

以法向量举例，A点、B点即可代表其所在那一列的点的法向量信息。

2.使用场景

可用于点云ICP配准之前的前置处理步骤，使待配准的点更加的具有针对性，且数量减少，配准时速度变快。

3.注意事项

记得算完之后用computeConditionNumber ()这个函数看看ICP配准的稳定性，别拷贝完代码直接就往工作里贴。点云不同它参数不同。

4.关键函数

（1）设置KD树的K领域搜索的领域点个数；领域点越多，搜索的范围越大，计算精度越高，搜索的越慢。

setKSearch()

（2）设置采样点的个数；采样点的个数决定了你最后生成出来的结果点云是有多少个点；参数建议使用【原始点云.size()/某整数】

setNumberOfSamples()

（3）衡量最后结果的稳定性，返回值越接近于1，ICP配准时稳定性越好。

double computeConditionNumber()

5.代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/covariance_sampling.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>


int main()
{
    /****************基于6D协方差的点云采样********************/
    // 原始点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());                    // 需使用PointXYZ格式的数据
    pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud);   // 加载原始点云数据
    // 法向量点云
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal>());
    // 结果点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

    // 点云法线计算
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;                                             // 点云法线生成器（OMP加速）
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);                        // 法线信息
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());             // kdtree
    n.setNumberOfThreads(10);                                                                           // OMP线程数
    n.setInputCloud(cloud);                                                                             // 输入点云
    n.setSearchMethod(tree);                                                                            // 通过kdtree计算
    n.setKSearch(10);                                                                                   // k值搜索（k值个数）
    n.compute(*normals);                                                                                // 计算法向量
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);                                           // 点云和法向量拼接得到新点云

    // 6D协方差滤波
    pcl::CovarianceSampling<pcl::PointNormal, pcl::PointNormal> filtered;
    filtered.setInputCloud(cloud_normals);                                                              // 输入点云
    filtered.setNormals(cloud_normals);                                                                 // 输入点云法向量
    filtered.setNumberOfSamples(cloud_normals->size() / 2);                                             // 采样点的个数（这个参数决定了你要使用多少个点进行6D协方差特征量的计算）
    pcl::IndicesPtr filtered_indices(new pcl::Indices());                                               // 结果点索引
    filtered.filter(*filtered_indices);                                                                 // 计算
    pcl::copyPointCloud(*cloud, *filtered_indices, *cloud_filtered);                                    // 通过索引将原始点云复制到新点云中

    double cond_num_walls = filtered.computeConditionNumber();                                          // 采样后点云输入ICP算法时的稳定性

        /****************展示********************/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> view_raw(new pcl::visualization::PCLVisualizer("raw"));
    view_raw->addPointCloud<pcl::PointXYZ>(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::PointXYZ>(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.结果展示