PCL_4---k-d tree 与八叉树

主要记录源码，加上了一些自己的理解。

一、k-d 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>);
  cloud->width =1000; //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);
  }

  /* 创建KdTreeFLANN对象 导入点云 创建查询点 */
  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、k近邻搜索K个近邻点  */
  int K =10;  
  std::vector<int> pointIdxNKNSearch(K);          //存储查询点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;
  }

  /* 2、k近邻搜索 在半径r内搜索近邻 半径随机产生*/
  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;
}

实验结果

• K近邻点
• 半径内近邻点

K nearest neighbor search at (411.158 880.737 460.739) with K=10
    344.052 824.84 440.437 (squared distance: 8039.8)
    340.176 824.447 451.771 (squared distance: 8287.43)
    457.357 961.427 498.372 (squared distance: 10061.4)
    386.369 949.562 377.057 (squared distance: 12354.2)
    342.65 961.743 422.617 (squared distance: 12708.6)
    432.821 936.234 560.646 (squared distance: 13530.6)
    325.776 817.144 510.196 (squared distance: 13780.3)
    427.329 924.143 569.831 (squared distance: 14046.6)
    328.045 786.592 415.483 (squared distance: 17819.2)
    493.282 774.574 513.514 (squared distance: 20800.1)
Neighbors within radius search at (411.158 880.737 460.739) with radius=182.626
    344.052 824.84 440.437 (squared distance: 8039.8)
    340.176 824.447 451.771 (squared distance: 8287.43)
    457.357 961.427 498.372 (squared distance: 10061.4)
    386.369 949.562 377.057 (squared distance: 12354.2)
    342.65 961.743 422.617 (squared distance: 12708.6)
    432.821 936.234 560.646 (squared distance: 13530.6)
    325.776 817.144 510.196 (squared distance: 13780.3)
    427.329 924.143 569.831 (squared distance: 14046.6)
    328.045 786.592 415.483 (squared distance: 17819.2)
    493.282 774.574 513.514 (squared distance: 20800.1)
    305.511 827.495 546.885 (squared distance: 21417.2)
    446.927 857.028 607.057 (squared distance: 23250.6)
    455.42 1004.78 566.654 (squared distance: 28564.7)
    560.851 792.209 449.129 (squared distance: 30379.9)
    312.008 909.221 604.652 (squared distance: 31353.2)
    533.265 1011.98 465.879 (squared distance: 32160.2)
    566.013 802.704 412.312 (squared distance: 32414.4)

二、octree（八叉树）实验

代码

#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初始化
  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);

  /* 1、体素内近邻搜索 */
  std::vector<int> pointIdxVec;  //存储体素近邻搜索的结果向量 本人小白暂时认为是存在数组中
  if (octree.voxelSearch (searchPoint, pointIdxVec))  //执行搜索，返回结果到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;
  }

  /* 2、K近邻搜索 搜索查询点周围的K个点*/
  int K =10;
  std::vector<int> pointIdxNKNSearch;          //存储k近邻搜索点的索引
  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;
  }

  /* 3、半径内近邻搜索 */
  std::vector<int> pointIdxRadiusSearch;          //存储k近邻搜索点的索引
  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;
    }
  
  return 0;
}

实验结果

• 体素内近邻点
• K近邻点
• 半径内近邻点

Neighbors within voxel search at (967.804 532.324 52.2935)
    937.831 531.68 49.3266
    981.128 498.083 80.6423
    913.34 493.828 87.5745
K nearest neighbor search at (967.804 532.324 52.2935) with K=10
    937.831 531.68 49.3266 (squared distance: 907.584)
    981.128 498.083 80.6423 (squared distance: 2153.6)
    1019.96 513.102 33.9256 (squared distance: 3426.77)
    913.34 493.828 87.5745 (squared distance: 5692.96)
    911.987 617.928 90.9567 (squared distance: 11938.6)
    945.028 430.944 91.9093 (squared distance: 12366)
    853.015 558.682 65.3296 (squared distance: 14041.2)
    931.551 437.429 156.498 (squared distance: 21177.7)
    836.204 453.168 79.5034 (squared distance: 24324.5)
    849.1 616.626 125.015 (squared distance: 26485.9)
Neighbors within radius search at (967.804 532.324 52.2935) with radius=81.4588
    937.831 531.68 49.3266 (squared distance: 907.584)
    981.128 498.083 80.6423 (squared distance: 2153.6)
    913.34 493.828 87.5745 (squared distance: 5692.96)
    1019.96 513.102 33.9256 (squared distance: 3426.77)

三、octree空间变化检测实验

代码

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

  /* 初始化空间变化检测对象 该类继承了Octree2BuffBase类 */
  float resolution =32.0f;  // 八叉树分辨率 即体素的大小
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);
  
  /* 在双缓冲内存中添加参考点云cloudA */
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ>);
  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 ();  //建立八叉树
  octree.switchBuffers ();            // 交换八叉树缓存，但是cloudA对应的八叉树仍在内存中

  /* 在双缓冲内存中添加点云cloudB 和cloudA共享八叉树对象 */
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
  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相对于cloudA多了多少 而不能检测少了多少 */
  std::vector<int> newPointIdxVector;    //存储新加点索引的对象
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);  //获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
  //打印输出点
  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;
  }

实验结果

检测cloudB相对于cloudA多了多少 而不能检测少了多少。

Output from getPointIndicesFromNewVoxels:
0# Index:36  Point:6.37422 56.3796 0.909605
1# Index:52  Point:8.7016 53.0328 9.36858
2# Index:5  Point:1.58947 50.6516 58.0161
3# Index:34  Point:5.84086 50.7628 57.8813