PCL常用代码总结(二)：kdtree & octree

1. kdtree近邻搜索

#include <pcl/point_cloud.h> //定义PointCloud格式点云
#include <pcl/kdtree/kdtree_flann.h> //KdTreeFLANN是一种KD-tree结构，使用FLANN（ 快速最近邻逼近搜索函数库）实现高效搜索

//【1】定义kdtree对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//将点云集输入到kdtree中
kdtree.setInputCloud (cloud);

//【2】设置搜索目标点
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);

//【3】K近邻搜索
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;

// 开始kdtree K近邻搜索
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;
  }

//【4】或者在半径r内搜索
float radius =256.0f* rand () / (RAND_MAX +1.0f); //设置搜索半径
std::vector<int> pointIdxRadiusSearch; //存放搜索到的点在原点云集中的索引
std::vector<float> pointRadiusSquaredDistance; //搜索到的点与目标点的平方距离

// 开始在半径r中搜索
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;

2. octree近邻搜索

#include <pcl/point_cloud.h> //定义PointCloud格式点云
#include <pcl/octree/octree.h> //OcTree

//【1】定义一个octree搜索对象，分辨率参数
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
//将点云集输入到octree中
octree.setInputCloud (cloud);
//Add points from input point cloud to octree
octree.addPointsFromInputCloud ();

//【2】设置搜索目标点
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);

//【3】K近邻搜索
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;

// 开始octree K近邻搜索
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;
  }

//【4】或者在半径r内搜索
float radius =256.0f* rand () / (RAND_MAX +1.0f); //设置搜索半径
std::vector<int> pointIdxRadiusSearch; //存放搜索到的点在原点云集中的索引
std::vector<float> pointRadiusSquaredDistance; //搜索到的点与目标点的平方距离

// 开始在半径r中搜索
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;
  }

//【5】体素内近邻搜索
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;
  }

3. octree点云压缩

#include <pcl/point_cloud.h> //PointCloud格式
#include <pcl/point_types.h> //PointT类型
#include <pcl/compression/octree_pointcloud_compression.h> //octree压缩

//【1】定义压缩和解压缩对象
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>*PointCloudEncoder;
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>*PointCloudDecoder;

// 将压缩相关的统计信息打印到标准输出
bool showStatistics=true;
// 压缩配置文件，表示5立方毫米，有颜色，快速在线编码
pcl::io::compression_Profiles_e compressionProfile=pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

//【2】实例化压缩与解压缩对象
PointCloudEncoder=new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);
PointCloudDecoder=new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();

//【3】压缩和解压缩
/*其中cloud为形参，pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud*/
// 存储压缩点云的字节流
std::stringstream compressedData;
// 压缩点云
PointCloudEncoder->encodePointCloud (cloud, compressedData);
// 解压缩点云
PointCloudDecoder->decodePointCloud (compressedData, cloudOut);

4. octree无序点云差异检测

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

//【1】定义octree变化检测对象
// 八叉树分辨率 即体素的大小
float resolution =32.0f;
// 实例空间变化检测对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

//【2】添加两个PointCloud数据集
//添加cloudA到八叉树，建立八叉树
octree.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();
// 交换八叉树缓存，但是cloudA对应的八叉树仍在内存中
octree.switchBuffers ();

//添加 cloudB到八叉树
octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();

//【3】octree查找差异点（A中没有，B中有）
std::vector<int> newPointIdxVector; // 查找到的点索引存放的数组
//获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
//打印输出点
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;

☆参考

点云PCL从入门到精通

https://pcl.readthedocs.io/projects/tutorials/en/latest/index.html