一、简介
采用随机采样一致性估计从带有噪声的点云中提取一个圆柱体模型,程序的整体流程如下:
(1)过滤掉远于1.5米的数据点;
(2)估计每个点的表面法线;
(3)分割出平面模型并进行保存;
(4)分割出圆柱体模型并进行保存。
二、代码分析
1)定义在代码中所使用的对象与数据对象,读取相应的点云数据:
// All the objects needed
//定义在程序中使用到的对象
pcl::PCDReader reader; //pcd文件读取对象
pcl::PassThrough<PointT> pass; //直通滤波对象
pcl::NormalEstimation<PointT, pcl::Normal> ne; //法线估计对象
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象
pcl::PCDWriter writer; //pcd文件写入对象
pcl::ExtractIndices<PointT> extract; //点提取对象
pcl::ExtractIndices<pcl::Normal> extract_normals; //点提取对象
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
// Datasets
//定义用到的数据对象
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
// Read in the cloud data
//读取点云对象
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
2)进行直通滤波。滤除z轴不在(0,1.5)范围内的点,将剩余的点存储在cloud_filtered对象中后续使用:
// Build a passthrough filter to remove spurious NaNs
//设置直通滤波器滤除噪声点
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.5);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
3)对过滤后的点云进行法线估计,为后续机型基于法线的分割准备数据:
// Estimate point normals
//对过滤后的点云进行发现估计
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
4)创建平面分割对象,将平面内点从点云中分割出来,并保存平面内点:
// Create the segmentation object for the planar model and set all the parameters
//创建平面分割的对象
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.03);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
//获取平面内点和平面模型系数
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
//从点云中抽取分割处在平面上的点集
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Write the planar inliers to disk
//存储平面内点
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
5)移除平面内点,并将剩余的点作为新的分割对象,进行圆柱分割:
// Remove the planar inliers, extract the rest
//移除平面内点后抽取剩余的点
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
// Create the segmentation object for cylinder segmentation and set all the parameters
//创建圆柱分割的对象
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.05);
seg.setRadiusLimits (0, 0.1);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Obtain the cylinder inliers and coefficients
//获取圆柱内点和圆柱模型参数
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
//保存圆柱内点
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
6)整体代码如下:
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
typedef pcl::PointXYZ PointT;
int main(int argc, char** argv)
{
pcl::PCDReader reader; //实例化读取pcd文件的读取器
pcl::PassThrough<PointT> pass; //实例化一个直通滤波器
pcl::NormalEstimation<PointT, pcl::Normal> ne; //实例化法线估计对象
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //实例化一个分割对象
pcl::PCDWriter writer; //实例化pcd文件写入对象
pcl::ExtractIndices<PointT> extract; //实例化点的索引提取对象
pcl::ExtractIndices<pcl::Normal> extract_normals; //实例化法线的索引提取对象
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>()); //初始化一个kd tree
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); //初始化一个输入点云
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>); //初始化过滤后的点云
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>); //初始化点云的法线对象
pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>); //初始化点云过滤对象2
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>); //初始化点云的发现对象2
pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients); //初始化点云平面模型与一个圆锥模型
pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices); //初始化平面内点与圆锥内点对象
reader.read("table_scene_mug_stereo_textured.pcd", *cloud); //读取并输出点云文件
std::cerr << "PointCloud has:" << cloud->points.size() << "data points." << std::endl;
pass.setInputCloud(cloud); //设置直通滤波器输入点云
pass.setFilterFieldName("z"); //设置直通滤波器过滤字段方向
pass.setFilterLimits(0, 1.5); //设置过滤范围
pass.filter(*cloud_filtered); //设置过滤后的输出点云
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
ne.setSearchMethod(tree); //设置法线估计的方式为kdtree
ne.setInputCloud(cloud_filtered); //输入过滤后的点云用于法线估计
ne.setKSearch(50); //设置k近邻点的个数为50
ne.compute(*cloud_normals); //法线估计后的对象保存至法线对象
seg.setOptimizeCoefficients(true); //设置进行模型一致性检验
seg.setModelType(pcl::SACMODEL_NORMAL_PLANE); //设置模型的类型
seg.setNormalDistanceWeight(0.1); //设置法线距离的权重
seg.setMethodType(pcl::SAC_RANSAC); //设置为随机一致性估计
seg.setMaxIterations(100); //设置最大迭代次数
seg.setDistanceThreshold(0.03); //设置距离的阈值
seg.setInputCloud(cloud_filtered); //设置输入的点云对象
seg.setInputNormals(cloud_normals); //设置输入的法线对象
seg.segment(*inliers_plane, *coefficients_plane); //分割平面内点与平面模型
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
extract.setInputCloud(cloud_filtered); //从点云中抽取分割点云内点
extract.setIndices(inliers_plane);
extract.setNegative(false);
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>()); //存储平面内点
extract.filter(*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
writer.write("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
extract.setNegative(true); //移除平面内点后抽取剩余的点
extract.filter(*cloud_filtered2);
extract_normals.setNegative(true);
extract_normals.setInputCloud(cloud_normals);
extract_normals.setIndices(inliers_plane);
extract_normals.filter(*cloud_normals2);
seg.setOptimizeCoefficients(true); //与平面分离类似创建圆锥分离的对象
seg.setMethodType(pcl::SACMODEL_CYLINDER);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setNormalDistanceWeight(0.1);
seg.setMaxIterations(10000);
seg.setDistanceThreshold(0.05);
seg.setRadiusLimits(0, 0.1);
seg.setInputCloud(cloud_filtered2);
seg.setInputNormals(cloud_normals2);
seg.segment(*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficents: " << *coefficients_cylinder << std::endl;
extract.setInputCloud(cloud_filtered2); //保存圆锥内点
extract.setIndices(inliers_cylinder);
extract.setNegative(false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
extract.filter(*cloud_cylinder);
if (cloud_cylinder->points.empty())
std::cerr << "can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl;
writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return(0);
}
三、编译结果
1)原始点云可视化结果:
2)分割得到的平面:
3)分割得到的圆柱: