pcl入门教程，点云读取，KdTree

快来打我* 2022-09-08 14:00 315阅读 0赞

### pcl教程整理 ###

*  [说明][Link 1]
 *  [I/O][I_O]
 *   *  [PCD文件格式][PCD]
     *   *  [文件格式标题（File format header）][File format header]
         *  [数据存储方式][Link 2]
     *  [从PCD文件中读取点云数据][PCD 1]
     *   *  [创建cpp文件（pcd\_read.cpp)][cpp_pcd_read.cpp]
         *  [CMakeLists.txt][]
     *  [点云数据写入PCD文件][PCD 2]
     *  [连接两个不同点云][Link 3]
 *  [KdTree搜索][KdTree]

# 说明 #

整理一下pcl点云库的 **tutorials**  
pcl官方教程[link][]  
这里省略了安装，编译

KD Tree的原理及Python实现

[https://zhuanlan.zhihu.com/p/45346117][https_zhuanlan.zhihu.com_p_45346117]

# I/O #

## PCD文件格式 ##

[link][link 1]  
PCD是 Point Cloud Data的简称，PCD文件格式并不意味着重新发明轮子，而是补充现有的文件格式，这些格式由于某种原因不支持/不支持PCL为nD点云处理带来的某些扩展。

PCD不是支持3D点云数据的第一种文件类型。特别是计算机图形和计算几何社区已经创建了许多格式来描述使用激光扫描仪获取的任意多边形和点云。其中一些格式包括：***PLY、STL、OBJ、X3D等等***，所有上述文件格式都有几个缺点，如下一节所述 - 这是很自然的，因为它们是在不同的目的和不同的时间创建的，在今天的传感技术和算法发明之前。

### 文件格式标题（File format header） ###

每个PCD文件都包含一个标头，用于标识和声明存储在文件中的点云数据的某些属性。PCD的标头必须以ASCII编码。

*  VERSION - 指定PCD文件版本
 *  FIELDS - 指定一个点可以具有的每个维度/字段的名称。例子：

FIELDS x y z                                # XYZ data
    FIELDS x y z rgb                            # XYZ + colors
    FIELDS x y z normal_x normal_y normal_z     # XYZ + surface normals
    FIELDS j1 j2 j3                             # moment invariants
    ...

*  SIZE - 指定每个维度的大小（以字节为单位）。例子：

unsigned char / char有1个字节
    unsigned short / short有2个字节
    unsigned int / int / float有4个字节
    double有8个字节

*  TYPE - 将每个维度的类型指定为char。目前接受的类型是：

I - 表示签名类型int8（char），int16（short）和int32（int）
    U - 表示无符号类型uint8（unsigned char），uint16（unsigned short），uint32（unsigned int）
    F - 表示浮点类型

*  COUNT - 指定每个维度具有的元素数量。例如，x数据通常有1个元素，但像VFH这样的特征描述符 有308.基本上，这是一种在每个点引入nD直方图描述符并将它们视为单个连续内存块的方法。默认情况下，如果COUNT不存在，则所有维度的计数都设置为1。
 *  WIDTH - 以点数指定点云数据集合的宽度。WIDTH有两个含义：

- 它可以为未组织的数据集指定云中的点总数（与下面的POINTS相同）;
     - 它可以指定有组织的点云数据集的宽度（一行中的总点数）。

*  HEIGHT - 以点数指定点云数据集的高度。高度有两个含义：

- 它可以指定有组织的点云数据集的高度（总行数）;
     - 对于无组织数据集，它被设置为1（因此用于检查数据集是否有组织）。

*  VIEWPOINT - 指定数据集中点的采集点。这可能稍后用于在不同坐标系之间构建变换，或者用于帮助需要一致方向的诸如表面法线的特征。  
    视点信息被指定为平移（tx ty tz）+四元数（qw qx qy qz）。默认值为：`VIEWPOINT 0 0 0 1 0 0 0`
 *  POINTS - 指定云中的总点数。从版本0.7开始，它的目的有点多余，所以我们期望在将来的版本中删除它。  
    例：`POINTS 307200 # the total number of points in the cloud`
 *  DATA - 指定存储点云数据的数据类型。从版本0.7开始，支持两种数据类型：ascii和binary。

### 数据存储方式 ###

从0.7版开始，.PCD文件格式使用两种不同的模式来存储数据。

*  以ASCII格式，每一个点都在一个新行上
 *  以二进制形式，其中数据是pcl :: PointCloud.points数组/向量的完整内存副本 。在Linux系统上，我们使用mmap / munmap 操作来尽可能快地读/写数据。

## 从PCD文件中读取点云数据 ##

这是第一个例子，会详细给出一个pcl工程的结构（其实一点都不复杂）。

### 创建cpp文件（pcd\_read.cpp) ###

#include <iostream>
    #include <pcl/io/pcd_io.h> //里面主要定义了I/O操作的类
    #include <pcl/point_types.h> //里面主要是 一些点云数据类型
    
    int
    main (int argc, char** argv)
    {
      // 创建一个共享指针并初始化
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    
      if (pcl::io::loadPCDFile<pcl::PointXYZ> ("test_pcd.pcd", *cloud) == -1) // 换成自己的路径
      {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
      }
      std::cout << "Loaded "
                << cloud->width * cloud->height
                << " data points from test_pcd.pcd with the following fields: "
                << std::endl;
      for (size_t i = 0; i < cloud->points.size (); ++i)
        std::cout << "    " << cloud->points[i].x
                  << " "    << cloud->points[i].y
                  << " "    << cloud->points[i].z << std::endl;
    
      return (0);
    }

### CMakeLists.txt ###

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
    
    project(pcd_read)
    
    find_package(PCL 1.2 REQUIRED)
    
    include_directories(${PCL_INCLUDE_DIRS})
    link_directories(${PCL_LIBRARY_DIRS})
    add_definitions(${PCL_DEFINITIONS})
    
    add_executable (pcd_read pcd_read.cpp)
    target_link_libraries (pcd_read ${PCL_LIBRARIES})

PS:我主要是在ROS底下加的，感觉ROS底下只要加上：`find_package(catkin REQUIRED pcl_ros)`，目前运行很多例子都没有出现问题。

## 点云数据写入PCD文件 ##

#include <iostream>
    #include <pcl/io/pcd_io.h> 
    #include <pcl/point_types.h>
    
    int
      main (int argc, char** argv)
    {
      pcl::PointCloud<pcl::PointXYZ> cloud;
    
      // Fill in the cloud data
      cloud.width    = 5;
      cloud.height   = 1;
      cloud.is_dense = false;
      cloud.points.resize (cloud.width * cloud.height);
    
      for (size_t i = 0; i < cloud.points.size (); ++i)
      {
        cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
      }
    
      pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);
      std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;
    
      for (size_t i = 0; i < cloud.points.size (); ++i)
        std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
    
      return (0);
    }

## 连接两个不同点云 ##

*  连接两个不同点云的点，两个点云的维度相同，个数不同
 *  连接两个不同点云的字段，个数相同，维度可不同  
    主程序：

#include <iostream>
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    
    int main(int argc, char** argv) {
        if (argc != 2) {
            std::cerr << "use '-f' or '-p' " << std::endl;
            // -p代表按点连接 -f按照维度拼接
            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;
        // 填充点云数据
        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 (size_t i = 0; i < cloud_a.points.size (); ++i)
        {
            cloud_a.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
            cloud_a.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
            cloud_a.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
        }
        if (strcmp(argv[1], "-p") == 0)
            for (size_t i = 0; i < cloud_b.points.size (); ++i)
            {
                cloud_b.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
                cloud_b.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
                cloud_b.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
            }
        else
            for (size_t i = 0; i < n_cloud_b.points.size (); ++i)
            {
                n_cloud_b.points[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
                n_cloud_b.points[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
                n_cloud_b.points[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
            }
            
        std::cerr << "Cloud A: " << std::endl;
        for (size_t i = 0; i < cloud_a.points.size (); ++i)
            std::cerr << "    " << cloud_a.points[i].x << " " << cloud_a.points[i].y << " " << cloud_a.points[i].z << std::endl;
        std::cerr << "Cloud B: " << std::endl;
        if (strcmp(argv[1], "-p") == 0)
            for (size_t i = 0; i < cloud_b.points.size (); ++i)
                std::cerr << "    " << cloud_b.points[i].x << " " << cloud_b.points[i].y << " " << cloud_b.points[i].z << std::endl;
        else
            for (size_t i = 0; i < n_cloud_b.points.size (); ++i)
                std::cerr << "    " << n_cloud_b.points[i].normal[0] << " " << n_cloud_b.points[i].normal[1] << " " << n_cloud_b.points[i].normal[2] << std::endl;
        // Copy the point cloud data
        if (strcmp(argv[1], "-p") == 0)
        {
            //连接点
            cloud_c  = cloud_a;
            cloud_c += cloud_b;
            std::cerr << "Cloud C: " << std::endl;
            for (size_t i = 0; i < cloud_c.points.size (); ++i)
                std::cerr << "    " << cloud_c.points[i].x << " " << cloud_c.points[i].y << " " << cloud_c.points[i].z << " " << std::endl;
        }
        else
        {
            //连接字段
            pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);
            std::cerr << "Cloud C: " << std::endl;
            for (size_t i = 0; i < p_n_cloud_c.points.size (); ++i)
                std::cerr << "    " <<
                          p_n_cloud_c.points[i].x << " " << p_n_cloud_c.points[i].y << " " << p_n_cloud_c.points[i].z << " " <<
                          p_n_cloud_c.points[i].normal[0] << " " << p_n_cloud_c.points[i].normal[1] << " " << p_n_cloud_c.points[i].normal[2] << std::endl;
        }
        return (0);
    }

输出：

连接点:
    Cloud A: 
        0.352222 -0.151883 -0.106395
        -0.397406 -0.473106 0.292602
        -0.731898 0.667105 0.441304
        -0.734766 0.854581 -0.0361733
        -0.4607 -0.277468 -0.916762
    Cloud B: 
        0.183749 0.968809 0.512055
        -0.998983 -0.463871 0.691785
        0.716053 0.525135 -0.523004
    Cloud C: 
        0.352222 -0.151883 -0.106395 
        -0.397406 -0.473106 0.292602 
        -0.731898 0.667105 0.441304 
        -0.734766 0.854581 -0.0361733 
        -0.4607 -0.277468 -0.916762 
        0.183749 0.968809 0.512055 
        -0.998983 -0.463871 0.691785 
        0.716053 0.525135 -0.523004 
        
    连接维度：
    Cloud A: 
        0.352222 -0.151883 -0.106395
        -0.397406 -0.473106 0.292602
        -0.731898 0.667105 0.441304
        -0.734766 0.854581 -0.0361733
        -0.4607 -0.277468 -0.916762
    Cloud B: 
        0.183749 0.968809 0.512055
        -0.998983 -0.463871 0.691785
        0.716053 0.525135 -0.523004
        0.439387 0.56706 0.905417
        -0.579787 0.898706 -0.504929
    Cloud C: 
        0.352222 -0.151883 -0.106395 0.183749 0.968809 0.512055
        -0.397406 -0.473106 0.292602 -0.998983 -0.463871 0.691785
        -0.731898 0.667105 0.441304 0.716053 0.525135 -0.523004
        -0.734766 0.854581 -0.0361733 0.439387 0.56706 0.905417
        -0.4607 -0.277468 -0.916762 -0.579787 0.898706 -0.504929

# KdTree搜索 #

如何使用KdTree进行搜索[link][link 2]  
首先明白k-d树是一种数据结构，可以用于多维空间关键数据的搜索（范围搜索和最近邻搜索）。  
详细了解，自行百度，这里贴一个通俗易懂的博客 [KD Tree的原理及Python实现][https_zhuanlan.zhihu.com_p_45346117]\\

#include <pcl/point_cloud.h>
    #include <pcl/kdtree/kdtree_flann.h>
    
    #include <iostream>
    #include <vector>
    #include <ctime>
    
    int
    main (int argc, char** argv)
    {
        srand (time (NULL));
    
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    
        // 生成点云数据
        cloud->width = 1000;
        cloud->height = 1;
        cloud->points.resize (cloud->width * cloud->height);
    
        for (size_t i = 0; i < cloud->points.size (); ++i)
        {
            cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
            cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
            cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
        }
    
        //创建一个kd_tree
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    
        kdtree.setInputCloud (cloud);
    
        pcl::PointXYZ searchPoint; // 要搜索的点
    
        searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
        searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
        searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
    
        // 最近邻搜索
        int K = 10;
    
        std::vector<int> pointIdxNKNSearch(K);
        std::vector<float> pointNKNSquaredDistance(K);
    
        std::cout << "K nearest neighbor search at (" << searchPoint.x
                  << " " << searchPoint.y
                  << " " << searchPoint.z
                  << ") with K=" << K << std::endl;
    
        if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
        {
            for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
                std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x
                          << " " << cloud->points[ pointIdxNKNSearch[i] ].y
                          << " " << cloud->points[ pointIdxNKNSearch[i] ].z
                          << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
        }
    
        // 按照范围搜索，按照给定的半径进行搜索
    
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;
    
        float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
    
        std::cout << "Neighbors within radius search at (" << searchPoint.x
                  << " " << searchPoint.y
                  << " " << searchPoint.z
                  << ") with radius=" << radius << std::endl;
    
    
        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;
        }
        
        return 0;
    }

[Link 1]: https://blog.csdn.net/johndonxy/article/details/90642137#_1
[I_O]: https://blog.csdn.net/johndonxy/article/details/90642137#IO_6
[PCD]: https://blog.csdn.net/johndonxy/article/details/90642137#PCD_7
[File format header]: https://blog.csdn.net/johndonxy/article/details/90642137#File_format_header_12
[Link 2]: https://blog.csdn.net/johndonxy/article/details/90642137#_59
[PCD 1]: https://blog.csdn.net/johndonxy/article/details/90642137#PCD_64
[cpp_pcd_read.cpp]: https://blog.csdn.net/johndonxy/article/details/90642137#cpppcd_readcpp_66
[CMakeLists.txt]: https://blog.csdn.net/johndonxy/article/details/90642137#CMakeListstxt_96
[PCD 2]: https://blog.csdn.net/johndonxy/article/details/90642137#PCD_113
[Link 3]: https://blog.csdn.net/johndonxy/article/details/90642137#_148
[KdTree]: https://blog.csdn.net/johndonxy/article/details/90642137#KdTree_279
[link]: http://www.pointclouds.org/documentation/tutorials/index.php#i-o
[https_zhuanlan.zhihu.com_p_45346117]: https://zhuanlan.zhihu.com/p/45346117
[link 1]: http://www.pointclouds.org/documentation/tutorials/pcd_file_format.php#pcd-file-format
[link 2]: http://www.pointclouds.org/documentation/tutorials/kdtree_search.php#kdtree-search