通过八叉树进行空间分割和搜索
一个octree是一个以树基础为的管理稀疏3-D数据的数据结构。每个中间的节点有8个子节点。在这次，我们将学习怎么使用octree进行稀疏分割和近邻搜索。尤其，我们将解释如何操作"体元近邻搜索"，和"最近邻搜索"和"半径近邻搜索".
我们将创建一个octree_search.cpp这个文件
#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>);
// 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;
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);
// Neighbors within voxel search
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;
}
// K nearest neighbor search
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;
}
// 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 (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;
}
}
代码解释
定义和实例化了一个PointCloud这个数据结构，并生成随机点云。
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);
}
接下去我们创造了一个用下面的这个分辨率为初始化的octree实例。octree保持了它的叶子节点的点下标。分辨率参数描述了小体元的长度。octree的深度因此是一个分辨率的函数和点云的空间维度一样。如果一个点云的边框盒子已知，它通过使用defineBoundingBox这个方法分配给octree。然后我们把一个指针分配给点云并把所有的点加入到octree里面。
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
一旦点云与octree相联系上，我们就能使用搜索的操作。我们第一个搜索方法"Neighbors within Voxel Search"。它将被分配给相应叶子节点的体元的搜索点并返回一个点下标的向量。这个下标和进入同一个体元里面的点有关系。搜索点与搜索结果距离依赖于octree的分辨率。
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;
}
接下去，显示了一个K最近邻搜索。在这个例子里面，K被设置为10，"K最近邻搜索"的方法的搜索结果被写入两个独立的向量里面。第一个，pointIdxNKNSearch，将会包含搜索结果(与相邻点云数据集相关的下标)。第二个下标向量保持相应的搜索得到节点和最近邻之间的平方距离。
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;
}
指定半径内的邻居搜索和"最近邻搜索"相似。它的搜索结果被写成2个独立的向量，描述了下标点和搜索点的平方距离。
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;
}
下面是运行结果
Neighbors within voxel search at (974.82 188.793 138.779)
903.656 82.8158 162.392
1007.34 191.035 61.7727
896.88 155.711 58.1942
K nearest neighbor search at (974.82 188.793 138.779) with K=10
903.656 82.8158 162.392 (squared distance: 16853.1)
903.18 247.058 54.3528 (squared distance: 15655)
861.595 149.96 135.199 (squared distance: 14340.7)
896.88 155.711 58.1942 (squared distance: 13663)
995.889 116.224 219.077 (squared distance: 12157.9)
885.852 238.41 160.966 (squared distance: 10869.5)
900.807 220.317 77.1432 (squared distance: 10270.7)
1002.46 117.236 184.594 (squared distance: 7983.59)
1007.34 191.035 61.7727 (squared distance: 6992.54)
930.13 223.335 174.763 (squared distance: 4485.15)
Neighbors within radius search at (974.82 188.793 138.779) with radius=109.783
1007.34 191.035 61.7727 (squared distance: 6992.54)
900.807 220.317 77.1432 (squared distance: 10270.7)
885.852 238.41 160.966 (squared distance: 10869.5)
1002.46 117.236 184.594 (squared distance: 7983.59)
930.13 223.335 174.763 (squared distance: 4485.15)
一些额外的细节
PCL八叉树组件里面提供了几种八叉树的类型。他们通过他们的独特的叶子节点特征来区别。
OctreePointCloudPointVector(等于OctreePointCloud):这个octree可以包含每个叶子节点的一系列的点的下标。
OctreePointCloudSinglePoint：这个八叉树类在每一个叶子节点里面包含一个点的下标。只有被分配到叶子节点里面最多的下标将会被存储。
OctreePointCloudOccupancy：这个八叉树没有存储任何点的信息在它的叶子节点上。
OctreePointCloudDensity：这个octree计算每个叶子节点体元的数量。它允许特殊的空间密度查询。
如果八叉树需要经常被创建，请看一下八叉树的double buffering implementation(Octree2BufBase这个类)。这个类同时保持两个并行的八叉树结构在内存里面。除了每个搜索操作，它也使得空间改变检测成为可能。更多，一个先进的内存管理减少了内存分配和回收操作在octree构建的时候。
总结:
PCL里面的octree是一个进行空间分割和搜索的有力的工具。