PCL:octree
可能最近的项目和数据结构之类的杠上了，开始恶补一些数据结构的知识，加上学习PCL经常会使用kd-tree，octree之类的，作为知识储备也好，或者觉得其实这些也挺有趣的，记录下来方便以后查阅也罢，总之还是会努力加油的啦O(∩_∩)O哈哈~
八叉树（Octree）是一种用于描述三维空间的树状数据结构，每个根节点有8个子节点（理想情况下），也许就是八叉树名字的由来。我们先从一维开始，如何快速的在x轴上找到一个理想的数，一般以0为基准，会问大于0还是小于零，也就是说一维的情况下就是以二叉树为代表做查找或者插入等操作。
那么在二维的情况下呢，这个最直观了，在白纸上就能这么分，把一整块区域分成四块，如下图所示，然后对每块在进行细分，这就是四叉树。
三维呢？西瓜都吃过吧，怎么切三刀把西瓜平均分成八分？这用到的就是八叉树的原理啦！然后把每一块再进行细分，直到达到最大的level数，或者说已经分到了体素级别了。什么叫体素（voxel），在二维图像中通常都会说像素，就是表示二维图像的最小单位（当然还有亚像素，一般情况下还是用像素作为基本的单位）对应到三维就是体素（如果长度为1），可能理解起来就是一个非常小的立方体，如果存在，则这个小立方体是填充状态，如果是空的，就代表没有数据。查找对应的数据，在通常情况下规模是八分之一的缩小的。极端情况下，比如说你查找的数是蹲在墙角的，那么找它的时间会长一些。
这段代码主要做的工作就是在随机生成的点云中，利用KNN或者RadiusSearch，在构造好的octree中查找指定的点云。
#include ::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);
}
//分辨率描述了最小体元素的长度为128，如果点云的边界框是已知的，应该使用defineBoundingBox的方法分配给octree
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch 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 << "Neighbour 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 neighbour 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
<< " ( square distance: " << pointNKNSquaredDistance[i] << " ) " << std::endl;
}
std::vector<int>pointIdxRadiusSearch;
std::vector<float> pointRadiusSquareDistance;
float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
std::cout << "Neighbours within radius search at ( " << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< " ) with radius = " << radius << std::endl;
if(octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 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: " << pointRadiusSquareDistance[i] << " ) " << std::endl;
}
} 
数据是随机取得，每次实验结果可能会有所不同O(∩_∩)O哈哈~
什么叫“organized” point cloud？什么又叫“unorganized” point cloud？看paper的时候有些疑惑，后来查了资料得知：“unorganized” （LZ感觉翻译过来总是怪怪的，就不翻译了）就是指点云的特征会有所不同，例如会有不同的尺寸，分辨率，密度或者排序，没有一个特别统一的形式。对于“organized”的点云通常是指有固定宽度或者高度的单个深度图，对于一些数据的分析相对来说可能快一些，例如直接用rgbd相机获取的点云应该就是属于organized的
下面这段代码的主要工作是随机生成两个点云，都构造成ocree，检索点云B中体素上的点，如果在点云A中不存在，就返回点的indices
#include octree(resolution);
pcl::PointCloud::Ptr cloudA(new pcl::PointCloud);
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);
}
//add points from cloudA to octree
octree.setInputCloud(cloudA);
octree.addPointsFromInputCloud();
//switch octree buffers: this resets octree but keeps previous tree structure in memery
octree.switchBuffers();
pcl::PointCloud::Ptr cloudB(new pcl::PointCloud);
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);
}
//add points from cloudB to octree
octree.setInputCloud(cloudB);
octree.addPointsFromInputCloud();
std::vector<int> newPointIdxVector;
//get vector of point indices from octree voxel which did not exist in previous buffers
octree.getPointIndicesFromNewVoxels(newPointIdxVector);
//Output points
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;
return 0;
}
参考地址：
http://www.pointclouds.org/documentation/tutorials/octree.php#octree-search
http://www.pointclouds.org/documentation/tutorials/octree_change.php#octree-change-detection