主要介绍PCL库的一些基本的点云类型、相关数据类型以及ROS接口消息,和一些常用的算法。
用到的一些PCL点云类型
pcl::PointXYZ: 这是最简单也可能是最常用到的点类型;它只储存了3D xyz的信息。
pcl::PointXYZI: 这种类型非常类似于上面的那种,但它还包含了一个描述点亮度(intensity)的字段。当想要获取传感器返回的亮度高于一定级别的点时非常有用。还有与此相似的其他两种标准的点数据类型:一是pcl:InterestPoint,它有一个字段储存强度(streneth);二是pcl::PointWithRange:它有一个字段储存距离(视点到采样点)。
pcl::PointXYZRGBA: 这种点类型储存3D信息,也储存颜色(RGB=Red,Green, Blue)和透明度(A=Alpha)。
pcl::PointXYZRGB: 这种点类型与前面的点类型相似,但是它没有透明度字段。
pcl:Normal: 这是最常用的点类型,表示曲面上给定点处的法线以及测量的曲率。
pcl::PointNormal: 这种点类型跟前一个点类型一样;它包含了给定点所在曲面法线以及曲率信息,但是它也包含了点的3Dxyz坐标。这种点类型的变异类型是 PointXYZRGBNormal和PointXYZINormal,正如名字所提示,它们包含了颜色(前者)和亮度(后者)。
用到的一些PCL点云算法:
计算点云质心
Eigen::Vector4f centroid; //质心
pcl::compute3DCentroid(*cloud_smoothed,centroid); //估计质心的坐标
获取点云点数
// 输出点云大小 cloud->width * cloud->height
std::cout << "点云大小:" << cloud->size() << std::endl;
知道需要保存点的索引,从原点云中拷贝点到新点云
#include <pcl/io/pcd_io.h>
#include <pcl/common/impl/io.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("C:\office3-after21111.pcd", *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int > indexs = { 1, 2, 5 };
pcl::copyPointCloud(*cloud, indexs, *cloudOut);
去除点云中的无效点nan
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/io/pcd_io.h>
using namespace std;
typedef pcl::PointXYZRGBA point;
typedef pcl::PointCloud<point> CloudType;
int main (int argc,char **argv)
{
CloudType::Ptr cloud (new CloudType);
CloudType::Ptr output (new CloudType);
pcl::io::loadPCDFile(argv[1],*cloud);
cout<<"size is:"<<cloud->size()<<endl;
vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud,*output,indices);
cout<<"output size:"<<output->size()<<endl;
pcl::io::savePCDFile("out.pcd",*output);
return 0;
}
点云面特征提取
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
//创建分割时所需要的模型系数对象,coefficients及存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation& lt;
pcl::PointXYZ& gt;
// 可选择配置,设置模型系数需要优化
seg.setOptimizeCoefficients(true);
// 必要的配置,设置分割的模型类型,所用的随机参数估计方法,距离阀值,输入点云
seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型
seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型
seg.setDistanceThreshold(0.01);//设定距离阀值,距离阀值决定了点被认为是局内点是必须满足的条件国,表示点到估计模型的距离最大值
seg.setInputCloud(cloud);
//引发分割实现,存储分割结果到点几何inliers及存储平面模型的系数coefficients
seg.segment(*inliers, *coefficients);
半径近邻搜索:搜索点云中一点在球体半径 R内的所有近邻点
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
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;
}
欧式聚类
void Cvisualization::ShowCloud4()
{
//读入点云数据table_scene_lms400.pcd
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("E:/ai/pcltest/20210903changhuAM-0001.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// /*从输入的.PCD文件载入数据后,我们创建了一个VoxelGrid滤波器对数据进行下采样,我们在这里进行下采样的原 因是来加速处理过程,越少的点意味着分割循环中处理起来越快。*/
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg; //体素栅格下采样对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f); //设置采样的体素大小
vg.filter (*cloud_filtered); //执行采样保存数据
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;//创建分割对象
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true); //设置对估计的模型参数进行优化处理
seg.setModelType (pcl::SACMODEL_PLANE);//设置分割模型类别
seg.setMethodType (pcl::SAC_RANSAC);//设置用哪个随机参数估计方法
seg.setMaxIterations (100); //设置最大迭代次数
seg.setDistanceThreshold (0.02); //设置判断是否为模型内点的距离阈值
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
// /*为了处理点云中包含多个模型,我们在一个循环中执行该过程,并在每次模型被提取后,我们保存剩余的点,进行迭代。模型内点通过分割过程获取,如下*/
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
//移去平面局内点,提取剩余点云
pcl::ExtractIndices<pcl::PointXYZ> extract; //创建点云提取对象
extract.setInputCloud (cloud_filtered); //设置输入点云
extract.setIndices (inliers); //设置分割后的内点为需要提取的点集
extract.setNegative (false); //设置提取内点而非外点
// Get the points associated with the planar surface
extract.filter (*cloud_plane); //提取输出存储到cloud_plane
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered); //创建点云索引向量,用于存储实际的点云信息
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.2); //设置近邻搜索的搜索半径为2cm
ec.setMinClusterSize (100);//设置一个聚类需要的最少点数目为100
ec.setMaxClusterSize (25000);//设置一个聚类需要的最大点数目为25000
ec.setSearchMethod (tree);//设置点云的搜索机制
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);//从点云中提取聚类,并将点云索引保存在cluster_indices中
// /* 为了从点云索引向量中分割出每个聚类,必须迭代访问点云索引,每次创建一个新的点云数据集,并且将所有当前聚类的点写入到点云数据集中 */
//迭代访问点云索引cluster_indices,直到分割出所有聚类
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
//创建新的点云数据集cloud_cluster,将所有当前聚类写入到点云数据集中
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "E:/ai/pcltest/cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("HelloMyFirstVisualPCL"));
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
线特征拟合:
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
using namespace std::chrono_literals;
pcl::visualization::PCLVisualizer::Ptr
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
// viewer->addCoordinateSystem (1.0, "global");
//viewer->initCameraParameters();
return (viewer);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr
create_line(double x0, double y0, double z0, double a, double b, double c, double point_size = 1000, double step = 0.1)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
cloud_line->width = point_size;
cloud_line->height = 1;
cloud_line->resize(cloud_line->width * cloud_line->height);
for (std::size_t i = 0; i < cloud_line->points.size(); ++i) {
cloud_line->points[i].x = x0 + a / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
cloud_line->points[i].y = y0 + b / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
cloud_line->points[i].z = z0 + c / std::pow(a * a + b * b + c * c, 0.5) * i * 0.1;
}
return cloud_line;
}
void fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double distance_threshold)
{
// fit line from a point cloud
pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(distance_threshold);
seg.setInputCloud(cloud);
seg.segment(*inliers1, *coefficients1);
// line parameters
double x0, y0, z0, a, b, c;
x0 = coefficients1->values[0];
y0 = coefficients1->values[1];
z0 = coefficients1->values[2];
a = coefficients1->values[3];
b = coefficients1->values[4];
c = coefficients1->values[5];
std::cout << "model parameters1:"
<< " (x - " << x0 << ") / " << a << " = (y - " << y0 << ") / " << b
<< " = (z - " << z0 << ") / " << c << std::endl;
// extract segmentation part
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers1);
extract.setNegative(false);
extract.filter(*cloud_line1);
// extract remain pointcloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remain(new pcl::PointCloud<pcl::PointXYZ>);
extract.setNegative(true);
extract.filter(*cloud_remain);
//显示原始点云
pcl::visualization::PCLVisualizer::Ptr viewer_ori;
viewer_ori = simpleVis(cloud);
while (!viewer_ori->wasStopped()) {
viewer_ori->spinOnce(100);
std::this_thread::sleep_for(100ms);
}
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_remain, "cloud_remain");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_remain");
viewer->addPointCloud<pcl::PointXYZ>(cloud_line1, "cloud_line1");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud_line1");
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.5, 0.5, "cloud_line1");
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
std::this_thread::sleep_for(100ms);
}
}
void demo()
{
// line parameters
double x0 = -2, y0 = -2, z0 = 0, a = 1, b = 1, c = 0;
auto line_pcd_create = create_line(x0, y0, z0, a, b, c);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_noise(new pcl::PointCloud<pcl::PointXYZ>);
std::size_t noise_points_size = line_pcd_create->points.size() / 10;
cloud_noise->width = noise_points_size;
cloud_noise->height = 1;
cloud_noise->points.resize(cloud_noise->width * cloud_noise->height);
// add noise
for (std::size_t i = 0; i < noise_points_size; ++i) {
int random_num = line_pcd_create->points.size() * rand() / (RAND_MAX + 1.0f);
cloud_noise->points[i].x =
line_pcd_create->points[random_num].x + 10 * rand() / (RAND_MAX + 1.0f) - 5;
cloud_noise->points[i].y =
line_pcd_create->points[random_num].y + 10 * rand() / (RAND_MAX + 1.0f) - 5;
cloud_noise->points[i].z =
line_pcd_create->points[random_num].z + 10 * rand() / (RAND_MAX + 1.0f) - 5;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr line_with_noise(new pcl::PointCloud<pcl::PointXYZ>);
*line_with_noise = *cloud_noise + *line_pcd_create;
fit_line(line_with_noise, 1);
}
int main(int argc, char* argv[])
{
if (argc < 3) {
std::cout << "please input parametars:\nfilepath\ndistance_threshold" << std::endl;
demo();
return -1;
}
std::string file_path = argv[1];
double distance_threshold = atof(argv[2]);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPLYFile(file_path, *cloud) < 0) {
std::cout << "can not read file " << file_path << std::endl;
return -1;
}
std::cout << "point size: " << cloud->points.size() << std::endl;
fit_line(cloud, distance_threshold);
return 0;
}
ROS中的PCL接口:
std_msgs::Header: 这不是真的消息类型。但它通常是每一个ROS消息的一部分。它包含消息发送时间、序列号和坐标系名称等信息。这个PCL类型等价于pcl::Header type。
sensor_msgs::PointCloud2: 这也许是最重要的消息类型。这个消息用来转换pcl::PointCloud类型。然而必须考虑的是,在未来支持pcl::PCLPointCloud2的PCL版本中这个消息类型将会弃用。
pcl_msgs::PointIndices: 这个消息类型储存了一个点云中点的索引,等价的PCL类型是pcl::PointIndices。(PointIndices:点索引)
pcl_msgs::PolygonMesh: 这个消息类型保存了描绘网格、顶点和多边形的信息。等价的PCL类型是pcl::PolygonMesh。(PolygonMesh:多边形网格)
pcl_msgs::Vertices: 这个消息类型将一组顶点的索引保存在一个数组中,例如描述一个多边形。等价的 PCL类型是pcl:Vertices。(Vertices:顶点)
pcl_msgs::ModelCoefficients: 这个消息类型储存了一个模型的不同系数,例如描述一个平面需要的四个参数。等价的PCL类型是pcl::ModelCoefficients。(ModelCoefficients:模型系数)