pcl基于八叉树进行空间划分和搜索操作
建立空间索引在点云数据处理中已被广泛应用，常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构，比较有代表性的包括 BSP 树、KD 树、KDB 树、 R树、R+树、CELL 树、四叉树和八叉树等索引结构，而在这些结构中 KD 树和八叉树在 3D点云数据排列中应用较为广泛。 PCL 对八叉树的数据结构建立和索引方法进行了实现，以方便在此基础上对点云进行处理操作 。
八叉树（Octree）的定义是：若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。那么，这要用来做什么？想象一个立方体，我们最少可以切成多少个相同等分的小立方体？答案就是8个。再想象我们有一个房间，房间里某个角落藏着一枚金币，我们想很快的把金币找出来，聪明的你会怎么做？我们可以把房间当成一个立方体，先切成八个小立方体，然后排除掉没有放任何东西的小立方体，再把有可能藏金币的小立方体继续切八等份….如此下去，平均在
（n表示房间内的所有物体数）的时间内就可找到金币。因此，八叉树就是用在3D空间中的场景管理，可以很快地知道物体在3D场景中的位置，或侦测与其它物体是否有碰撞以及是否在可视范围内。
这里引入了一个概念：
Voxel翻译为体积元素，简称体素。描述了一个预设的最小单位的正方体
pcl的octree库提供了从点云数据创建具有层次的数据结构的方法。这样就可以对点数据集进行空间分区，下采样和搜索操作。每个八叉树节点有八个子节点或没有子节点。根节点描述了一个包围所有点的3维包容盒子。
pcl_octree实现提供了有效的最近邻居搜索（邻域搜索）API，例如“ 体素（Voxel）邻居搜索”，“ K最近邻居搜索”和“半径搜索邻居”。叶子节点类也提供其他功能，例如空间“占用率”和“每个体素（Voxel）的点密度”检查；序列化和反序列化功能可将八叉树结构有效地编码为二进制格式；此外，内存池实现减少了昂贵的内存分配和释放操作，以便快速创建八叉树。
下图说明了最低树级别的八叉树节点的体素边界框。八叉树体素围绕着兔子表面的每个3D点。红点代表点数据。该图像是使用octree_viewer创建的（visualization/tools/octree_viewer）
方式二：K 近邻搜索
本例中K被设置成10, K 近邻搜索（K nearest neighbor search）方法把搜索结果写到两个分开的向量中, 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉， 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。
#include
#include
#include
#include
#include
#include
int main(int argc, char** argv) {
srand((unsigned int)time(NULL));
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// 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);
}
float resolution = 128.0f; // 设置分辨率为128
// resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
pcl::octree::OctreePointCloudSearch octree(resolution);
octree.setInputCloud(cloud); // 设置输入点云
octree.addPointsFromInputCloud(); // 通过点云构建octree
pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
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);
searchCloud->push_back(searchPoint);
int K = 10;
std::vector pointIdxNKNSearch;
std::vector 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;
}
pcl::visualization::PointCloudColorHandlerCustom originColorHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
viewer.addPointCloud(cloud, originColorHandler, "cloud");
viewer.addPointCloud(searchCloud, searchColorHandler, "search_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
viewer.addLine(originPoint, searchPoint, "line"); // 添加从原点到搜索点的线段
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0.2784, "line");
viewer.addCoordinateSystem(200); // 指定坐标轴的长度，单位mm
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
} 
octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) pcl::octree::OctreePointCloudSearch方法返回一个整数值，表示搜索操作是否成功并且找到了足够的最近邻点。如果搜索成功并且找到了至少K个最近邻点，则返回实际找到的最近邻点数量；否则，返回0。
该方法将给定的搜索点作为参数，并在八叉树中搜索最近的K个邻近点。找到的最近邻点的索引将存储在pointIdxNKNSearch向量中，而对应的欧氏距离平方将存储在pointNKNSquaredDistance向量中。
方式一：“体素近邻搜索”
体素近邻搜索(Neighbors within voxel search)：它把查询点所在的体素中其他点的索引作为查询结果返回,结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数。
#include
#include
#include
#include
#include
#include
int main(int argc, char** argv) {
srand((unsigned int)time(NULL));
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// 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);
}
float resolution = 128.0f; // 设置分辨率为128
// resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
pcl::octree::OctreePointCloudSearch octree(resolution);
octree.setInputCloud(cloud); // 设置输入点云
octree.addPointsFromInputCloud(); // 通过点云构建octree
pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
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);
searchCloud->push_back(searchPoint);
std::vector 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;
}
pcl::visualization::PointCloudColorHandlerCustom originColorHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
viewer.addPointCloud(cloud, originColorHandler, "cloud");
viewer.addPointCloud(searchCloud, searchColorHandler, "search_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
viewer.addLine(originPoint, searchPoint, "line"); // 添加从原点到搜索点的线段
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0.2784, "line");
viewer.addCoordinateSystem(200); // 指定坐标轴的长度，单位mm
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
} 
octree.voxelSearch(searchPoint, pointIdxVec) pcl::octree::OctreePointCloudSearch方法返回一个bool类型的值，表示搜索操作是否成功。如果搜索成功并且找到了匹配的体素，则返回true；否则，返回false。
该方法将给定的搜索点作为参数，并在八叉树中搜索包含该点的体素。如果找到匹配的体素，则会将体素内的点索引存储在pointIdxVec向量中，并返回true。如果没有找到匹配的体素，则pointIdxVec向量将为空，并返回false。
方式三：半径内近邻搜索
半径内近邻搜索（Neighbors within radius search）原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中, 这两个向量分别存储结果点的索引和对应的距离平方
#include
#include
#include
#include
#include
#include
int main(int argc, char** argv) {
srand((unsigned int)time(NULL));
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// 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);
}
float resolution = 128.0f; // 设置分辨率为128
// resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
pcl::octree::OctreePointCloudSearch octree(resolution);
octree.setInputCloud(cloud); // 设置输入点云
octree.addPointsFromInputCloud(); // 通过点云构建octree
pcl::PointCloud::Ptr searchCloud(new pcl::PointCloud);
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);
searchCloud->push_back(searchPoint);
std::vector pointIdxRadiusSearch;
std::vector 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;
}
pcl::visualization::PointCloudColorHandlerCustom originColorHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom searchColorHandler(searchCloud, 0, 255, 0);
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
viewer.addPointCloud(cloud, originColorHandler, "cloud");
viewer.addPointCloud(searchCloud, searchColorHandler, "search_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
viewer.addLine(originPoint, searchPoint, "line"); // 添加从原点到搜索点的线段
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0.2784, "line");
viewer.addCoordinateSystem(200); // 指定坐标轴的长度，单位mm
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
} 
octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)pcl::octree::OctreePointCloudSearch方法是用于在八叉树中执行半径搜索的函数。它根据给定的搜索点和半径，在八叉树中找到落在指定半径范围内的所有点。
以下是该方法的参数说明：
searchPoint：要进行半径搜索的点，类型为pcl::PointXYZ。即你希望以这个点为中心进行半径搜索。radius：指定的搜索半径，类型为double。它定义了从搜索点出发的球体的半径，只有落在该球体内的点才会被搜索到。pointIdxRadiusSearch：存储在搜索半径范围内的点的索引的向量，类型为std::vector。pointRadiusSquaredDistance：存储在搜索半径范围内的点与搜索点之间的欧氏距离平方的向量，类型为std::vector。
使用radiusSearch()方法时，传递一个搜索点和半径作为输入参数，并提供两个向量作为输出参数。方法将填充pointIdxRadiusSearch向量以保存搜索半径范围内的点的索引，同时填充pointRadiusSquaredDistance向量以保存每个点与搜索点之间的欧氏距离平方。
pcl_octree_viewer工具
pointCloudLibrary点云库在windows下使用pcl_viewer工具
（1）首先在点云库安装目录下找到pcl_octree_viewer.exe,具体根据自己的安装目录确定
(2) 使用cmd命令行打开
cd 到pcl_octree_viewer.exe 路径，敲入命令:
pcl_octree_viewer.exe F:\DarkHorse\data\rabbit.pcd 0.01"pcl_octree_viewer.exe
- v -> 隐藏或显示octree立方体
- b -> 隐藏或显示体素中心点
- n -> 隐藏或显示原始点云
- q -> 退出