PCL 实战记录 （一）
目录
-
- PCL 实战记录 （一）
- 1、读取、显示、保存
- 2、KD-TREE 搜索
- 3、八叉树 点云采样
- 4、OCTREE 空间分割
- 5、滤波
- 6、采样
-
- 6.1 下采样
- 6.2 均匀采样
- 6.3 增采样
- 7、ICP 配准
- 8、点云分割
-
- 8.1 平面点云
- 8.2 区域生长分割
- 8.3 最小切线分割
- 8.4 欧式聚类分割
- 8.5 渐进形态滤波器
在学习、实战过程中，可参考PCL官方文档，链接：
PCL官方文档：PCL官方文档链接
1、读取、显示、保存
#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>using namespace std;int main()
{
//*********************************************************************************
//***************************read PLY file*****************************************
//*********************************************************************************cout << "read ply file...\n";pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>());pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>());pcl::io::loadPLYFile("1.ply", *target);pcl::io::loadPLYFile("2.ply", *source);//去除无效点std:vector<int> index;pcl::removeNaNFromPointCloud(*source, *source, index);pcl::removeNaNFromPointCloud(*target, *target, index);//*********************************************************************************
//********************visualization pointcloud*************************************
//*********************************************************************************cout << "Visualization_PointCloud...\n";boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer(new pcl::visualization::PCLVisualizer("Ransac"));viewer->setBackgroundColor(0, 0, 0);//创建窗口int vp;viewer->createViewPort(0.0, 0.0, 1.0, 1.0, vp);//设置点云颜色pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 255, 255);viewer->addPointCloud<pcl::PointXYZ>(source, source_color, "source", vp);viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target", vp);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target");viewer->spin();//*********************************************************************************
//**************************save pointcloud*************************************
//*********************************************************************************pcl::io::savePCDFileASCII("test_pcd.pcd",source);std::cerr<<"Saved "<<source.points.size()<<" data points to test_pcd.pcd."<<std::endl;for(size_t i=0;i<source.points.size();++i)std::cerr<<" "<<source.points[i].x<<" "<<source.points[i].y<<" "<<source.points[i].z<<std::endl;
}2、KD-TREE 搜索
#include <pcl/point_cloud.h> //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件#include <iostream>
#include <vector>
#include <ctime>int
main (int argc, char** argv)
{srand (time (NULL)); //用系统时间初始化随机种子//创建一个PointCloud<pcl::PointXYZ>pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);// 随机点云生成cloud->width = 1000; //此处点云数量cloud->height = 1; //表示点云为无序点云cloud->points.resize (cloud->width * cloud->height);for (size_t i = 0; i < cloud->points.size (); ++i) //循环填充点云数据{cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);}//创建KdTreeFLANN对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;//设置搜索空间kdtree.setInputCloud (cloud);//设置查询点并赋随机值pcl::PointXYZ searchPoint;searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);// K 临近搜索//创建一个整数（设置为10）和两个向量来存储搜索到的K近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方int K = 10;std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方//打印相关信息std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with K=" << K << std::endl;if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索{//打印所有近邻坐标for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x << " " << cloud->points[ pointIdxNKNSearch[i] ].y << " " << cloud->points[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;}/**********************************************************************************下面的代码展示查找到给定的searchPoint的某一半径（随机产生）内所有近邻，重新定义两个向量pointIdxRadiusSearch pointRadiusSquaredDistance来存储关于近邻的信息********************************************************************************/// 半径 R内近邻搜索方法std::vector<int> pointIdxRadiusSearch; //存储近邻索引std::vector<float> pointRadiusSquaredDistance; //存储近邻对应距离的平方float radius = 256.0f * rand () / (RAND_MAX + 1.0f); //随机的生成某一半径//打印输出std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with radius=" << radius << std::endl;if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法{for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x << " " << cloud->points[ pointIdxRadiusSearch[i] ].y << " " << cloud->points[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;}return 0;
}
3、八叉树 点云采样
#include <pcl/point_cloud.h> // 点云类型
#include <pcl/point_types.h> //点数据类型
#include <pcl/io/openni_grabber.h> //点云获取接口类
#include <pcl/visualization/cloud_viewer.h> //点云可视化类#include <pcl/compression/octree_pointcloud_compression.h> //点云压缩类#include <stdio.h>
#include <sstream>
#include <stdlib.h>#ifdef WIN32
# define sleep(x) Sleep((x)*1000)
#endifclass SimpleOpenNIViewer
{
public:SimpleOpenNIViewer () :viewer (" Point Cloud Compression Example"){}
/************************************************************************************************在OpenNIGrabber采集循环执行的回调函数cloud_cb_中，首先把获取的点云压缩到stringstream缓冲区，下一步就是解压缩，它对压缩了的二进制数据进行解码，存储在新的点云中解码了点云被发送到点云可视化对象中进行实时可视化
*************************************************************************************************/void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){if (!viewer.wasStopped ()){// 存储压缩点云的字节流对象std::stringstream compressedData;// 存储输出点云pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());// 压缩点云PointCloudEncoder->encodePointCloud (cloud, compressedData);// 解压缩点云PointCloudDecoder->decodePointCloud (compressedData, cloudOut);// 可视化解压缩的点云viewer.showCloud (cloudOut);}}
/**************************************************************************************************************在函数中创建PointCloudCompression类的对象来编码和解码，这些对象把压缩配置文件作为配置压缩算法的参数所提供的压缩配置文件为OpenNI兼容设备采集到的点云预先确定的通用参数集，本例中使用MED_RES_ONLINE_COMPRESSION_WITH_COLOR配置参数集，用于快速在线的压缩，压缩配置方法可以在文件/io/include/pcl/compression/compression_profiles.h中找到，在PointCloudCompression构造函数中使用MANUAL——CONFIGURATION属性就可以手动的配置压缩算法的全部参数
******************************************************************************************************************/void run (){bool showStatistics = true; //设置在标准设备上输出打印出压缩结果信息// 压缩选项详情在: /io/include/pcl/compression/compression_profiles.hpcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;// 初始化压缩和解压缩对象 其中压缩对象需要设定压缩参数选项，解压缩按照数据源自行判断PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();/***********************************************************************************************************下面的代码为OpenNI兼容设备实例化一个新的采样器，并且启动循环回调接口，每从设备获取一帧数据就回调函数一次，，这里的回调函数就是实现数据压缩和可视化解压缩结果。************************************************************************************************************///创建从OpenNI获取点云的抓取对象pcl::Grabber* interface = new pcl::OpenNIGrabber ();// 建立回调函数boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);//建立回调函数和回调信息的绑定boost::signals2::connection c = interface->registerCallback (f);// 开始接受点云的数据流interface->start ();while (!viewer.wasStopped ()){sleep (1);}interface->stop ();// 删除压缩与解压缩的实例delete (PointCloudEncoder);delete (PointCloudDecoder);}pcl::visualization::CloudViewer viewer;pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;};int
main (int argc, char **argv)
{SimpleOpenNIViewer v; //创建一个新的SimpleOpenNIViewer 实例并调用他的run方法v.run ();return (0);
}
4、OCTREE 空间分割
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>#include <iostream>
#include <vector>
#include <ctime>int
main (int argc, char** argv)
{srand ((unsigned int) time (NULL)); //用系统时间初始化随机种子// 八叉树的分辨率，即体素的大小float resolution = 32.0f;// 初始化空间变化检测对象pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> ); //创建点云实例cloudA生成的点云数据用于建立八叉树octree对象// 为cloudA点云填充点数据cloudA->width = 128; //设置点云cloudA的点数cloudA->height = 1; //无序点cloudA->points.resize (cloudA->width * cloudA->height); //总数for (size_t i = 0; i < cloudA->points.size (); ++i) //循环填充{cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);}// 添加点云到八叉树中，构建八叉树octree.setInputCloud (cloudA); //设置输入点云octree.addPointsFromInputCloud (); //从输入点云构建八叉树/***********************************************************************************点云cloudA是参考点云用其建立的八叉树对象描述它的空间分布，octreePointCloudChangeDetector类继承自Octree2BufBae类，Octree2BufBae类允许同时在内存中保存和管理两个octree，另外它应用了内存池该机制能够重新利用已经分配了的节点对象，因此减少了在生成点云八叉树对象时昂贵的内存分配和释放操作通过访问 octree.switchBuffers ()重置八叉树 octree对象的缓冲区，但把之前的octree数据仍然保留在内存中************************************************************************************/// 交换八叉树的缓冲，但是CloudA对应的八叉树结构仍然在内存中octree.switchBuffers ();//cloudB点云用于建立新的八叉树结构，与前一个cloudA对应的八叉树共享octree对象，同时在内存中驻留pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> ); //实例化点云对象cloudB// 为cloudB创建点云 cloudB->width = 128;cloudB->height = 1;cloudB->points.resize (cloudB->width * cloudB->height);for (size_t i = 0; i < cloudB->points.size (); ++i){cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);}// 添加cloudB到八叉树中octree.setInputCloud (cloudB);octree.addPointsFromInputCloud ();/**************************************************************************************************************为了检索获取存在于couodB的点集R，此R并没有cloudA中的元素，可以调用getPointIndicesFromNewVoxels方法，通过探测两个八叉树之间体素的不同，它返回cloudB 中新加点的索引的向量，通过索引向量可以获取R点集 很明显这样就探测了cloudB相对于cloudA变化的点集，但是只能探测到在cloudA上增加的点集，二不能探测减少的
****************************************************************************************************************/std::vector<int> newPointIdxVector; //存储新添加的索引的向量// 获取前一cloudA对应八叉树在cloudB对应在八叉树中没有的点集octree.getPointIndicesFromNewVoxels (newPointIdxVector);// 打印点集std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;for (size_t i = 0; i < newPointIdxVector.size (); ++i)std::cout << i << "# Index:" << newPointIdxVector[i]<< " Point:" << cloudB->points[newPointIdxVector[i]].x << " "<< cloudB->points[newPointIdxVector[i]].y << " "<< cloudB->points[newPointIdxVector[i]].z << std::endl;}
5、滤波
常用滤波方法
直通滤波器：对于在空间分布有一定空间特征的点云数据，比如使用线结构光扫描的方式采集点云，沿z向分布较广，但x,y向的分布处于有限范围内。此时可使用直通滤波器，确定点云在x或y方向上的范围，可较快剪除离群点，达到第一步粗处理的目的。
体素滤波器：体素的概念类似于像素，使用AABB包围盒将点云数据体素化，一般体素越密集的地方信息越多，噪音点及离群点可通过体素网格去除。另一方面如果使用高分辨率相机等设备对点云进行采集，往往点云会较为密集。过多的点云数量会对后续分割工作带来困难。体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能。
统计滤波器：考虑到离群点的特征，则可以定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。给定均值与方差，可剔除3∑之外的点。
条件滤波：条件滤波器通过设定滤波条件进行滤波，有点分段函数的味道，当点云在一定范围则留下，不在则舍弃。
半径滤波器：半径滤波器与统计滤波器相比更加简单粗暴。以某点为中心画一个圆计算落在该圆中点的数量，当数量大于给定值时，则保留该点，数量小于给定值则剔除该点。此算法运行速度快，依序迭代留下的点一定是最密集的，但是圆的半径和圆内点的数目都需要人工指定。
#include<iostream>
#include<pcl/point_types.h>
#include<pcl/filters/passthrough.h> //直通滤波器头文件
#include<pcl/filters/voxel_grid.h> //体素滤波器头文件
#include<pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/conditional_removal.h> //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h> //半径滤波器头文件int main(int argc, char** argv)
{///****************************************************/*创建点云数据集。*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);cloud->width = 500;cloud->height = 1;cloud->points.resize(cloud->width*cloud->height);std::cout << "创建原始点云数据" << std::endl;for (size_t i = 0; i < cloud->points.size(); ++i){cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);}for (size_t i = 0; i < cloud->points.size(); i++){std::cerr << " " << cloud->points[i].x << " "<< cloud->points[i].y << " "<< cloud->points[i].z << std::endl;}std::cout << "原始点云数据点数：" << cloud->points.size()<< std::endl << std::endl;///****************************************************///****************************************************/*方法一：直通滤波器对点云进行处理。*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);//pcl::PassThrough<pcl::PointXYZ> passthrough;passthrough.setInputCloud(cloud);//输入点云passthrough.setFilterFieldName("z");//对z轴进行操作passthrough.setFilterLimits(0.0, 400.0);//设置直通滤波器操作范围//passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外passthrough.filter(*cloud_after_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThroughstd::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough->points.size() << std::endl;///****************************************************///****************************************************/*方法二：体素滤波器实现下采样*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_voxelgrid(new pcl::PointCloud<pcl::PointXYZ>);//pcl::VoxelGrid<pcl::PointXYZ> voxelgrid; voxelgrid.setInputCloud(cloud);//输入点云数据voxelgrid.setLeafSize(10.0f, 10.0f, 10.0f);//AABB长宽高voxelgrid.filter(*cloud_after_voxelgrid);std::cout << "体素化网格方法后点云数据点数：" << cloud_after_voxelgrid->points.size() << std::endl;///****************************************************///****************************************************/*方法三：统计滤波器滤波*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_StatisticalRemoval(new pcl::PointCloud<pcl::PointXYZ>);//pcl::StatisticalOutlierRemoval<pcl::PointXYZ> Statistical;Statistical.setInputCloud(cloud);Statistical.setMeanK(20);//取平均值的临近点数Statistical.setStddevMulThresh(5);//临近点数数目少于多少时会被舍弃Statistical.filter(*cloud_after_StatisticalRemoval);std::cout << "统计分析滤波后点云数据点数：" << cloud_after_StatisticalRemoval->points.size() << std::endl;///****************************************************///****************************************************/*方法四：条件滤波器*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_Condition(new pcl::PointCloud<pcl::PointXYZ>);pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_condition(new pcl::ConditionAnd<pcl::PointXYZ>());range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(newpcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0))); //GT表示大于等于range_condition->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(newpcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8))); //LT表示小于等于pcl::ConditionalRemoval<pcl::PointXYZ> condition;condition.setCondition(range_condition);condition.setInputCloud(cloud); //输入点云condition.setKeepOrganized(true);condition.filter(*cloud_after_Condition);std::cout << "条件滤波后点云数据点数：" << cloud_after_Condition->points.size() << std::endl;///****************************************************///****************************************************/*方法五：半径滤波器*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);pcl::RadiusOutlierRemoval<pcl::PointXYZ> radiusoutlier; //创建滤波器radiusoutlier.setInputCloud(cloud); //设置输入点云radiusoutlier.setRadiusSearch(100); //设置半径为100的范围内找临近点radiusoutlier.setMinNeighborsInRadius(2); //设置查询点的邻域点集数小于2的删除radiusoutlier.filter(*cloud_after_Radius);std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size() << std::endl;int a;std::cin >> a;return (0);}6、采样
6.1 下采样
一般下采样是通过构造一个三维体素栅格，然后在每个体素内用体素内的所有点的重心近似显示体素中的其他点，这样体素内所有点就用一个重心点来表示，进行下采样的来达到滤波的效果，这样就大大的减少了数据量，特别是在配准，曲面重建等工作之前作为预处理，可以很好的提高程序的运行速度
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>int
main(int argc, char** argv)
{// 创建点云对象pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);// 读取PCD文件if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0){return -1;}// 创建滤波对象pcl::VoxelGrid<pcl::PointXYZ> filter;filter.setInputCloud(cloud);// 设置体素栅格的大小为 1x1x1cmfilter.setLeafSize(0.01f, 0.01f, 0.01f);filter.filter(*filteredCloud);
}
6.2 均匀采样
这个类基本上是相同的，但它输出的点云索引是选择的关键点在计算描述子的常见方式。
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>int
main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0){return -1;}// Uniform sampling object.pcl::UniformSampling<pcl::PointXYZ> filter;filter.setInputCloud(cloud);filter.setRadiusSearch(0.01f);// We need an additional object to store the indices of surviving points.pcl::PointCloud<int> keypointIndices;filter.compute(keypointIndices);pcl::copyPointCloud(*cloud, keypointIndices.points, *filteredCloud);
}
6.3 增采样
增采样 ：增采样是一种表面重建方法，当你有比你想象的要少的点云数据时，增采样可以帮你恢复原有的表面（S），通过内插你目前拥有的点云数据，这是一个复杂的猜想假设的过程。所以构建的结果不会百分之一百准确，但有时它是一种可选择的方案。所以，在你的点云云进行下采样时，一定要保存一份原始数据！
#include <pcl/io/pcd_io.h>
#include <pcl/surface/mls.h>int main(int argc,char** argv)
{
// 新建点云存储对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZ>);// 读取文件if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) != 0){return -1;}// 滤波对象pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> filter;filter.setInputCloud(cloud);//建立搜索对象pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree;filter.setSearchMethod(kdtree);//设置搜索邻域的半径为3cmfilter.setSearchRadius(0.03);// Upsampling 采样的方法有 DISTINCT_CLOUD, RANDOM_UNIFORM_DENSITYfilter.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ>::SAMPLE_LOCAL_PLANE);// 采样的半径是filter.setUpsamplingRadius(0.03);// 采样步数的大小filter.setUpsamplingStepSize(0.02);filter.process(*filteredCloud);
}
7、ICP 配准
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>int main (int argc, char** argv)
{//Creates two pcl::PointCloud<pcl::PointXYZ> boost shared pointers and initializes thempcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);// Fill in the CloudIn datacloud_in->width = 5;cloud_in->height = 1;cloud_in->is_dense = false;cloud_in->points.resize (cloud_in->width * cloud_in->height);for (size_t i = 0; i < cloud_in->points.size (); ++i){cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);}*cloud_out = *cloud_in;//performs a simple rigid transform on the point cloudfor (size_t i = 0; i < cloud_in->points.size (); ++i)cloud_out->points[i].x = cloud_in->points[i].x + 1.5f;//creates an instance of an IterativeClosestPoint and gives it some useful informationpcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputCloud(cloud_in);icp.setInputTarget(cloud_out);//Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithmpcl::PointCloud<pcl::PointXYZ> Final;//Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.icp.align(Final);//Return the state of convergence after the last align run. //If the two PointClouds align correctly then icp.hasConverged() = 1 (true). std::cout << "has converged: " << icp.hasConverged() <<std::endl;//Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) std::cout << "score: " <<icp.getFitnessScore() << std::endl; std::cout << "----------------------------------------------------------"<< std::endl;//Get the final transformation matrix estimated by the registration method. std::cout << icp.getFinalTransformation() << std::endl;return (0);
}
结果为刚体变换矩阵：
8、点云分割
PCL提供的点云分割的方法：
-
Plane model segmentation (平面模型分割）
-
Cylinder model segmentation（圆柱模型分割）
-
Euclidean Cluster Extraction （欧几里德聚类提取）
-
Region growing segmentation (区域蔓延分割)
-
Color-based region growing segmentation (基于彩色信息区域蔓延分割）
-
Min-cut Based Segmentation (基于最少切割的分割）
-
Conditional Euclidean Clustering（有条件的欧几里德群聚类生成）
-
Difference of Normals Based Segmentation (基于局部法线间不同值的分割）
-
Clustering of Pointclouds into Supervoxels (把点云聚类为超体元）
-
Identifying ground returns using ProgressiveMorphologicalFilter segementation (渐进型形态学过滤器分割）
-
Filtering a PointCloud using ModelOutlierRemoval (基于移除模型离群值的方法过滤点云）
8.1 平面点云
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>intmain (int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);// Fill in the cloud datacloud->width = 15;cloud->height = 1;cloud->points.resize (cloud->width * cloud->height);// Generate the datafor (std::size_t i = 0; i < cloud->points.size (); ++i){cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);cloud->points[i].z = 1.0;}// Set a few outlierscloud->points[0].z = 2.0;cloud->points[3].z = -2.0;cloud->points[6].z = 4.0;std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;for (std::size_t i = 0; i < cloud->points.size (); ++i)std::cerr << " " << cloud->points[i].x << " "<< cloud->points[i].y << " "<< cloud->points[i].z << std::endl;pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);pcl::PointIndices::Ptr inliers (new pcl::PointIndices);// Create the segmentation objectpcl::SACSegmentation<pcl::PointXYZ> seg;// Optionalseg.setOptimizeCoefficients (true);// Mandatoryseg.setModelType (pcl::SACMODEL_PLANE);seg.setMethodType (pcl::SAC_RANSAC);seg.setDistanceThreshold (0.01);seg.setInputCloud (cloud);seg.segment (*inliers, *coefficients);if (inliers->indices.size () == 0){PCL_ERROR ("Could not estimate a planar model for the given dataset.");return (-1);}std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " "<< coefficients->values[2] << " " << coefficients->values[3] << std::endl;std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;for (std::size_t i = 0; i < inliers->indices.size (); ++i)std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "<< cloud->points[inliers->indices[i]].y << " "<< cloud->points[inliers->indices[i]].z << std::endl;return (0);
}
8.2 区域生长分割
#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>int
main (int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("region_growing_tutorial.pcd", *cloud) == -1){std::cout << "Cloud reading failed." << std::endl;return (-1);}pcl::search::Search<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;normal_estimator.setSearchMethod (tree);normal_estimator.setInputCloud (cloud);normal_estimator.setKSearch (50);normal_estimator.compute (*normals);pcl::IndicesPtr indices (new std::vector <int>);pcl::PassThrough<pcl::PointXYZ> pass;pass.setInputCloud (cloud);pass.setFilterFieldName ("z");pass.setFilterLimits (0.0, 1.0);pass.filter (*indices);pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;reg.setMinClusterSize (50);reg.setMaxClusterSize (1000000);reg.setSearchMethod (tree);reg.setNumberOfNeighbours (30);reg.setInputCloud (cloud);//reg.setIndices (indices);reg.setInputNormals (normals);reg.setSmoothnessThreshold (3.0 / 180.0 * M_PI);reg.setCurvatureThreshold (1.0);std::vector <pcl::PointIndices> clusters;reg.extract (clusters);std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;std::cout << "First cluster has " << clusters[0].indices.size () << " points." << std::endl;std::cout << "These are the indices of the points of the initial" <<std::endl << "cloud that belong to the first cluster:" << std::endl;int counter = 0;while (counter < clusters[0].indices.size ()){std::cout << clusters[0].indices[counter] << ", ";counter++;if (counter % 10 == 0)std::cout << std::endl;}std::cout << std::endl;pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();pcl::visualization::CloudViewer viewer ("Cluster viewer");viewer.showCloud(colored_cloud);while (!viewer.wasStopped ()){}return (0);
}
原始图片：
分割效果：
8.3 最小切线分割
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/min_cut_segmentation.h>int main (int argc, char** argv)
{pcl::PointCloud <pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZ>);if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("min_cut_segmentation_tutorial.pcd", *cloud) == -1 ){std::cout << "Cloud reading failed." << std::endl;return (-1);}pcl::IndicesPtr indices (new std::vector <int>);pcl::PassThrough<pcl::PointXYZ> pass;pass.setInputCloud (cloud);pass.setFilterFieldName ("z");pass.setFilterLimits (0.0, 1.0);pass.filter (*indices);pcl::MinCutSegmentation<pcl::PointXYZ> seg;seg.setInputCloud (cloud);seg.setIndices (indices);pcl::PointCloud<pcl::PointXYZ>::Ptr foreground_points(new pcl::PointCloud<pcl::PointXYZ> ());pcl::PointXYZ point;point.x = 68.97;point.y = -18.55;point.z = 0.57;foreground_points->points.push_back(point);seg.setForegroundPoints (foreground_points);seg.setSigma (0.25);seg.setRadius (3.0433856);seg.setNumberOfNeighbours (14);seg.setSourceWeight (0.8);std::vector <pcl::PointIndices> clusters;seg.extract (clusters);std::cout << "Maximum flow is " << seg.getMaxFlow () << std::endl;pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = seg.getColoredCloud ();pcl::visualization::CloudViewer viewer ("Cluster viewer");viewer.showCloud(colored_cloud);while (!viewer.wasStopped ()){}return (0);
}
8.4 欧式聚类分割
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/time.h>#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>typedef pcl::PointXYZI PointTypeIO;
typedef pcl::PointXYZINormal PointTypeFull;bool
enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
{if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)return (true);elsereturn (false);
}bool
enforceCurvatureOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
{Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap (), point_b_normal = point_b.getNormalVector3fMap ();if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)return (true);if (std::abs (point_a_normal.dot (point_b_normal)) < 0.05)return (true);return (false);
}bool
customRegionGrowing (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
{Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap (), point_b_normal = point_b.getNormalVector3fMap ();if (squared_distance < 10000){if (std::abs (point_a.intensity - point_b.intensity) < 8.0f)return (true);if (std::abs (point_a_normal.dot (point_b_normal)) < 0.06)return (true);}else{if (std::abs (point_a.intensity - point_b.intensity) < 3.0f)return (true);}return (false);
}int
main (int argc, char** argv)
{// Data containers usedpcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);pcl::console::TicToc tt;// Load the input point cloudstd::cerr << "Loading...\n", tt.tic ();pcl::io::loadPCDFile ("Statues_4.pcd", *cloud_in);std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";// Downsample the cloud using a Voxel Grid classstd::cerr << "Downsampling...\n", tt.tic ();pcl::VoxelGrid<PointTypeIO> vg;vg.setInputCloud (cloud_in);vg.setLeafSize (80.0, 80.0, 80.0);vg.setDownsampleAllData (true);vg.filter (*cloud_out);std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";// Set up a Normal Estimation class and merge data in cloud_with_normalsstd::cerr << "Computing normals...\n", tt.tic ();pcl::copyPointCloud (*cloud_out, *cloud_with_normals);pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;ne.setInputCloud (cloud_out);ne.setSearchMethod (search_tree);ne.setRadiusSearch (300.0);ne.compute (*cloud_with_normals);std::cerr << ">> Done: " << tt.toc () << " ms\n";// Set up a Conditional Euclidean Clustering classstd::cerr << "Segmenting to clusters...\n", tt.tic ();pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);cec.setInputCloud (cloud_with_normals);cec.setConditionFunction (&customRegionGrowing);cec.setClusterTolerance (500.0);cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);cec.segment (*clusters);cec.getRemovedClusters (small_clusters, large_clusters);std::cerr << ">> Done: " << tt.toc () << " ms\n";// Using the intensity channel for lazy visualization of the outputfor (int i = 0; i < small_clusters->size (); ++i)for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;for (int i = 0; i < large_clusters->size (); ++i)for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;for (int i = 0; i < clusters->size (); ++i){int label = rand () % 8;for (int j = 0; j < (*clusters)[i].indices.size (); ++j)cloud_out->points[(*clusters)[i].indices[j]].intensity = label;}// Save the output point cloudstd::cerr << "Saving...\n", tt.tic ();pcl::io::savePCDFile ("output.pcd", *cloud_out);std::cerr << ">> Done: " << tt.toc () << " ms\n";return (0);
}
8.5 渐进形态滤波器
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/progressive_morphological_filter.h>int
main (int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointIndicesPtr ground (new pcl::PointIndices);// Fill in the cloud datapcl::PCDReader reader;// Replace the path below with the path where you saved your filereader.read<pcl::PointXYZ> ("samp11-utm.pcd", *cloud);std::cerr << "Cloud before filtering: " << std::endl;std::cerr << *cloud << std::endl;// Create the filtering objectpcl::ProgressiveMorphologicalFilter<pcl::PointXYZ> pmf;pmf.setInputCloud (cloud);pmf.setMaxWindowSize (20);pmf.setSlope (1.0f);pmf.setInitialDistance (0.5f);pmf.setMaxDistance (3.0f);pmf.extract (ground->indices);// Create the filtering objectpcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud (cloud);extract.setIndices (ground);extract.filter (*cloud_filtered);std::cerr << "Ground cloud after filtering: " << std::endl;std::cerr << *cloud_filtered << std::endl;pcl::PCDWriter writer;writer.write<pcl::PointXYZ> ("samp11-utm_ground.pcd", *cloud_filtered, false);// Extract non-ground returnsextract.setNegative (true);extract.filter (*cloud_filtered);std::cerr << "Object cloud after filtering: " << std::endl;std::cerr << *cloud_filtered << std::endl;writer.write<pcl::PointXYZ> ("samp11-utm_object.pcd", *cloud_filtered, false);return (0);
}
支付宝扫一扫
微信扫一扫
