点云处理PCL常用函数与工具

前言

本文主要记录pcl点云处理常用工具，在此将每种方法写为一个函数，便于使用的时候直接调用。

一、点云读取与保存

数据读取

数据读取的类型包括.pcd和.ply文件

void loadFile(std::string path, pcl::PointCloud<POINTTYPE>::Ptr cloud) {std::cout << path << endl;std::string fileType = path.substr(path.find_last_of('.') + 1);//获取文件后缀if (fileType == "ply") {if (pcl::io::loadPLYFile <POINTTYPE>(path, *cloud) == -1){std::cout << "Cloud reading .ply failed." << std::endl;}}else if (fileType == "pcd") {if (pcl::io::loadPCDFile <POINTTYPE>(path, *cloud) == -1){std::cout << "Cloud reading .pcd failed." << std::endl;}}
}

数据保存

数据保存的格式将根据自定义的文件名后缀确定，包括ply和pcd两种类型

int saveFile(std::string path, pcl::PointCloud<POINTTYPE>::Ptr cloud) {std::string fileType = path.substr(path.find_last_of('.') + 1);//获取文件后缀if (fileType == "ply") {if (pcl::io::savePLYFile(path, *cloud) == -1){std::cout << "Cloud saving .ply failed." << std::endl;return 0;}}else if (fileType == "pcd") {if (pcl::io::savePCDFile(path, *cloud) == -1){std::cout << "Cloud saving .pcd failed." << std::endl;return 0;}}return 1;
}

以上两种点云保存的数据格式包括：

pcl::PointXYZI
pcl::PointXYZRGB
pcl::PointXYZ

自定义的点云保存格式

那么除了以上几种点云数据格式，如果需要保存自定义的数据格式，例如在包含坐标、法线、颜色、强度信息外、额外的保存一些其信息（标签、id）等信息那么就需要对保存的点云格式进行自定义，具体方法如下：

//首先定义新增数据格式的结构体
#define PCL_ADD_LABEL \struct \{ \int p_label;\int p_id;\}; \
// 定义点云数据类型的结构体
struct PointXYZRGBINormalLI
{PCL_ADD_POINT4D;PCL_ADD_RGB;PCL_ADD_INTENSITY;PCL_ADD_NORMAL4D;PCL_ADD_LABEL;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
}EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBINormalLI,(float, x, x)(float, y, y)(float, z, z)(uint8_t, b, b)(uint8_t, g, g)(uint8_t, r, r)(float, rgb, rgb)(float, intensity, intensity)(float, normal_x, normal_x)(float, normal_y, normal_y)(float, normal_z, normal_z)(int, p_label, p_label)(int, p_id, p_id)
)

以上数据结构体几乎包含了点特征的基本信息，如果有其他信息需要保存可自行修改。
随后保存的时候将 POINTTYPE的类型改为PointXYZRGBINormalLI即可

pcl::PointCloud<PointXYZRGBINormalSI>::Ptr point_cloud(new pcl::PointCloud<PointXYZRGBINormalSI>);
pcl::io::loadPCDFile(pcd_path, *point_cloud);// 点云读取
saveFile(pcd_path, point_cloud);//点云保存

二、点云显示

点云显示-根据颜色

void ShowPCLPointsXYZRGB(pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud)
{cout << "point size:" << color_cloud->size() << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer1->addPointCloud<pcl::PointXYZRGB>(color_cloud);viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小viewer1->addCoordinateSystem(0);while (!viewer1->wasStopped()){viewer1->spinOnce(100);}
}

根据点云指定轴数值进行颜色显示

点云显示-根据指定轴数值

void ShowPCLPointsXYZRGB(pcl::PointCloud<pcl::PointXYZ>::Ptr color_cloud)
{cout << "point size:" << color_cloud->size() << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer1->addPointCloud<pcl::PointXYZ>(color_cloud);viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小viewer1->addCoordinateSystem(0);while (!viewer1->wasStopped()){viewer1->spinOnce(100);}
}

点云显示-根据指定信息显示

void ShowPCLPointsInput(pcl::PointCloud<pcl::PointXYZI>::Ptr color_cloud,std::string str)
{// str: x,y,z intensitystd::cout << "point size:" << color_cloud->size() << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));pcl::visualization::PointCloudColorHandlerGenericField<PointT> fildColor(Input_cloud, str); // 按照x字段进行渲染viewer1->addPointCloud<PointT>(color_cloud, fildColor, "Input_RGB"); // 显示点云，其中fildColor为颜色显示viewer1->setBackgroundColor(0, 0, 0); // 设置点云大小viewer1->addCoordinateSystem(0);while (!viewer1->wasStopped()){viewer1->spinOnce(100);}
}

多组点云显示

给定一个vector容器，里面用于存储多组不同的点云,然后为每组点云随机赋予颜色

#define Random(x) (rand() % x)
void ShowClusterPointsXYZRGB(std::vector<pcl::PointCloud<pcl::PointXYZ>>& cloud_clusters) {pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointXYZRBG(new pcl::PointCloud<pcl::PointXYZRGB>);pcl::PointXYZRGB point;cout << "End show particle size:" << cloud_clusters.size() << endl;for (int i = 0; i < cloud_clusters.size(); i++) {int color_B = Random(255);int color_G = Random(255);int color_R = Random(255);for (int j = 0; j < cloud_clusters[i].size(); j++) {point.x = cloud_clusters[i].points[j].x;point.y = cloud_clusters[i].points[j].y;point.z = cloud_clusters[i].points[j].z;point.r = color_R;point.g = color_G;point.b = color_B;pointXYZRBG->push_back(point);}}ShowPCLPointsXYZRGB(pointXYZRBG);
}

三、点云滤波

直通滤波

// 点云直通滤波
void parrProcessZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<float>& start_end, pcl::PointCloud<pcl::PointXYZ>::Ptr pass_cloud) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());pcl::PassThrough<pcl::PointXYZ>pass;//沿z轴过滤pass.setInputCloud(cloud);    //输入点云pass.setFilterFieldName("z"); // 沿Z轴滤波，也可沿 X Ypass.setFilterLimits(start_end[0], start_end[1]);    //选取0-1之间//pass.setFilterLimitsNegative(true);    //可选择0-1之间数据保留还是舍弃pass.filter(*pass_cloud);    //过滤
}

统计滤波

void StatisticalFilterCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr inputCloud, int k, int std, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &outCloud)
{pcl::PointCloud<pcl::PointXYZRGB>::Ptr filterCloud(new pcl::PointCloud<pcl::PointXYZRGB>);pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sta;sta.setInputCloud(inputCloud);sta.setMeanK(k);sta.setStddevMulThresh(std);sta.filter(*filterCloud);outCloud = filterCloud;
}

均匀下采样滤波

void UniformSamplingFilterCloud(pcl::PointCloud<PointT>::Ptr inputCloud, pcl::PointCloud<PointT>::Ptr &outCloud)
{pcl::UniformSampling<PointT> us;		//创建滤波器对象us.setInputCloud(inputCloud);				//设置待滤波点云us.setRadiusSearch(0.005f);				//设置滤波球半径us.filter(*outCloud);				//执行滤波
}

VoxelGrid 点云体素下采样滤波

void VoxelGridFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{pcl::VoxelGrid<pcl::PointXYZ> vg;		//创建滤波器对象vg.setInputCloud(inputCloud);				//设置待滤波点云vg.setLeafSize(0.05f, 0.05f, 0.05f);	//设置体素大小vg.filter(*outCloud);			//执行滤波
}

RadiusOutlierRemoval 半径滤波

void RadiusOutlierFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;	//创建滤波器对象ror.setInputCloud(inputCloud);						//设置待滤波点云ror.setRadiusSearch(0.02);						//设置查询点的半径范围ror.setMinNeighborsInRadius(5);					//设置判断是否为离群点的阈值，即半径内至少包括的点数//ror.setNegative(true);						//默认false，保存内点；true，保存滤掉的外点ror.filter(*outCloud);					//执行滤波
}

ConditionRemoval 条件滤波器

#include <pcl/filters/conditional_removal.h>
void ConditionalFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{/*创建条件限定下的滤波器*/pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());//创建条件定义对象range_cond//为条件定义对象添加比较算子range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(newpcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GT, -0.1)));//添加在x字段上大于 -0.1 的比较算子range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(newpcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 1.0)));//添加在x字段上小于 1.0 的比较算子pcl::ConditionalRemoval<pcl::PointXYZ> cr;	//创建滤波器对象cr.setCondition(range_cond);				//用条件定义对象初始化cr.setInputCloud(inputCloud);					//设置待滤波点云//cr.setKeepOrganized(true);				//设置保持点云的结构//cr.setUserFilterValue(5);					//将过滤掉的点用（5，5，5）代替cr.filter(*outCloud);					//执行滤波，保存滤波结果于cloud_filtered

平面投影滤波器

void SACMODEL_PLANEFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{//平面投影//创建 x+y+z=0 平面pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());coefficients->values.resize(4);	//设置模型系数的大小coefficients->values[0] = 1.0;	//x系数coefficients->values[1] = 1.0;	//y系数coefficients->values[2] = 1.0;	//z系数coefficients->values[3] = 0.0;	//常数项//投影滤波pcl::ProjectInliers<pcl::PointXYZ> proj;//创建投影滤波器对象proj.setModelType(pcl::SACMODEL_PLANE);	//设置对象对应的投影模型proj.setInputCloud(inputCloud);				//设置输入点云proj.setModelCoefficients(coefficients);//设置模型对应的系数proj.filter(*outCloud);			//执行投影滤波//圆柱投影
}

球面投影滤波器

void SACMODEL_SPHEREFilterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &outCloud)
{//--------- 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 -------------pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());coefficients->values.resize(4);coefficients->values[0] = 0;coefficients->values[1] = 0;coefficients->values[2] = -1.0;coefficients->values[3] = 2.0;//========= 创建球面模型 (x - x0)^2 + (y - y0)^2 + (z - z0)^2 = r^2 ============//-------------------------------- 执行投影滤波 --------------------------------PointCloudT::Ptr cloud_projected(new PointCloudT);pcl::ProjectInliers<pcl::PointXYZ> proj;proj.setModelType(pcl::SACMODEL_SPHERE);		//球面模型proj.setInputCloud(inputCloud);proj.setModelCoefficients(coefficients);proj.filter(*outCloud);
}

四、点云轮廓提取

点云轮廓点提取

void GetEageXYZ(pcl::PointCloud<pcl::PointXYZ>cloud_clusters, pcl::PointCloud<pcl::PointXYZ>& boundPointsXYZ) {pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;pcl::search::Search<pcl::PointXYZ>::Ptr GetEageTree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;pcl::PointCloud<pcl::Normal>::Ptr beGetEageNormals(new pcl::PointCloud<pcl::Normal>);pcl::PointCloud<pcl::Boundary> boundaries;pcl::PointCloud<pcl::PointXYZ>::Ptr beGetEageClusters(new pcl::PointCloud<pcl::PointXYZ>);beGetEageClusters = cloud_clusters.makeShared(); //use .makeShared() -> pcl::PointCloud<pcl::PointXYZ> -> pcl::PointCloud<pcl::PointXYZ>::Ptr normEst.setInputCloud(beGetEageClusters);normEst.setSearchMethod(GetEageTree);normEst.setKSearch(9);normEst.compute(*beGetEageNormals);est.setInputCloud(beGetEageClusters);est.setInputNormals(beGetEageNormals);/*M_PI_2 */est.setAngleThreshold(M_PI_2);   ///在这里 由于构造函数已经对其进行了初始化 为Π/2 ，必须这样 使用 M_PI/2  M_PI_2  est.setSearchMethod(GetEageTree);est.setKSearch(30);  //一般这里的数值越高，最终边界识别的精度越好 20//  est.setRadiusSearch(everagedistance);  //搜索半径est.compute(boundaries);pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints(new pcl::PointCloud<pcl::PointXYZ>);int countBoundaries = 0;for (int j = 0; j < beGetEageClusters->size(); j++) {uint8_t x = (boundaries.points[j].boundary_point);int a = static_cast<int>(x); //该函数的功能是强制类型转换if (a == 1){boundPoints->push_back(beGetEageClusters->points[j]);}}boundPointsXYZ = *boundPoints;
}

提取凸包轮廓

/** 提取轮廓凸包*/
void getPointCloudConvexBoundary(pcl::PointCloud<pcl::PointXYZ> inputCluster, pcl::PointCloud<pcl::PointXYZ>& outputTbBoundary) {// 采用Graham凸包检测算法获取点云的2D凸包轮廓pcl::ConvexHull<pcl::PointXYZ> convexhull;convexhull.setInputCloud(inputCluster.makeShared());//载入点云convexhull.setDimension(2);//设置凸包维度为2维std::vector<pcl::Vertices> polygons;//保存凸包的容器convexhull.reconstruct(outputTbBoundary, polygons);//计算凸包结果
}

提取凹包轮廓

/** 提取轮廓凹包*/
void getPointCloudConcaveBoundary(pcl::PointCloud<pcl::PointXYZ> inputCluster, pcl::PointCloud<pcl::PointXYZ>& outputAbBoundary) {// pcl::ConcaveHull<pcl::PointXYZ> concavehull;concavehull.setInputCloud(inputCluster.makeShared());//载入点云concavehull.setDimension(2);//设置凹包维度为2维concavehull.setAlpha(2);std::vector<pcl::Vertices> polygons;//保存凹包的容器concavehull.reconstruct(outputAbBoundary, polygons);//计算凹包结果
}

常用处理工具

点云沿指定轴旋转

//点云旋转
void PointCloudRoate(pcl::PointCloud<pcl::PointXYZ>::Ptr inputcloud,int type, pcl::PointCloud<pcl::PointXYZ>::Ptr outcloud) {double angle = M_PI;Eigen::Matrix4f transform_z = Eigen::Matrix4f::Identity();//沿X旋转if (type == 1) {transform_z(1, 1) = cos(angle);transform_z(1, 2) = -sin(angle);transform_z(2, 1) = sin(angle);transform_z(2, 2) = cos(angle);pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);}//沿X旋转if (type == 2) {transform_z(0, 0) = cos(angle);transform_z(0, 2) = sin(angle);transform_z(2, 0) = -sin(angle);transform_z(2, 2) = cos(angle);pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);}//沿X旋转if (type == 3) {transform_z(0, 0) = cos(angle);transform_z(0, 1) = -sin(angle);transform_z(1, 0) = sin(angle);transform_z(1, 1) = cos(angle);pcl::transformPointCloud(*inputcloud, *outcloud, transform_z);}
}

点云沿向量旋转

void RoateByVector(pcl::PointCloud<PointT>::Ptr input, pcl::PointCloud<PointT>::Ptr& out，double v_y,double v_z，double v_z){//向量旋转到(0,0,1)Eigen::Vector3f vecbefore;vecbefore << v_x, v_y, v_z;Eigen::Vector3f vecafter;vecafter << 0, 0, 1;double tem = vecbefore.dot(vecafter);//分子double tep = sqrt(vecbefore.dot(vecbefore) * vecafter.dot(vecafter));//分母double angle = acos(tem / tep);if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00{angle = acos(tep / tem);}Eigen::Vector3f axis2 = vecbefore.cross(vecafter);Eigen::Affine3f RoatTransform = Eigen::Affine3f::Identity();// Define a translation of 2.5 meters on the x axis.RoatTransform.translation() << 0, 0, 0;// The same rotation matrix as before; theta radians arround Z axisRoatTransform.rotate(Eigen::AngleAxisf(angle, axis2.normalized()));pcl::transformPointCloud(*input, *out, RoatTransform);

贪婪三角剖分

void Greedy_projection_triangulation(pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_Cloud) {// 法线估计pcl::search::Search<pcl::PointXYZ>::Ptr InputTree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);pcl::PointCloud <pcl::Normal>::Ptr Inputnormals(new pcl::PointCloud <pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;normal_estimator.setSearchMethod(InputTree);normal_estimator.setInputCloud(inputCloud);normal_estimator.setKSearch(20);normal_estimator.compute(*Inputnormals);// 将点云位姿、颜色、法线信息连接到一起pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);pcl::concatenateFields(*inputCloud, *Inputnormals, *cloud_with_normals);pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>);tree->setInputCloud(cloud_with_normals);//三角化pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;pcl::PolygonMesh triangles;gp3.setSearchRadius(1);gp3.setMu(5); //设置被样本点搜索其邻近点的最远距离为2.5，为了适应点云密度的变化gp3.setMaximumNearestNeighbors(100); //设置样本点可搜索的邻域个数为100gp3.setMinimumAngle(M_PI / 18); // 设置三角化后得到的三角形内角的最小的角度为10°gp3.setMaximumAngle(2 * M_PI / 3); // 设置三角化后得到的三角形内角的最大角度为120°gp3.setMaximumSurfaceAngle(M_PI / 4); // 设置某点法线方向偏离样本点法线的最大角度45°，如果超过，连接时不考虑该点gp3.setNormalConsistency(false);  //设置该参数为true保证法线朝向一致，设置为false的话不会进行法线一致性检查gp3.setInputCloud(cloud_with_normals);     //设置输入点云为有向点云gp3.setSearchMethod(tree);   //设置搜索方式gp3.reconstruct(triangles);  //重建提取三角化boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);  //viewer->addPolygonMesh(triangles, "wangge");  // 基于三角剖分的曲面重建while (!viewer->wasStopped()){viewer->spinOnce(100);}//std::vector<int> parts = gp3.getPartIDs();//获得重建后每点的 ID, Parts 从 0 开始编号， a-1 表示未连接的点。/*获得重建后每点的状态，取值为 FREE 、 FRINGE 、 BOUNDARY 、 COMPLETED 、 NONE 常量，其中 NONE 表示未定义，FREE 表示该点没有在 三 角化后的拓扑内，为自由点，COMPLETED 表示该点在三角化后的拓扑内，并且邻域都有拓扑点，BOUNDARY 表示该点在三角化后的拓扑边缘，FRINGE 表示该点在 三 角化后的拓扑内，其连接会产生重叠边。*///std::vector<int> states = gp3.getPointStates();// First get the xyz informationpcl::PointCloud<pcl::PointXYZ> xyz_cloud;pcl::fromPCLPointCloud2(triangles.cloud, xyz_cloud);out_Cloud = xyz_cloud.makeShared();
}

持续更新…