kd-tree和八叉树的概念及相关算法_kd树和八叉树

点云数据主要是，表征目标表面的海量点集合，并不具备传统实体网格数据的几何拓扑结构。
点云数据处理中最为核心的问题就是，建立离散点间的拓扑关系，实现基于邻域关系的快速查找。
1. K维树（KD-tree）
1.1 KD-tree 概念简介
KD-tree 又称 K 维树,是计算机科学中使用的一种数据结构，用来组织表示 K 维空间中点集合。它是一种带有其他约束条件的二分查找树。KD-tree对于区间和近邻搜索十分有用。我们为了达到目的，通常只在三个维度中进行处理，因此所有的 KD-tree 都将是三维 KD-tree。 如下图所示， KD-tree 的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开（即如果第一维坐标小于根节点的点它将被分在左边的子树中，如果大于根节点的点它将分在右边的子树中）。

树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度，建立 KD-tree 最高效的方法是像快速分类一样使用分割法，把指定维度的值放在根上，在该维度上包含较小数值的在左子树，较大的在右子树。然后分别在左边和右边的子树上重复这个过程，直到用户准备分类的最后一个树仅仅由一个元素组成。

1.2 PCL中KD-tree使用

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

  // Generate pointcloud data
  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);
  }

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

  // K nearest neighbor search

  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;
  }

  // Neighbors within radius search

  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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

代码解读
设置kdtree搜索对象和输入数据，然后使用随机坐标搜索方式

  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);
1
2
3
4
5
6
7
8
9

设置临近点个数 （10），两个向量来存储搜索到的 K 近邻，两个向量中一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方

   // K nearest neighbor search

  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;
1
2
3
4
5
6
7
8
9
10
11

1.3 KD-tree 算法伪代码

function kdtree (list of points pointList, int depth)
{
    // Select axis based on depth so that axis cycles through all valid values
    var int axis := depth mod k;
        
    // Sort point list and choose median as pivot element
    select median by axis from pointList;
        
    // Create node and construct subtree
    node.location := median;
    node.leftChild := kdtree(points in pointList before median, depth+1);
    node.rightChild := kdtree(points in pointList after median, depth+1);
    return node;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

2. 八叉树（octree）
2.1 octree 概念简介
八叉树结构是由 Hunter 博士于1978年首次提出的一种数据模型。八叉树结构通过对三维空间的几何实体进行体元剖分，每个体元具有相同的时间和空间复杂度，通过循环递归的划分方法对大小为 (2n∗2n∗2n)
的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性，则该体元构成一个叶节点；否则继续对该体元剖分成8个子立方体，依次递剖分，对于 (2n∗2n∗2n)大小的空间对象，最多剖分 n次，如下图所示。

2.2 在PCL中实现点云压缩
点云由海量的数据集组成，这些数据通过距离、颜色、法线等附加信息来描述空间三维点。此外，点云能以非常高的速率被创建出来，因此需要占用相当大的存储资源，一旦点云需要存储或者通过速率受限制的通信信道进行传输，提供针对这种数据的压缩方法就变得十分有用。PCL库提供了点云压缩功能，它允许编码压缩所有类型的点云，包括无序点云，它具有无参考点和变化的点尺寸、分辨率、分布密度和点顺序等结构特征。而且，底层的octree数据结构允许从几个输入源高效地合并点云数据。
下面解释单个点云和点云数据流是如何高效压缩的，在给出的例子中用PCL点云压缩技术来压缩用OpenNIGrabber抓取到的点云。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/compression/octree_pointcloud_compression.h>

#include <stdio.h>
#include <sstream>
#include <stdlib.h>

#ifdef WIN32
# define sleep(x) Sleep((x)*1000)
#endif

class SimpleOpenNIViewer
{
public:
  SimpleOpenNIViewer () :
    viewer (" Point Cloud Compression Example")
  {
  }

  void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
  {
    if (!viewer.wasStopped ())
    {
      // stringstream to store compressed point cloud
      std::stringstream compressedData;
      // output pointcloud
      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());
    
      // compress point cloud
      PointCloudEncoder->encodePointCloud (cloud, compressedData);
    
      // decompress point cloud
      PointCloudDecoder->decodePointCloud (compressedData, cloudOut);


      // show decompressed point cloud
      viewer.showCloud (cloudOut);
    }
  }

  void run ()
  {

    bool showStatistics = true;

    // for a full list of profiles see: /io/include/pcl/compression/compression_profiles.h
    pcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
    
    // instantiate point cloud compression for encoding and decoding
    PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);
    PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();
    
    // create a new grabber for OpenNI devices
    pcl::Grabber* interface = new pcl::OpenNIGrabber ();
    
    // make callback function from member function
    boost::function<void
    (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
    
    // connect callback function for desired signal. In this case its a point cloud with color values
    boost::signals2::connection c = interface->registerCallback (f);
    
    // start receiving point clouds
    interface->start ();
    
    while (!viewer.wasStopped ())
    {
      sleep (1);
    }
    
    interface->stop ();
    
    // delete point cloud compression instances
    delete (PointCloudEncoder);
    delete (PointCloudDecoder);

  }

  pcl::visualization::CloudViewer viewer;

  pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;
  pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;

};

int main (int argc, char **argv)
{
  SimpleOpenNIViewer v;
  v.run ();

  return (0);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

计算机图形学中的Mesh(网格)数据结构
Mesh(网格)是，点(Point，也可用Vertex表示)、法向量(Normal Vector)、面(Face)，的集合，它定义了一个3D物体的形状。

看一个例子：

(a)中的四面体有4个顶点（带索引值），4个面的法向量（单位化，且带索引值），4个面，其中面中保存的是该面的N个（该例中是3个）顶点和该顶点法向量（与该面法向量相同）的索引。所以一定要小心顶点、法向量数组它们各自的索引值。

Mesh数据结构可以将该结构写入文件，供其它程序使用，以上图为例的Mesh文件为：

4 4 4
0 0 0
1 0 0
0 1 0
0 0 1
0.577 0.577 0.577
0 0 -1
-1 0 0
0 -1 0
3 1 2 3 0 0 0
3 0 2 1 1 1 1
3 0 3 2 2 2 2
3 1 3 0 3 3 3
1
2
3
4
5
6
7
8
9
10
11
12
13

第1行3个数，分别为点、单位法向量、面的数目
2-5行为每个顶点的坐标（索引值依次为0、1、2、3）
6-9行为每个单位法向量的i，j，k值（索引值依次为0、1、2、3）
10-13行为每个面的信息，其中第1列为顶点个数3，2-4列为3个顶点的索引值，5-7列为每个顶点的法向量（与该面法向量相同）的索引值。

拓扑关系
拓扑关系，是指满足拓扑几何学原理的各空间数据间的相互关系。即用结点、弧段和多边形所表示的实体之间的邻接、关联、包含和连通关系。如：点与点的邻接关系、点与面的包含关系、线与面的相离关系、面与面的重合关系等。

k-d tree
k-d tree的概念
k-d tree，即k-dimensional tree，是一种高维索引树形数据结构，常用于在大规模的高维数据空间进行最近邻查找(Nearest Neighbor)和近似最近邻查找(Approximate Nearest Neighbor)，例如图像检索和识别中的高维图像特征向量的K近邻查找与匹配。

k-d tree的每一级(level)在指定维度上分开所有的子节点。在树的根部，所有的子节点在第一个维度上被分开(第一维坐标小于根节点的点将被分在左边的子树中，大于根节点的点将被分在右边的子树中)。树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度**，直到你准备分类的最后一个树仅仅由有一个元素组成。**

以上图为例，先选点A为根节点，按x轴划分，x轴坐标小于点A的为左边子节点，x轴坐标大于点A的为右边子节点。分开后，左右再各自选取节点。

用构建好的k-d tree查询最近邻的思路
k-d tree构建好以后，给出一个外来节点，需要查询它的最近邻，方法如下：
这个外来的节点与k-d tree的根节点进行比较，比较两者在根节点划分时的维度的值的大小，若这个外来节点在该维的值小，则进入根节点的左子树，否则进入右子树。依次类推，进行查找，直到到达树的叶子节点。(注意在每一级比较的都仅是划分时用的维度的值)
设当前到达的叶子节点为目前的最近邻，并且记录目前的最近邻距离。沿着来时的路向前回溯，让目前的最近邻距离与，（这个外来节点，与当前叶子节点的父节点形成的分割超平面的距离），进行比较，若当前最近邻比较小，则不用遍历当前叶子节点的父节点的另一边，否则需要遍历查找以更新最近邻距离和最近邻节点。
按照上一步中所说依次遍历，直到到达根节点为止，查询结束。

举个例子：例中的外来节点是(2, 4.5)

pcl应用k-d tree实例
在pcl中如何实现快速邻域搜索
c++中：
A.B，则A为对象或者结构体
A->B，则A为指针，->是成员提取，A->B是提取A中的成员B，A只能是指向类、结构、联合的指针
::是作用域运算符，A::B表示作用域A中的名称B，A可以是命名空间、类、结构
：一般用来表示继承

#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>); // 创建一个PointCloud<PointXYZ> boost共享指针，并进行实例化为cloud
  
  // 随机生成一个1000个点的无序点云
  cloud->width =1000; // 注意因为cloud是指针，所以这里用->
  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);
  }
  
  pcl::KdTreeFLANN<pcl::PointXYZ>kdtree; // 创建k-d tree对象
  kdtree.setInputCloud (cloud); // 将cloud设为k-d tree是搜索空间
  
  // 随机生成查询点
  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); // 设置一个整型的<vector>，用于存放第几近邻的索引
  std::vector<float>pointNKNSquaredDistance(K); // 设置一个浮点型的<vector>, 用于存放第几近邻与查询点的平方距离

  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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

程序运行结果如下：

函数nearestKSearch定义在pcl/kdtree/kdtree_flann.h中

/** \brief Search for k-nearest neighbors for the given query point.
  * 
  * \attention This method does not do any bounds checking for the input index
  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
  * 
  * \param[in] point a given \a valid (i.e., finite) query point
  * \param[in] k the number of neighbors to search for
  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 
  * a priori!)
  * \return number of neighbors found
  * 
  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
  */
int
nearestKSearch (const PointT &point, int k,
                std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

第一个参数：要查询的点
第二个参数：找top几最近邻
第三个参数：一个vector，执行后这里面存放找到的top几最近邻的索引（也就是指点云数据集中的第几个点）
第四个参数：一个vector，执行后这里面存放找到的top几最近邻到查询点的平方距离
这个函数返回找到的近邻点的数量

函数radiusSearch定义在pcl/kdtree/kdtree_flann.h中

/** \brief Search for all the nearest neighbors of the query point in a given radius.
  * 
  * \attention This method does not do any bounds checking for the input index
  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
  * 
  * \param[in] point a given \a valid (i.e., finite) query point
  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
  * \param[out] k_indices the resultant indices of the neighboring points
  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
  * returned.
  * \return number of neighbors found in radius
  *
  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
  */
int
radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
              std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const override;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

第一个参数：要查询的点
第二个参数：搜索半径阈值
第三个参数：一个vector，执行后这里面存放找到的top几最近邻的索引（也就是指点云数据集中的第几个点）
第四个参数：一个vector，执行后这里面存放找到的top几最近邻到查询点的平方距离
第五个参数：最多找几个近邻。如果设置为0或者大于点云数据中的数据数量，则返回满足阈值的所偶近邻
这个函数返回找到的近邻点的数量

pcl应用八叉树实例
基于八叉树的空间划分及搜索操作

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>

int main (int argc, char**argv)
{
  srand ((unsigned int) 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);
  }

  float resolution =128.0f;
  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //设置最深层八叉树的最小体素尺寸，初始化八叉数
  octree.setInputCloud (cloud);
  octree.addPointsFromInputCloud ();
  
  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);

  std::vector<int>pointIdxVec;
  if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
    std::cout<<"Neighbors within voxel search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<")"<<std::endl;
    for (size_t i=0; i<pointIdxVec.size (); ++i)
    {
      std::cout<<"    "<< cloud->points[pointIdxVec[i]].x <<" "<< cloud->points[pointIdxVec[i]].y <<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
    }
  }

  int K =10;
  std::vector<int>pointIdxNKNSearch;
  std::vector<float>pointNKNSquaredDistance;
  std::cout<<"K nearest neighbor search at ("<<searchPoint.x<<" "<<searchPoint.y<<" "<<searchPoint.z<<") with K="<< K <<std::endl;
  if (octree.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 (octree.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;
    }
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

程序运行结果如下：

类OctreePointCloudSearch定义在文件pcl/octree/octree_search.h中

/** \brief Constructor.
  * \param[in] resolution octree resolution at lowest octree level
  */
OctreePointCloudSearch (const double resolution) :
  OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
{
}
1
2
3
4
5
6
7

浮点型参数resolution：八叉树最深层的最小体素的尺寸
体素：体素(voxel)，是体积元素(volumepixel)的简称。体素是数字数据于三维空间分割上的最小单位，体素用于三维成像、科学数据与医学影像等领域。概念上类似二维空间的最小单位——像素，像素用在二维计算机图像的影像数据上。有些真正的三维显示器运用体素来描述它们的分辨率。

函数voxelSearch定义在文件pcl/octree/octree_search.h中
体素近邻搜索，它把查询点所在的体素中其他点的索引作为查询结果返回，结果以点索引的向量的形式保存，因此搜索点和搜索结果之间的距离取决于八叉树的最深层的最小体素尺寸参数。

/** \brief Search for neighbors within a voxel at given point
  * \param[in] point point addressing a leaf node voxel
  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
  * \return "true" if leaf node exist; "false" otherwise
  */
bool
voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
1
2
3
4
5
6
7

第一次参数：要查询的点
第二个参数：一个vector，执行后这里面存放的是查询点所在的体素中其他点的索引
如果这个查询点在八叉树的一个叶子节点中，返回True，否则返回False
无序点云数据集的空间变化检测

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
	
int main (int argc, char**argv)
{
  srand ((unsigned int) time (NULL));

  float resolution =32.0f; 
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution); // 初始化空间变化检测对象
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> ); // 128个点的无序点云cloudA
  cloudA->width =128;
  cloudA->height =1;
  cloudA->points.resize (cloudA->width * cloudA->height);
  for (size_t i=0; i<cloudA->points.size (); ++i)
  {
    cloudA->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
    cloudA->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
    cloudA->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
  }

  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud (); // 用cloudA构建八叉树

  octree.switchBuffers ();
  // 类OctreePointCloudChangeDetector定义在pcl/octree/octree_pointcloud_changedetector.h中，继承自Octree2BufBase类
  // Octree2BufBase类允许同时在内存中保存和管理两个八叉树。
  // 通过成员函数switchBuffers()，重置了八叉树对象的缓冲区，但把之前的八叉树数据仍保留在内存中。
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> ); // 128个点的无序点云cloudB
  cloudB->width =128;
  cloudB->height =1;
  cloudB->points.resize (cloudB->width *cloudB->height);
  for (size_t i=0; i<cloudB->points.size (); ++i)
  {
    cloudB->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
    cloudB->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
    cloudB->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
  }

  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud (); // 用cloudB构建八叉树，新的八叉树和旧的八叉树共享八叉树对象，但同时在内存中驻留
  
  std::vector<int>newPointIdxVector; // 放索引值

  octree.getPointIndicesFromNewVoxels (newPointIdxVector); // 用于探测cloudB相对于cloudA增加的点集，但是不能探测在cloudA上减少的点集

  std::cout<<"Output from getPointIndicesFromNewVoxels:"<<std::endl;
  for (size_t i=0; i<newPointIdxVector.size (); ++i)
  {
    std::cout<<i<<"# Index:"<<newPointIdxVector[i]<<"  Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "<<cloudB->points[newPointIdxVector[i]].y <<" "<<cloudB->points[newPointIdxVector[i]].z <<std::endl;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

程序运行结果如下：

参考：
https://blog.csdn.net/zengzeyu/article/details/79755373
https://blog.csdn.net/Zhang_Chen_/article/details/101000886