OcTree在PCL中的应用
八叉树作为三维空间中点的重要数据结构在PCL中有很多应用,下面就分别从空间划分、近邻搜索、点云压缩和空间变化检测几个方面来介绍。
空间划分和近邻搜索
八叉树是将空间平均得分为8个部分,每个节点就是一个部分,我们假设三维空间是一个正立方体,每个节点就是一个小立方体。划分到最后,一个叶子结点代表一个最小的空间范围,我们称之为体素。八叉树的建立过程也就是空间划分的过程。
基于八叉树的近邻搜索可以分为三种:
- 范围搜索
- k最近邻搜索
- 体素内搜索
前两种搜索方式已经在k-d树种介绍过了,这里就不再赘述了,而体素内搜索是octree特有的。它是在给定查询点后,将其所在体素内的所有其他点作为搜索目标的搜索方式。
体素内搜索
头文件
#include <pcl/octree/octree.h>
步骤
- 初始化ocTree树
// 确定分辨率,即最小体素的边长
float resolution = 1.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
// 载入点云
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
载入点云后,八叉树会根据分辨率和点云的空间维度得到深度,并得到一个Bounding Box,即一个长方体盒子,它能将整个点云装在它的内部,也就相当于划分出来了点云的空间范围。如果点云的空间范围我们是知道的,那么可以通过defineBoundingBox函数可以直接设定
octree.defineBoundingBox(0, 0, 0, 1, 1, 1); // 参数分别为min_x,min_y,min_z,max_x,max_y,max_z
- 输入查询点进行搜索
octree.voxelSearch(searchPoint, pointIdxVec)
关键函数:
bool
voxelSearch (const PointT& point, std::vector<int>& point_idx_data);
- 输入输出参数:
- point[in]:查询点,必须保证在一个最小体素内
- point_idx_data[out]:搜索到的相同体素内的点集,point_ind_data为它们在点云中的下标
- 返回值(bool):若返回true,证明这个最小体素存在,否则不存在。
示例
我们载入一个1mx1mx1m的立方体点云,立方体内点均匀分布,一共有100x100x100个点,最小距离1cm。
选择查询点为立方体中心点,八叉树分辨率为0.3m,得到的结果如下:
红色的点为搜索到的点
我们打印出了一些信息
程序
/*
* 功能:octree体素内搜索介绍
*/
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/octree/octree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main(int argc, char** argv)
{
if (argc<5)
{
std::cout << "Usage: " << "<Resolution> " << "<SearchPoint.x>" << "<SearchPoint.y>" << "<SearchPoint.z>" << std::endl;
return -2;
}
else
cout << "Resolution : " << argv[1] << endl;
构造一个1m X 1m X 1m立方体点云,每个点之间间隔1cm
//pcl::PointCloud<pcl::PointXYZ>::Ptr pCubeCloud(new pcl::PointCloud<pcl::PointXYZ>);
//for (int x = 0; x < 100; x++)
//{
// for (int y = 0; y < 100; y++)
// {
// for (int z = 0; z < 100; z++)
// {
// // 注意这里是pCubeCloud->push_back,而不是pCubeCloud->points->push_back
// pCubeCloud->push_back(pcl::PointXYZ((float)x/100.0f, (float)y/100.0f, (float)z/100.0f));
// }
// }
//}
save
//pcl::io::savePCDFile("cube.pcd", *pCubeCloud, true);
// load
pcl::PointCloud<pcl::PointXYZ>::Ptr pCube(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile("cube.pcd", *pCube) == -1)
return -1;
// 初始化八叉树
float resolution = atof(argv[1]); // 设置体素大小????
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
//octree.defineBoundingBox(0, 0, 0, 1, 1, 1);
// 将点云载入八叉树
octree.setInputCloud(pCube);
octree.addPointsFromInputCloud();
// 初始化查询点
pcl::PointXYZ searchPoint(atof(argv[2]), atof(argv[3]), atof(argv[4])); // 查询点
// 体素内搜索
std::vector<int> pointIdxVec;
pcl::PointCloud<pcl::RGB>::Ptr pPointsRGB(new pcl::PointCloud<pcl::RGB>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pCloudShow(new pcl::PointCloud<pcl::PointXYZRGBA>);
if (octree.voxelSearch(searchPoint, pointIdxVec))
{
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
std::cout << "Searched Points Number : " << pointIdxVec.size() << endl;
std::cout << "Leaf Count : " << octree.getLeafCount() << std::endl; // 叶子数
std::cout << "Tree Depth : " << octree.getTreeDepth() << std::endl; // 八叉树深度
std::cout << "Branch Count : " << octree.getBranchCount() << std::endl; // 非叶子结点数
std::cout << "Voxel Diameter : " << octree.getVoxelSquaredDiameter() << std::endl; // ???Voxel Side Length*3
std::cout << "Voxel Side Length : " << octree.getVoxelSquaredSideLen() << std::endl;// 分辨率的平方
double minx, miny, minz, maxx, maxy, maxz;
octree.getBoundingBox(minx, miny, minz, maxx, maxy, maxz);
std::cout << "BoundingBox: " << "(" << minx << " - " << maxx << ")" << " , " << "(" << miny << " - " << maxy << ")" << " , " << "(" << minz << " - " << maxz << ")" << std::endl; // 整个八叉树的范围
}
// 给搜索到的点上色,原始点云中的点全为蓝色,搜索到的上为红色
pPointsRGB->width = pCube->size();
pPointsRGB->height = 1;
pPointsRGB->resize(pPointsRGB->width*pPointsRGB->height);
pCloudShow->width = pCube->size();
pCloudShow->height = 1;
pCloudShow->resize(pPointsRGB->width*pPointsRGB->height);
for (size_t i = 0; i < pPointsRGB->size(); i++)
{
pPointsRGB->points[i].b = 255;
}
for (size_t i = 0; i < pointIdxVec.size(); ++i)
{
pPointsRGB->points[pointIdxVec[i]].b = 0;
pPointsRGB->points[pointIdxVec[i]].r = 255;
}
// 合并不同字段
pcl::concatenateFields(*pCube, *pPointsRGB, *pCloudShow);
// 可视窗口初始化
pcl::visualization::PCLVisualizer viewer;
viewer.setCameraFieldOfView(0.785398); // fov 大概45度
viewer.setBackgroundColor(0.5, 0.5, 0.5); // 背景设为灰色
viewer.setCameraPosition(
0, 0, 5, // camera位置
0, 0, -1, // view向量--相机朝向
0, 1, 0 // up向量
);
viewer.addPointCloud(pCloudShow,"Out");
viewer.addCoordinateSystem(1.0, "cloud", 0);
while (!viewer.wasStopped()) { // 显示,直到‘q’键被按下
viewer.spinOnce();
}
//pcl::io::savePLYFileBinary("voxelSearchResult.ply", *pCloudShow); // 用cloudcompare读入时rgba变为0
system("pause");
return 0;
}