PCL——（6）八叉树Octree
作者：互联网
@目录
一、八叉树简介：
- 体素化使用空间的均匀分割
- 八叉树对空间非均匀分割(按需分割)
- 1D数据的2叉树表示
- 2D数据的4叉树表示
- 3D数据的8叉树表示
二、构建步骤
(1).设定最大递归深度。
(2).找出场景的最大尺寸，并以此尺寸建立第一个立方体。
(3).依序将单位元元素丢入能被包含且没有子节点的立方体。
(4).若没达到最大递归深度，就进行细分八等份，再将该立
方体所装的单位元元素全部分担给八个子立方体。
(5).若发现子立方体所分配到的单位元元素数量不为零且跟
父立方体是一样的，则该子立方体停止细分，因为根据空
间分割理论，细分的空间所得到的分配必定较少，若是一
样数目，则再怎么切数目还是一样，会造成无穷切割的情
形。
(6).重复3，直到达到最大递归深度。
三、点云八叉树应用算法:
- 搜索操作(邻域、半径、体素搜索)
- 降采样(体素网格/体素质心滤波器)
- 点云压缩
- 空间变化检测
- 空间点密度分析
- 占用检查/占位地图
- 碰撞检测
- 点云合并
3.1 Octree用于点云压缩
在下面，我们将解释如何有效地压缩单点云和点云流。在这个例子中，我们使用OpenNIGrabber捕捉点云，并使用PCL点云压缩技术进行压缩。
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/compression/octree_pointcloud_compression.h>
#include <stdio.h>
#include <sstream>
#include <stdlib.h>
#ifdef WIN32
# define sleep(x) Sleep((x)*1000)
#endif
class SimpleOpenNIViewer
{
public:
SimpleOpenNIViewer () :
viewer (" Point Cloud Compression Example")
{
}
void
cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if (!viewer.wasStopped ())
{
// stringstream to store compressed point cloud
std::stringstream compressedData;
// output pointcloud
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());
// compress point cloud
PointCloudEncoder->encodePointCloud (cloud, compressedData);
// decompress point cloud
PointCloudDecoder->decodePointCloud (compressedData, cloudOut);
// show decompressed point cloud
viewer.showCloud (cloudOut);
}
}
void
run ()
{
bool showStatistics = true;
// for a full list of profiles see: /io/include/pcl/compression/compression_profiles.h
pcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
// instantiate point cloud compression for encoding and decoding
PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);
PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();
// create a new grabber for OpenNI devices
pcl::Grabber* interface = new pcl::OpenNIGrabber ();
// make callback function from member function
std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
[this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) { cloud_cb_ (cloud); };
// connect callback function for desired signal. In this case its a point cloud with color values
boost::signals2::connection c = interface->registerCallback (f);
// start receiving point clouds
interface->start ();
while (!viewer.wasStopped ())
{
sleep (1);
}
interface->stop ();
// delete point cloud compression instances
delete (PointCloudEncoder);
delete (PointCloudDecoder);
}
pcl::visualization::CloudViewer viewer;
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;
};
int
main (int argc, char **argv)
{
SimpleOpenNIViewer v;
v.run ();
return (0);
}
3.2 基于Octree的空间划分及搜索
一共有三种搜索方式：
- 体素内搜索
- K近邻搜索
- 半径内搜索
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.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 (std::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); // instantiate an object ptr
// Set input cloud data
octree.setInputCloud (cloud);
// 定义Octree边界框(可选操作)
//计算输入点云的边界框
octree.defineBoundingBox();
//手动定义点云的边界框
octree.defineBoundingBox(minX,minY,minZ,maxX,maxY,maxZ);
// 输入点云添加到Octree
octree.addPointsFromInputCloud ();
// create a virtual searchPoint
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 (std::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
// k近邻搜索
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 (std::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 (std::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;
}
// 删除八叉树，释放内存
octree.deleteTree()
}
PCL八叉树组件提供了几种八叉树类型。它们的叶节特征不同。
| CctreePointCloudPointVector (等于OctreePointCloud): |
该八叉树能够保存每一个叶节点上的点索引列。 |
|---|---|
| OctreePointCloudSinglePoint: | 该八叉树类仅仅保存每一个叶节点上的单个点索引。仅仅保存最后分配给叶节点的点索引。 |
| OctreePointCloudOccupancy | 该八叉树不存储它的叶节点上的任何点信息。它能用于空间填充情况检查。 |
| OctreePointCloudDensity: | 该八叉树存储每一个叶节点体素中的点的数目。它可以进行空间点集密程度查询。 |
3.3 无序点云的空间变化检测
检测出点云B中不在点云A空间的点的索引
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <iostream>
#include <vector>
#include <ctime>
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<pcl::PointXYZ> octree (resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );
// Generate pointcloud data for cloudA
cloudA->width = 128;
cloudA->height = 1;
cloudA->points.resize (cloudA->width * cloudA->height);
for (std::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 memory.
//交换八叉树缓存,但是cloudA对应的八叉树结构仍在内存中
octree.switchBuffers ();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
// Generate pointcloud data for cloudB
cloudB->width = 128;
cloudB->height = 1;
cloudB->points.resize (cloudB->width * cloudB->height);
for (std::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 voxels which did not exist in previous buffer
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
// Output points
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
for (std::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;
}
3.4 占据检测
3.5 获取所有占用体素的中心点(Voxel grid filter/downsampling)
std::vector<PointXYZ> pointGrid;
octree.getOccupiedVoxelCenters(pointGrid) ;
3.6 删除点所在的体素
3.7 迭代器
3.8 光线跟踪碰撞检测
# 打赏
码字不易，如果对您有帮助，就打赏一下吧O(∩_∩)O
支付宝
微信
标签：std,pcl,八叉树,points,octree,PCL,Octree,include,cloud 来源： https://www.cnblogs.com/long5683/p/13276787.html