可能最近的项目和数据结构之类的杠上了,开始恶补一些数据结构的知识,加上学习PCL经常会使用kd-tree,octree之类的,作为知识储备也好,或者觉得其实这些也挺有趣的,记录下来方便以后查阅也罢,总之还是会努力加油的啦O(∩_∩)O哈哈~
八叉树(Octree)是一种用于描述三维空间的树状数据结构,每个根节点有8个子节点(理想情况下),也许就是八叉树名字的由来。我们先从一维开始,如何快速的在x轴上找到一个理想的数,一般以0为基准,会问大于0还是小于零,也就是说一维的情况下就是以二叉树为代表做查找或者插入等操作。
那么在二维的情况下呢,这个最直观了,在白纸上就能这么分,把一整块区域分成四块,如下图所示,然后对每块在进行细分,这就是四叉树。
三维呢?西瓜都吃过吧,怎么切三刀把西瓜平均分成八分?这用到的就是八叉树的原理啦!然后把每一块再进行细分,直到达到最大的level数,或者说已经分到了体素级别了。什么叫体素(voxel),在二维图像中通常都会说像素,就是表示二维图像的最小单位(当然还有亚像素,一般情况下还是用像素作为基本的单位)对应到三维就是体素(如果长度为1),可能理解起来就是一个非常小的立方体,如果存在,则这个小立方体是填充状态,如果是空的,就代表没有数据。查找对应的数据,在通常情况下规模是八分之一的缩小的。极端情况下,比如说你查找的数是蹲在墙角的,那么找它的时间会长一些。
这段代码主要做的工作就是在随机生成的点云中,利用KNN或者RadiusSearch,在构造好的octree中查找指定的点云。
#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);
}
//分辨率描述了最小体元素的长度为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
#include
#include
#include
#include
#include
int main(int argc, char **argv) {
srand((unsigned int) time (NULL));
//octree resolution - side length of octree voxels
float resolution = 32.0f;
//Instantiate octree-based point cloud change detection class
pcl::octree::OctreePointCloudChangeDetector 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