【点云处理技术之PCL】Octree

1. 八叉树简介

八叉树是一种用于描述三维空间的树状数据结构，八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，将八个子节点所表示的体积元素加在一起就等于父节点的体积。

2. 点云的压缩

点云信息一般比较包含了丰富的信息而且点云数量也是比较多，这就需要我们对点云数据进行压缩。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)
#endifclass SimpleOpenNIViewer
{
public:SimpleOpenNIViewer () :viewer (" Point Cloud Compression Example"){}
/************************************************************************************************在OpenNIGrabber采集循环执行的回调函数cloud_cb_中，首先把获取的点云压缩到stringstream缓冲区，下一步就是解压缩，它对压缩了的二进制数据进行解码，存储在新的点云中解码了点云被发送到点云可视化对象中进行实时可视化
*************************************************************************************************/void  cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){if (!viewer.wasStopped ()){// 存储压缩点云的字节流对象// stringstream to store compressed point cloudstd::stringstream compressedData;// 存储输出点云// output pointcloudpcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());// 压缩点云// compress point cloudPointCloudEncoder->encodePointCloud (cloud, compressedData);// 解压缩点云// decompress point cloudPointCloudDecoder->decodePointCloud (compressedData, cloudOut);// 可视化解压缩的点云// show decompressed point cloudviewer.showCloud (cloudOut);}}
/**************************************************************************************************************在函数中创建PointCloudCompression类的对象来编码和解码，这些对象把压缩配置文件作为配置压缩算法的参数所提供的压缩配置文件为OpenNI兼容设备采集到的点云预先确定的通用参数集，本例中使用MED_RES_ONLINE_COMPRESSION_WITH_COLOR配置参数集，用于快速在线的压缩，压缩配置方法可以在文件/io/include/pcl/compression/compression_profiles.h中找到，在PointCloudCompression构造函数中使用MANUAL——CONFIGURATION属性就可以手动的配置压缩算法的全部参数
******************************************************************************************************************/void run (){bool showStatistics = true;  //设置在标准设备上输出打印出压缩结果信息// 压缩选项详情在: /io/include/pcl/compression/compression_profiles.h// for a full list of profiles see: /io/include/pcl/compression/compression_profiles.hpcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;// 初始化压缩和解压缩对象  其中压缩对象需要设定压缩参数选项，解压缩按照数据源自行判断// instantiate point cloud compression for encoding and decodingPointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();/***********************************************************************************************************下面的代码为OpenNI兼容设备实例化一个新的采样器，并且启动循环回调接口，每从设备获取一帧数据就回调函数一次，，这里的回调函数就是实现数据压缩和可视化解压缩结果。************************************************************************************************************///创建从OpenNI获取点云的抓取对象// create a new grabber for OpenNI devicespcl::Grabber* interface = new pcl::OpenNIGrabber ();// 建立回调函数// make callback function from member functionboost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);//建立回调函数和回调信息的绑定// connect callback function for desired signal. In this case its a point cloud with color valuesboost::signals2::connection c = interface->registerCallback (f);// 开始接受点云的数据流// start receiving point cloudsinterface->start ();while (!viewer.wasStopped ()){sleep (1);}interface->stop ();// 删除压缩与解压缩的实例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;  //创建一个新的SimpleOpenNIViewer  实例并调用他的run方法v.run ();return (0);
}

3. 八叉树的搜索

octree是一种用于管理稀疏3D数据的树形数据结构，每个内部节点都正好有八个子节点，pcl中的octree搜索有三种方式：

• 实现“体素内*邻搜索（Neighbors within VOxel Search）”
• “K*邻搜索（K Nearest Neighbor Search）”
• “半径内*邻搜索”（Neighbors within Radius Search）

代码示例如下：

#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->size (); ++i){(*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);(*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);(*cloud)[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);}float resolution = 128.0f;//设置octree体素分辨率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 (std::size_t i = 0; i < pointIdxVec.size (); ++i)std::cout << "    " << (*cloud)[pointIdxVec[i]].x << " " << (*cloud)[pointIdxVec[i]].y << " " << (*cloud)[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)[ pointIdxNKNSearch[i] ].x << " " << (*cloud)[ pointIdxNKNSearch[i] ].y << " " << (*cloud)[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;}// Neighbors within radius search/***********************半径R搜索********************************/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)[ pointIdxRadiusSearch[i] ].x << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;}
}

4. 无序点云数据集的空间变化检测

除了搜索功能，pcl中的八叉树还有一种功能，就是提取或检测两个点云数据集中，点云的非共同点云数据，也就是检测两个点云数据的变化部分。octree使用“双缓冲”技术实现这一功能。

代码示例：

#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 classpcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );// Generate pointcloud data for cloudA//生成点云cloudAcloudA->width = 128;cloudA->height = 1;cloudA->points.resize (cloudA->width * cloudA->height);for (std::size_t i = 0; i < cloudA->size (); ++i){(*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);(*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);(*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);}//输出cloudA// std::cout<<"cloudA:"<<std::endl;// for (const auto& point: *cloudA)//   std::cerr << "    " << point.x << " "//                       << point.y << " "//                       << point.z << std::endl;// Add points from cloudA to octreeoctree.setInputCloud (cloudA);octree.addPointsFromInputCloud ();// Switch octree buffers: This resets octree but keeps previous tree structure in memory.octree.switchBuffers ();//双缓冲pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );// Generate pointcloud data for cloudB //生成点云cloudBcloudB->width = 128;cloudB->height = 1;cloudB->points.resize (cloudB->width * cloudB->height);for (std::size_t i = 0; i < cloudB->size (); ++i){(*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);(*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);(*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);}//输出cloudB// std::cout<<"cloudB:"<<std::endl;// for (const auto& point: *cloudA)//   std::cerr << "    " << point.x << " "//                       << point.y << " "//                       << point.z << std::endl;// Add points from cloudB to octreeoctree.setInputCloud (cloudB);octree.addPointsFromInputCloud ();std::vector<int> newPointIdxVector;//索引// Get vector of point indices from octree voxels which did not exist in previous buffer// 获取前一cloudA对应八叉树在cloudB对应在八叉树中没有的点集octree.getPointIndicesFromNewVoxels (newPointIdxVector);// Output points//输出cloudB中，cloudA没有的点云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)[newPointIdxVector[i]].x << " "<< (*cloudB)[newPointIdxVector[i]].y << " "<< (*cloudB)[newPointIdxVector[i]].z << std::endl;}