pcl常用函数

给出一些可能会用到的工具函数。代码来自https://segmentfault.com/a/1190000007125502

1.时间计算

pcl中计算程序运行时间有很多函数，其中利用控制台的时间计算是：

#include <pcl/console/time.h>

pcl::console::TicToc time; time.tic(); 

+程序段 + 

cout<<time.toc()/1000<<"s"<<endl;

就可以以秒输出“程序段”的运行时间。

2.pcl::PointCloud::Ptr和pcl::PointCloud的两个类相互转换

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
 
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointer(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud = *cloudPointer;
cloudPointer = cloud.makeShared();

3.如何查找点云的x，y，z的极值？

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ> ("your_pcd_file.pcd", *cloud);
pcl::PointXYZ minPt, maxPt;
pcl::getMinMax3D (*cloud, minPt, maxPt);
cout << minPt  << endl << maxPt << endl << endl;

4.知道需要保存点的索引，从原点云中拷贝点到新点云

#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);

5.如何从点云里删除和添加点

#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>::iterator index = cloud->begin();
cloud->erase(index);//删除第一个
index = cloud->begin() + 5;
cloud->erase(cloud->begin());//删除第5个
pcl::PointXYZ point = { 1, 1, 1 };
//在索引号为5的位置1上插入一点，原来的点后移一位
cloud->insert(cloud->begin() + 5, point);
cloud->push_back(point);//从点云最后面插入一点
std::cout << cloud->points[5].x;//输出1

6.如何对点云进行全局或局部变换

pcl::transformPointCloud(*cloud,*transform_cloud1,transform_1);  
//局部
pcl::transformPointCloud(*cloud,pcl::PointIndices indices,*transform_cloud1,matrix); //第一个参数为输入，第二个参数为输入点云中部分点集索引，第三个为存储对象，第四个是变换矩阵。

7.链接两个点云字段（两点云大小必须相同）

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("/home/yxg/pcl/pcd/mid.pcd",*cloud);
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>()); 
ne.setKSearch(8);
 //ne.setRadisuSearch(0.3);
ne.compute(*cloud_normals);    
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_nomal (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud,*cloud_normals,*cloud_with_nomal);

8.如何从点云中删除无效点

pcl中的无效点是指：点的某一坐标值为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;
}

9.将xyzrgb格式转换为xyz格式的点云

#include <pcl/io/pcd_io.h>
#include <ctime>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

using namespace std;
typedef pcl::PointXYZ point;
typedef pcl::PointXYZRGBA pointcolor;

int main(int argc,char **argv)
{
        pcl::PointCloud<pointcolor>::Ptr input (new pcl::PointCloud<pointcolor>);
        pcl::io::loadPCDFile(argv[1],*input);
   
        pcl::PointCloud<point>::Ptr output (new pcl::PointCloud<point>);
        int M = input->points.size();
        cout<<"input size is:"<<M<<endl;

        for (int i = 0;i <M;i++)
        {
                point p;
                p.x = input->points[i].x;
                p.y = input->points[i].y;
                p.z = input->points[i].z; 
                output->points.push_back(p);
        }
        output->width = 1;
        output->height = M;
        
        cout<< "size is"<<output->size()<<endl;
        pcl::io::savePCDFile("output.pcd",*output);
}

10.计算质心

Eigen::Vector4f centroid;  //质心
pcl::compute3DCentroid(*cloud_smoothed,centroid); //估计质心的坐标

11.从网格提取顶点（将网格转化为点）

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>//loadPolygonFileOBJ所属头文件；
#include <pcl/io/vtk_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
using namespace pcl;
int main(int argc,char **argv)
{
   pcl::PolygonMesh mesh;
  //   pcl::io::loadPolygonFileOBJ(argv[1], mesh);
  pcl::io::loadPLYFile(argv[1],mesh);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new     pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
  pcl::io::savePCDFileASCII("result.pcd", *cloud);
  return 0;
}

以上代码可以从.obj或.ply面片格式转化为点云类型。

12.计算点的索引

例如sift算法中，pcl无法直接提供索引（主要原因是sift点是通过计算出来的，在某些不同参数下，sift点可能并非源数据中的点，而是某些点的近似），若要获取索引，则可利用以下函数：

void getIndices (pointcloud::Ptr cloudin, pointcloud keypoints, pcl::PointIndices::Ptr indices)
{
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(cloudin);
        std::vector<float>pointNKNSquareDistance; //近邻点集的距离
        std::vector<int> pointIdxNKNSearch;

        for (size_t i =0; i < keypoints.size();i++)
        {
                kdtree.nearestKSearch(keypoints.points[i],1,pointIdxNKNSearch,pointNKNSquareDistance);
                // cout<<"the distance is:"<<pointNKNSquareDistance[0]<<endl;
                // cout<<"the indieces is:"<<pointIdxNKNSearch[0]<<endl;
                
                indices->indices.push_back(pointIdxNKNSearch[0]);
                
        }

}

其思想就是：将原始数据插入到flann的kdtree中，寻找keypoints的最近邻，如果距离等于0,则说明是同一点，提取索引即可.

连接两个点云:含匹配函数strcmp(argv[1], “-p”)

将两个点云放入一个点云

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int
  main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "please specify command line arg '-f' or '-p'" << std::endl;
    exit(0);
  }
  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
  pcl::PointCloud<pcl::Normal> n_cloud_b;
  pcl::PointCloud<pcl::PointNormal> p_n_cloud_c;

  // Fill in the cloud data
  cloud_a.width  = 5;
  cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
    cloud_a.points.resize (cloud_a.width * cloud_a.height);
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_b.width  = 3;
    cloud_b.points.resize (cloud_b.width * cloud_b.height);
  }
  else{
    n_cloud_b.width = 5;
    n_cloud_b.points.resize (n_cloud_b.width * n_cloud_b.height);
  }

  for (std::size_t i = 0; i < cloud_a.size (); ++i)
  {
    cloud_a[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
    {
      cloud_b[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
    {
      n_cloud_b[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  std::cerr << "Cloud A: " << std::endl;
  for (std::size_t i = 0; i < cloud_a.size (); ++i)
    std::cerr << "    " << cloud_a[i].x << " " << cloud_a[i].y << " " << cloud_a[i].z << std::endl;

  std::cerr << "Cloud B: " << std::endl;
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
      std::cerr << "    " << cloud_b[i].x << " " << cloud_b[i].y << " " << cloud_b[i].z << std::endl;
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
      std::cerr << "    " << n_cloud_b[i].normal[0] << " " << n_cloud_b[i].normal[1] << " " << n_cloud_b[i].normal[2] << std::endl;

  // Copy the point cloud data
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_c  = cloud_a;
    cloud_c += cloud_b;
    //pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);
    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < cloud_c.size (); ++i)
      std::cerr << "    " << cloud_c[i].x << " " << cloud_c[i].y << " " << cloud_c[i].z << " " << std::endl;
  }
  else
  {
    pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);
    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < p_n_cloud_c.size (); ++i)
      std::cerr << "    " <<
        p_n_cloud_c[i].x << " " << p_n_cloud_c[i].y << " " << p_n_cloud_c[i].z << " " <<
        p_n_cloud_c[i].normal[0] << " " << p_n_cloud_c[i].normal[1] << " " << p_n_cloud_c[i].normal[2] << std::endl;
  }
  return (0);
}