pcl代码解析

一、库基础代码解析：

PCL库基础：点云类型与算法详解-CSDN博客

主要介绍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：模型系数）

————————————————

                            版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
                        
原文链接：https://blog.csdn.net/SmallPink0713/article/details/129027492