点云有用的操作
前言：最近在做点云的工作，通过资料及其他网页，总结一些比较常用且实用的操作，留给自己查看，同时也希望能给别人带来方便。
１. 两片点云cloudA、cloudB，若在cloudB中找到cloudA的数据点，则从cloudB中删除该点。
#include <pcl/point_cloud.h> //点类型定义头文件 #include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件 #include <iostream> #include <vector> #include <ctime> const double eps = 1.0e-6; int main(int argc, char** argv) { //srand(time(NULL)); //用系统时间初始化随机种子 //创建一个PointCloud<pcl::PointXYZ> pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>); // 随机点云生成 cloudA->width = 20; //此处点云数量 cloudA->height = 1; //表示点云为无序点云 cloudA->points.resize(cloudA->width * cloudA->height); for (size_t i = 0; i < cloudA->points.size(); ++i) //循环填充点云数据 { cloudA->points[i].x = i + 1; cloudA->points[i].y = i + 1; cloudA->points[i].z = i + 1; } cloudB->width = 20; //此处点云数量 cloudB->height = 1; //表示点云为无序点云 cloudB->points.resize(cloudB->width * cloudB->height); for (size_t i = 0; i < cloudB->points.size(); ++i) //循环填充点云数据 { cloudB->points[i].x = i + 8; cloudB->points[i].y = i + 8; cloudB->points[i].z = i + 8; } //cloudBackup->points = cloud->points; //创建KdTreeFLANN对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; pcl::PointXYZ searchPoint; int K = 1; std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引 std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方 std::vector<pcl::PointXYZ> DeleteData; int num = 0; for (auto iterA = cloudA->begin(); iterA != cloudA->end(); iterA++) { searchPoint.x = iterA->x; searchPoint.y = iterA->y; searchPoint.z = iterA->z; kdtree.setInputCloud(cloudB); //在cloudB中找到对应点后，在cloudB中直接删除该点 num = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance); if (num > 0) { if (sqrt(pointNKNSquaredDistance[0])<eps) { auto iterB = cloudB->begin() + pointIdxNKNSearch[0]; cloudB->erase(iterB); DeleteData.push_back(searchPoint); if (cloudB->size()==0) { break; } searchPoint.x = 0; searchPoint.y = 0; searchPoint.z = 0; num = 0; pointIdxNKNSearch.clear(); pointNKNSquaredDistance.clear(); } } } for (auto iter = DeleteData.begin(); iter != DeleteData.end(); iter++) { std::cout << iter->x << " " << iter->y << " " << iter->z << std::endl; } system("pause"); return 0; }
２．几种常用的操作
（１）保存点云数据
//普通格式ASSIC（占用内存较大） pcl::PCDWriter writer; writer.write(s_pcd,laserCloudIn); //bin格式（占用内存较小） pcl::io::savePCDFileBinary(s_pcd, *cloud);
（２）查找点云在X、Y、Z轴的极值
pcl::PointXYZ minPt, maxPt;
pcl::getMinMax3D (*cloud, minPt, maxPt);
（３）知道要保存点的索引，从原点云中拷贝点到新点云
pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud); std::vector indexs = { 1, 2, 5 }; pcl::copyPointCloud(*cloud, indexs, *cloudOut);
（４）从点云里删除和添加点
pcl::PointCloud::iterator index = cloud->begin(); cloud->erase(index);//删除第一个 index = cloud->begin() + 5; cloud->erase(index);//删除第5个 pcl::PointXYZ point = { 1, 1, 1 }; //在索引号为5的位置上插入一点，之后的所有点后移一位 cloud->insert(cloud->begin() + 5, point); cloud->push_back(point);//从点云最后面插入一点
（５）对点云进行全局或局部变换
//全部 pcl::transformPointCloud(*cloud,*transform_cloud1,transform_1); //局部 //第一个参数为输入，第二个参数为输入点云中部分点集索引，第三个为存储对象，第四个是变换矩阵。 pcl::transformPointCloud(*cloud,pcl::PointIndices indices,*transform_cloud1,matrix);
（６）链接两个点云字段（两点云大小必须相同）
pcl::PointCloud::Ptr cloud_with_nomal (new pcl::PointCloud); pcl::concatenateFields(*cloud,*cloud_normals,*cloud_with_nomal);
（７）从点云中删除无效点
vector indices;
pcl::removeNaNFromPointCloud(*cloud,*output,indices);
（８）计算质心
Eigen::Vector4f centroid;//质心 pcl::compute3DCentroid(*cloud_smoothed,centroid); //估计质心的坐标
（９）计算点云的法向量
pcl::NormalEstimation ne; pcl::PointCloud::Ptr pcNormal(new pcl::PointCloud); pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); tree->setInputCloud(inCloud); ne.setInputCloud(inCloud); ne.setSearchMethod(tree); ne.setKSearch(50); //ne->setRadiusSearch (0.03); ne.compute(*pcNormal);
（１０）提取点云的边界
pcl::PointCloud boundaries; //保存边界估计结果pcl::BoundaryEstimation boundEst; //定义一个进行边界特征估计的对象 pcl::NormalEstimation normEst; //定义一个法线估计的对象 pcl::PointCloud::Ptr normals(new pcl::PointCloud); //保存法线估计的结果 pcl::PointCloud::Ptr cloud_boundary (new pcl::PointCloud); normEst.setInputCloud(pcl::PointCloud::Ptr(cloud)); normEst.setRadiusSearch(reforn); //设置法线估计的半径 normEst.compute(*normals); //将法线估计结果保存至normals boundEst.setInputCloud(cloud); //设置输入的点云 boundEst.setInputNormals(normals); //设置边界估计的法线，因为边界估计依赖于法线 boundEst.setRadiusSearch(re); //设置边界估计所需要的半径 boundEst.setAngleThreshold(M_PI/4); //边界估计时的角度阈值 boundEst.setSearchMethod(pcl::search::KdTree::Ptr (new pcl::search::KdTree)); //设置搜索方式KdTree boundEst.compute(boundaries); //将边界估计结果保存在boundaries
（１１）k近邻和半径搜索
pcl::KdTreeFLANNkdtree; //创建kdtree搜索对象 kdtree.setInputCloud(cloud); //载入点云 pcl::PointXYZ searchPoint; //设置查询点并赋随机值 //K近邻搜索 int K = 10; //搜索最近邻的点数 vectorpointIdxNKNSearch(K); //存放最近邻点的索引 vectorpointNKNSquaredDistance(K);//对应的距离平方 kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) //半径内搜索 std::vector pointIdxRadiusSearch; std::vector pointRadiusSquaredDistance; float radius; //定义搜索半径 kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
（１２）添加新的点云类
//有自定义点云类型时要加入该头文件才能使用PCL库模板函数 #include //运用相应的函数需加入以下相应头文件 //如：直通滤波器 #include //定义一个新的点云类 struct PointXYZIR { PCL_ADD_POINT4D float intensity; uint16_t ring; ///< laser ring number EIGEN_MAKE_ALIGNED_OPERATOR_NEW //确保new操作符对齐 } EIGEN_ALIGN16; //强制SSE对齐 POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring))
３. 几种点云常用的滤波器
（１）直通滤波器
// 创建滤波器对象 pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 1.0); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_filtered);
（２）VoxelGrid滤波器
创建三维体素栅格，将体素栅格内所有点的重心代替体素中其他点，实现下采样。
// 创建滤波器对象 pcl::VoxelGrid<pcl::PointXYZ> sor; sor.setInputCloud(cloud); sor.setLeafSize(0.01f, 0.01f, 0.01f); sor.filter(*cloud_filtered);
（３）StatisticalOutlireRemoval滤波器
对每一个点的近邻进行一个统计分析，计算点到近邻点的距离，计算所有近邻点的平均距离，平均距离在标准范围之外的点被视为离群点
// 创建滤波器对象 pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; sor.setInputCloud (cloud); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud_filtered);
（４）使用参数化模型投影点云
基于对模型的结构和尺寸等信息，选择特殊模型，如：球等，设置参数进行滤波。
首先，填充ModelCoefficients的值，该例中使用X-Y平面
//创建一个系数为（0,0,1,0）的平面 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients()); coefficients->values.resize(4); coefficients->values[0] = coefficients->values[1] = 0; coefficients->values[2] = 1.0; coefficients->values[3] = 0;
然后，创建ProjectInliers对象，使用ModelCoefficients作为投影对象的模型参数
//创建滤波器对象 pcl::ProjectInliers<pcl::PointXYZ>proj; proj.setModelType(pcl::SACMODEL_PLANE); proj.setInputCloud(cloud); proj.setModelCoefficients(coefficients); proj.filter(*cloud_projected);
（５）ExtractIndices滤波器
基于某一分割算法提取点云的一个子集
首先，使用VoxelGrid滤波器对数据下采样，加快处理速度
// 创建滤波器对象:使用叶大小为1cm的下采样 pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud(cloud_blob); sor.setLeafSize(0.01f, 0.01f, 0.01f); sor.filter(*cloud_filtered_blob);
接着，参数化分割
然后，设置extraction filter实际参数
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients()); pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); // 创建分割对象 pcl::SACSegmentation<pcl::PointXYZ> seg; // 可选 seg.setOptimizeCoefficients(true); // 必选 seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.01); // 创建滤波器对象 pcl::ExtractIndices<pcl::PointXYZ> extract; int i = 0, nr_points = (int)cloud_filtered->points.size(); // 当还有30%原始点云数据时 while (cloud_filtered->points.size() > 0.3 * nr_points) { // 从余下的点云中分割最大平面组成部分 seg.setInputCloud(cloud_filtered); seg.segment(*inliers, *coefficients); if (inliers->indices.size() == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // 分离内层 extract.setInputCloud(cloud_filtered); extract.setIndices(inliers); extract.setNegative(false); extract.filter(*cloud_p); std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl; std::stringstream ss; ss << "table_scene_lms400_plane_" << i << ".pcd"; writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false); // 创建滤波器对象 extract.setNegative(true); extract.filter(*cloud_f); cloud_filtered.swap(cloud_f); i++; }
需要将sensor_msgs::PointCloud2::Ptr改为pcl::PCLPointCloud2::Ptr；pcl::fromROSMsg()改为pcl::fromPCLPointCloud2()；
（６）ConditionalRemoval或RadiusOutlierRemoval类
ConditionalRemoval滤波器可以删除设定的一个或多个条件指标的所有数据点；
首先，创建条件限定对象，为限定对象添加比较算子，创建滤波器并用条件定义对象初始化，
// 创建环境 pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>()); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0))); range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8))); // 创建滤波器 pcl::ConditionalRemoval<pcl::PointXYZ> condrem(range_cond); condrem.setInputCloud(cloud); condrem.setKeepOrganized(true); // 应用滤波 condrem.filter(*cloud_filtered);
然后，RadiusOutlinerRemoval滤波器删除搜索半径内不满足设定的近邻点数，可用于移除离群点。
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem; // 创建滤波器 outrem.setInputCloud(cloud); outrem.setRadiusSearch(0.8); // 设置搜索半径 outrem.setMinNeighborsInRadius(2); // 设置查询点的近邻点数 // 应用滤波器 outrem.filter(*cloud_filtered);
（７）CropHull滤波器
获取2D封闭多边形内部或者外部的点云
首先，设置2D封闭多边形顶点；
然后，使用ConvexHull类构造2D凸包；
最后，创建Crophull对象，滤波获取凸包范围内的点云。
pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr(new pcl::PointCloud<pcl::PointXYZ>); boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0)); boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1, 0)); boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1, 0)); boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1, 0)); boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1, 0)); pcl::ConvexHull<pcl::PointXYZ> hull; hull.setInputCloud(boundingbox_ptr); hull.setDimension(2); std::vector<pcl::Vertices> polygons; pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZ>); hull.reconstruct(*surface_hull, polygons); pcl::PointCloud<pcl::PointXYZ>::Ptr objects(new pcl::PointCloud<pcl::PointXYZ>); pcl::CropHull<pcl::PointXYZ> bb_filter; bb_filter.setDim(2); bb_filter.setInputCloud(cloud); bb_filter.setHullIndices(polygons); bb_filter.setHullCloud(surface_hull); bb_filter.filter(*objects);
参考：
