聊一聊点云PCL中常用的高级采样方法

最新推荐文章于 2024-07-01 21:15:56 发布

自动驾驶之心

最新推荐文章于 2024-07-01 21:15:56 发布

阅读量2.3k

点赞数 6

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247521925&idx=4&sn=3911505d37c6cf8863a3807c4c009d15&chksm=ceb8bb4cf9cf325afeb355f7ae70f35c687fb8b9fec6e4ac53d5405e317ac0306cc676645463&scene=126&sessionid=0

版权

作者 | 敢敢のwings 编辑 | 古月居

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【多传感器融合】技术交流群

后台回复【多传感器融合综述】获取图像/激光雷达/毫米波雷达融合综述等干货资料！

0. 简介

我们在使用PCL时候，常常不满足于常用的降采样方法，这个时候我们就想要借鉴一些比较经典的高级采样方法。这一讲我们将对常用的高级采样方法进行汇总，并进行整理，来方便读者完成使用

1.基础下采样

1.1 点云随机下采样

点云下采样是对点云以一定的采样规则重新进行采样，目的是在保证点云整体几何特征不变的情况下，降低点云的密度，进而可以降低相关处理的数据量和算法复杂度。

随机下采样顾名思义，随机下采样就似乎在原始点云中随机采样一定点数的点。这种方法最终得到的点云数量也是固定的。

pcl::PointCloud<PointT>::Ptr cloud_sub(new pcl::PointCloud<PointT>);    //随机下采样点云
    pcl::RandomSample<PointT> rs;    //创建滤波器对象
    rs.setInputCloud(cloud);                //设置待滤波点云
    rs.setSample(20000);                    //设置下采样点云的点数
    //rs.setSeed(1);                        //设置随机函数种子点
    rs.filter(*cloud_sub);                    //执行下采样滤波，保存滤波结果于cloud_sub

1.2 体素下采样

体素下采样的原理如图1所示，首先将点云空间进行网格化，也称体素化，即图1(b)，网格化后的每一个格子称为体素，在这些划分为一个个极小的格子中包含一些点，然后对这些点取平均或加权平均得到一个点，以此来替代原来网格中所有的点，即图1(c)中蓝色的点。显然，网格选取越大则采样之后的点云越少，处理速度变快，但会对原先点云过度模糊，网格选取越小，则作用相反。

pcl::VoxelGrid<pcl::PointXYZ> sor;    //创建体素网格采样处理对象
 sor.setInputCloud(cloud);             //设置输入点云
 sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置体素大小，单位：m
 sor.filter(*cloud_filtered);          //进行下采样

1.3 均匀采样

均匀采样的原理类似于体素化网格采样方法，同样是将点云空间进行划分，不过是以半径=r的球体，在当前球体所有点中选择距离球体中心最近的点替代所有点，注意，此时点的位置是不发生移动的。

球体半径选取越大则采样之后的点云越少，处理速度变快，但会对原先点云过度模糊，网格选取越小，则作用相反。

pcl::UniformSampling<pcl::PointXYZ> form;   // 创建均匀采样对象
form.setInputCloud(cloud);                  //设置输入点云
form.setRadiusSearch(0.02f);                //设置半径大小,单位:m
form.filter(*after_cloud);                  //执行滤波处理

1.4 增采样

增采样的特点是可极大的增加点云数据，但由于内插点的不确定性会导致最后输出的结果不一定准确。

//创建增采样对象
pcl::MovingLeastSquares<pcl::PointXYZ,pcl::PointXYZ> filter;    
filter.setInputCloud(cloud);                     //设置输入点云
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;  //定义搜索方法
filter.setSearchMethod(kdtree);                  //设置搜索方法
filter.setSearchRadius(0.03);    //设置搜索邻域的半径为3cm  
//Upsampling 采样的方法还有 DISTINCT_CLOUD, RANDOM_UNIFORM_DENSITY
filter.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::SAMPLE_LOCAL_PLANE);     //对点云进行上采样
filter.setUpsamplingRadius(0.03);    //设置采样半径大小，3cm
filter.setUpsamplingStepSize(0.02);  //设置采样步长大小，2cm
filter.process(*after_cloud);      //执行采样操作

1.5 滑动最小二乘法采样

滑动最小二乘法采样的原理是将点云进行了滑动最小二乘法的映射，使得输出的点云更加平滑。

pcl::PointCloud<pcl::PointNormal>::Ptr smoothedCloud(new pcl::PointCloud<pcl::PointNormal>);   //定义法线
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> filter;
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;  //定义搜索方法
filter.setInputCloud(cloud);    //设置输入点云
filter.setUpsamplingMethod();  //增加密度较小区域的密度对于holes的填补却无能为力，具体方法要结合参数使用
filter.setSearchRadius(10);// 用于拟合的K近邻半径。在这个半径里进行表面映射和曲面拟合。半径越小拟合后曲面的失真度越小，反之有可能出现过拟合的现象。
filter.setPolynomialFit(true);  //对于法线的估计是有多项式还是仅仅依靠切线。true为加多项式；false不加，速度较快
filter.setPolynomialFit(3);      // 拟合曲线的阶数
filter.setComputeNormals(true);  // 是否存储点云的法向量，true 为存储，false 不存储
filter.setSearchMethod(kdtree); //设置搜索方法
filter.process(*smoothedCloud); //处理点云并输出

2. 最远点采样(Farthest Point Sampling)

这个PCL代码目前已经集成到https://github.com/farthest_point_sampling.hpp。这里我们来单独看一下调用代码，这里可以看到PCL中支持直接调用farthest_sampling这个函数可以实现最远点采样。

最远点采样(Farthest Point Sampling)是一种非常常用的采样算法，由于能够保证对样本的均匀采样，被广泛使用，像3D点云深度学习框架中的PointNet++对样本点进行FPS采样再聚类作为感受野，3D目标检测网络VoteNet对投票得到的散乱点进行FPS采样再进行聚类，6D位姿估计算法PVN3D中用于选择物体的8个特征点进行投票并计算位姿。FPS算法原理：

1、输入点云有N个点，从点云中选取一个点P0作为起始点，得到采样点集合S={P0}；

2、计算所有点到P0的距离，构成N维数组L，从中选择最大值对应的点作为P1，更新采样点集合S={P0，P1}；

3、计算所有点到P1的距离，对于每一个点Pi，其距离P1的距离如果小于L[i]，则更新L[i] = d(Pi, P1)，因此，数组L中存储的一直是每一个点到采样点集合S的最近距离；

3、选取L中最大值对应的点作为P2，更新采样点集合S={P0，P1，P2}；

4、重复2-4步，一直采样到N’个目标采样点为止。

std::vector<pcl::PointCloud<pcl::PointXYZ>> input_point_clouds(1);
  std::vector<pcl::PointCloud<pcl::PointXYZ>> output_point_clouds;


  ASSERT_NE(pcl::io::loadPLYFile<pcl::PointXYZ>(STR(INPUT_POINT_CLOUD_PATH),
            input_point_clouds[0]), -1) << "Couldn't read file test point cloud file";
  farthest_sampling::samplePointCloudsCuda(input_point_clouds, output_point_clouds, 4096);
  boost::filesystem::path output_path = STR(OUTPUT_POINT_CLOUD_PATH);
  if (output_path.has_parent_path() && !boost::filesystem::exists(output_path.parent_path()))
  {
    boost::filesystem::create_directories(output_path.parent_path());
  }
  pcl::io::savePLYFile(STR(OUTPUT_POINT_CLOUD_PATH), output_point_clouds[0]);
  ASSERT_EQ(output_point_clouds[0].size(), 4096);

3. 法线空间采样

NormalSpaceSampling即：法线空间采样，它在法向量空间内均匀随机抽样，使所选点之间的法线分布尽可能大，结果表现为地物特征变化大的地方剩余点较多，变化小的地方剩余点稀少，可有效保持地物特征。实现方法如下：

1、首先计算每个点的K领域，然后计算点到领域点的法线夹角值，以此来近似达到曲率的效果并提高计算效率，因为曲率越大的地方，夹角值越大。

2、设置一个角度阈值，当点的领域夹角值大于阈值时被认为是特征明显的区域，其余区域为不明显区域。

3、对明显和不明显区域进行均匀采样，采样数分别为U_(1-V)和U_V,U是目标采样数，V是均匀采样性。

// 创建基于邻域的法向估计类对象
 // // 基于omp并行加速，需配置开启OpenMP
 // pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne; 
 // ne.setNumberOfThreads(10);
 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
 // 创建一个空的kdtree对象，并把它传递给法线估计对象，
 // 用于创建基于输入点云数据的邻域搜索kdtree
 pcl::search::KdTree<pcl::PointXYZ>::Ptr \
             tree(new pcl::search::KdTree<pcl::PointXYZ>());
 // 传入待估计法线的点云数据，智能指针
 ne.setInputCloud(cloud_src);
 // 传入kdtree对象，智能指针
 ne.setSearchMethod(tree);
 // 设置邻域搜索半径
 ne.setRadiusSearch(0.1f);    // 设置半径时，要考虑到点云空间间距
 // // 也可以设置最近邻点个数
 // ne.setKSearch(25);
 // 设置视点源点，用于调整点云法向（指向视点），默认（0，0，0）
 ne.setViewPoint(0,0,0);
 // 计算法线数据
 ne.compute(*cloud_normals);


 // 通过concatenateFields函数将point和normal组合起来形成PointNormal点云数据
 pcl::PointCloud<pcl::PointNormal>::Ptr \
             cloud_with_normal(new pcl::PointCloud<pcl::PointNormal>());
 pcl::PointCloud<pcl::PointNormal>::Ptr \
     cloud_with_normal_sampled(new pcl::PointCloud<pcl::PointNormal>());
 pcl::concatenateFields(*cloud_src, *cloud_normals, *cloud_with_normal);


 // 创建法向空间采样（模板）类对象
 pcl::NormalSpaceSampling<pcl::PointNormal, pcl::Normal> nss;
 // 设置xyz三个法向空间的分类组数，此处设置为一致，根据具体场景可以调整
 const int kBinNum = 8;
 nss.setBins(kBinNum, kBinNum, kBinNum);
 // 如果传入的是有序点云，此处可以尝试设置为true
 nss.setKeepOrganized(false);
 // 设置随机种子，这样可以保证同样的输入可以得到同样的结果，便于debug分析
 nss.setSeed(200);   // random seed
 // 传入待采样的点云数据
 nss.setInputCloud(cloud_with_normal);
 // 传入用于采样分析的法线数据，需与传入点云数据一一对应
 nss.setNormals(cloud_normals);
 // 设置采样总数，即目标点云的总数据量
 const float kSampleRatio = 0.1f;
 nss.setSample(cloud_with_normal->size()*kSampleRatio);
 // 执行采样并带出采样结果
 nss.filter(*cloud_with_normal_sampled);

4. 泊松盘采样

泊松盘采样（possion disk sampling）的特点是任何两个点的距离都不会隔得太近。

比如下图，左边是随机生成的点，右边是泊松盘采样生成的点。

具体流程如下:

1、设定好两个点之间最近的距离r，以及采样点所在空间的维度n，比如2维平面

2、在空间里生成足够多的网格，保证不接触的两个网格之间的点的距离大于r，并且网格数量足够多保证每个网格至多只需装一个采样点就能满足采样数量。为了最优化，一般取网格边长为r/\sqrt{n}。

3、随机生成一个点，再创建两个数组，第一个是处理数组，第二个是结果数组，即最终的输出数组。把这个点放进处理数组中和结果数组中。

4、如果处理数组非空，从中随机选择一个点，如下图的红点，并把这个点从处理数组中删除。如果处理数组是空的，直接输出结果数组并结束算法。

5、设定最小距离minr，比如r，最大距离maxr，比如2*r。以红点为中心生成一个圆环，如下图灰色圆环，在这个圆环中生成一个采样点，如下图蓝点。

#include <pcl/surface/poisson.h>
//泊松重建
cout << "begin poisson reconstruction" << endl;
Poisson<PointXYZRGBNormal> poisson;
//poisson.setDegree(2);
poisson.setDepth(8);
poisson.setSolverDivide (6);
poisson.setIsoDivide (6);


poisson.setConfidence(false);
poisson.setManifold(false);
poisson.setOutputPolygons(false);


poisson.setInputCloud(cloud_smoothed_normals);
PolygonMesh mesh;
poisson.reconstruct(mesh);

5. 非均匀体素采样

SamplingSurfaceNormal,将输入空间划分为网格,直到每个网格中最多包含N个点,并在每个网格中随机采样点。使用每个网格的N个点计算法线。在网格内采样的所有点都分配有相同的法线。

PointCloud <PointNormal>::Ptr incloud (new PointCloud <PointNormal> ());
PointCloud <PointNormal> outcloud;


//Creating a point cloud on the XY plane
for (float i = 0.0f; i < 5.0f; i += 0.1f)
{
  for (float j = 0.0f; j < 5.0f; j += 0.1f)
  {
    PointNormal pt;
    pt.x = i;
    pt.y = j;
    pt.z = 1;
    incloud->points.push_back (pt);
  }
}
incloud->width = 1;
incloud->height = uint32_t (incloud->points.size ());


pcl::SamplingSurfaceNormal <pcl::PointNormal> ssn_filter;
ssn_filter.setInputCloud (incloud);
ssn_filter.setRatio (0.3f);
ssn_filter.filter (outcloud);


// All the sampled points should have normals along the direction of Z axis
for (unsigned int i = 0; i < outcloud.points.size (); i++)
{
  EXPECT_NEAR (outcloud.points[i].normal[0], 0, 1e-3);
  EXPECT_NEAR (outcloud.points[i].normal[1], 0, 1e-3);
  EXPECT_NEAR (outcloud.points[i].normal[2], 1, 1e-3);
}

6. 半径滤波器采样

对整个输入迭代一次，对于每个点进行半径R邻域搜索，如果邻域点的个数低于某一阈值，则该点将被视为噪声点并被移除。

流程：读入点云→创建半径滤波器对象→设置离群点阈值→执行下采样→保存采样结果

pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_ptr(pcl_cloud);


boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcl_vg_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vg_cloud_ptr(pcl_vg_cloud);


boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcl_ror_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ror_cloud_ptr(pcl_ror_cloud);


//Use VoxelGrid to make points sparse
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (pcl_cloud_ptr);
sor.setLeafSize (0.08, 0.1, 0.1); 
sor.filter (*pcl_vg_cloud_ptr);


//Use RadiusOutlierRemoval to remove the point whitch is far away to others
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(pcl_vg_cloud_ptr);
outrem.setRadiusSearch(0.5);
outrem.setMinNeighborsInRadius (3);
outrem.filter (*pcl_ror_cloud_ptr); 


//transfrom and publish
sensor_msgs::PointCloud2Ptr msg_pointcloud(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*pcl_ror_cloud, *msg_pointcloud);
msg_pointcloud->header.frame_id = optical_frame_id_[RS_STREAM_DEPTH];;  
msg_pointcloud->header.stamp = ros::Time::now();

往期回顾

史上最全综述 | 3D目标检测算法汇总！（单目/双目/LiDAR/多模态/时序/半弱自监督）

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称

自动驾驶之心

关注

6
点赞
踩
38

收藏

觉得还不错? 一键收藏
0
评论
聊一聊点云PCL中常用的高级采样方法

作者|敢敢のwings 编辑| 古月居点击下方卡片，关注“自动驾驶之心”公众号ADAS巨卷干货，即可获取点击进入→自动驾驶之心【多传感器融合】技术交流群后台回复【多传感器融合综述】获取图像/激光雷达/毫米波雷达融合综述等干货资料！0. 简介我们在使用PCL时候，常常不满足于常用的降采样方法，这个时候我们就想要借鉴一些比较经典的高级采样方法。这一讲我们将对常用的高级采样方法进行汇总，并进行整理...
复制链接

扫一扫