天天看点

PCL笔记四:k-d tree和八叉树;近邻搜索;半径搜索;体素内搜索;空间变化检测;一:近邻搜索二、八叉树octree点云压缩三、基于八叉树的空间划分及搜索操作四、无序点云数据集的空间变化检测

点云数据主要是表征目标表面的海量点集合,并不具备传统实体网格数据的几何拓扑信息。

点云数据处理中最为核心的问题就是建立离散点间的拓扑关系,实现基于邻域关系的快速查找。

建立空间索引在点云数据处理中已被广泛应用,常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构:BSP树、k-d tree、KDB树、R树、R+树、CELL树、四叉树、八叉树等。

k-d tree对于区间和近邻搜索十分有用。

PCL中k-d tree 库提供了k-d tree数据结构,基于FLANN进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、领域特征提取中是非常基础的核心操作。

一:近邻搜索

  • KNN算法:用k-d tree 树找到具体点或空间位置的k近邻,
  • R半径搜索:找到用户指定的某一半径内的所有近邻。
近邻搜索需要用的kdtree头文件,kdtree是依赖于FLANN实现的。
#include<iostream>
#include<vector>
#include<ctime>
#include<pcl/point_cloud.h>
#include<pcl/kdtree/kdtree_flann.h>
using namespace std;

int main()
{
	srand(time(NULL));  // 利用系统时间初始化rand()函数的种子。
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// 点云生成
	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);
	}
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  // 创建一个KdTreeFLANN对象,输入点云。
	kdtree.setInputCloud(cloud);

	pcl::PointXYZ searchPoint;   // 创建一个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);

	// k近邻搜索
	int K = 10;    // 10个近邻点。
	vector<int>pointIdxNKNSearch(K);    // 存储搜索到查询点近邻的索引。
	vector<float>pointNKNSquaredDistance(K);   // 存储对应近邻的平方距离。
	cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y
		<< " " << searchPoint.z << ") with K = " << K << endl;

	// kdtree.nearestKSearch函数搜索距离索引点最近的K个点,搜到后返回搜到的点数。
	int res = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
	cout << "nearestKSearch返回值为:" << res << endl;
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		// 打印输出。
		for (size_t i = 0; i < pointIdxNKNSearch.size(); i++)
		{
			cout << " " << cloud->points[pointIdxNKNSearch[i]].x << " " << cloud->points[pointIdxNKNSearch[i]].y
				 << " " << cloud->points[pointIdxNKNSearch[i]].z << " " << "(squared distance:"
				 << pointNKNSquaredDistance[i] << ")" << endl;
		}
	}

	cout << endl;

	// R半径搜索:在半径r内搜索近邻。
	vector<int> pointIdxRadiusSearch;  // 存储近邻索引。
	vector<float> pointRadiusSquaredDistance;   // 存储近邻对应的平均距离。
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z
		 << ") with radius=" << radius << endl;
	int b = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
	cout << "R半径搜索方法:kdtree.radiusSearch返回值为:" << b << endl;
	if (b > 0)
	{
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); i++)
		{
			cout << " " << cloud->points[pointIdxRadiusSearch[i]].x << " "
				<< cloud->points[pointIdxRadiusSearch[i]].y << " "
				<< cloud->points[pointIdxRadiusSearch[i]].z << " "
				<< "(squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;
		}
	}
	return 0;
}
           

输出:

K nearest neighbor search at (297.688 508.344 647.969) with K = 10
nearestKSearch返回值为:10
 289.781 493.781 696 (squared distance:2581.58)
 281.125 449.469 574.219 (squared distance:9179.64)
 390.625 521.188 667.844 (squared distance:9197.36)
 349.406 558.469 576.313 (squared distance:10322)
 340.656 456.781 556.625 (squared distance:12848.7)
 339.844 615.188 636.969 (squared distance:13313.7)
 245.125 426.063 717.094 (squared distance:14311.3)
 220.969 604.781 639.875 (squared distance:15251.5)
 309.844 435.563 759.656 (squared distance:17919)
 347.813 627.563 684.969 (squared distance:18094.6)

Neighbors within radius search at (297.688 508.344 647.969) with radius=218
R半径搜索方法:kdtree.radiusSearch返回值为:42
 289.781 493.781 696 (squared distance: 2581.58)
 281.125 449.469 574.219 (squared distance: 9179.64)
 390.625 521.188 667.844 (squared distance: 9197.36)
 349.406 558.469 576.313 (squared distance: 10322)
 340.656 456.781 556.625 (squared distance: 12848.7)
 339.844 615.188 636.969 (squared distance: 13313.7)
 245.125 426.063 717.094 (squared distance: 14311.3)
 220.969 604.781 639.875 (squared distance: 15251.5)
 309.844 435.563 759.656 (squared distance: 17919)
 347.813 627.563 684.969 (squared distance: 18094.6)
 277.063 627.781 588.406 (squared distance: 18238.4)
 320.219 634.188 577.094 (squared distance: 21367.6)
 432.281 574.031 618.625 (squared distance: 23291.4)
 399.438 488.844 533.25 (squared distance: 23893.7)
 260.25 521.469 798 (squared distance: 24083.2)
 292.781 647 569.625 (squared distance: 25387.4)
 244.281 563.438 795.469 (squared distance: 27643.8)
 251.125 658 585.219 (squared distance: 28502.6)
 407.844 469 522.469 (squared distance: 29432.6)
 409.938 548.531 523.969 (squared distance: 29591.1)
 189.594 573.656 769.875 (squared distance: 30811.1)
 256.781 643.156 752.688 (squared distance: 30813.7)
 122.938 489.313 656.5 (squared distance: 30972.5)
 272.063 335.688 607.063 (squared distance: 32140.1)
 462.969 579.313 652.688 (squared distance: 32376.7)
 359.469 380.594 536.375 (squared distance: 32590.1)
 320.281 399.063 504.344 (squared distance: 33081)
 176.406 372.781 663.344 (squared distance: 33322.7)
 179.563 443.125 522.844 (squared distance: 33863.3)
 449.188 609.594 699.281 (squared distance: 35836.8)
 374.531 614.969 510.188 (squared distance: 36257.5)
 363.875 604.656 495.031 (squared distance: 37046.8)
 277.219 664.938 773.625 (squared distance: 40730.1)
 333.688 589.469 466.688 (squared distance: 40740.2)
 420.813 357.625 707.75 (squared distance: 41449.7)
 293.031 319.281 568.844 (squared distance: 42027.1)
 271.844 712.188 645.594 (squared distance: 42225.8)
 415.406 337.469 660.75 (squared distance: 43219.3)
 120.344 422.563 714.656 (squared distance: 43256.4)
 342.25 586.313 460.281 (squared distance: 43291.5)
 151.125 368.563 580.719 (squared distance: 45541.9)
 148.656 647.344 713.375 (squared distance: 45809.3)

           

二、八叉树octree点云压缩

点云由庞大的数据集组成,这些数据集通过距离、颜色、法线等附加信息来描述空间三维点。此外,点云能以非常高的速率被创建出来,因此需要占用相当大的存储资源,一旦点云需要存储或者通过速率受限制的通信信道进行传输,提供针对这种数据的压缩方法就变得十分有用。

PCL库点云压缩功能,允许编码压缩所有类型的点云,包括无序点云。

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())
		{
			// 存储压缩点云的字节流。
			std::stringstream compressedData;
			// 输出点云
			pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZRGBA>());
			// 压缩点云
			PointCloudEncoder->encodePointCloud(cloud, compressedData);
			// 解压缩点云
			PointCloudDecoder->decodePointCloud(compressedData, cloudOut);
			// 可视化解压缩点云
			viewer.showCloud(cloudOut);
		}
	}

	void run()
	{
		bool showStatistics = true;
		// 压缩选项详见:/io/include/pcl/compression/compression_profiles.h
		pcl::octree::compression_Profiles_e compressionProfile = pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;
		// 初始化压缩与解压缩对象。
		PointCloudEncoder = new pcl::octree::PointCloudCompression<pcl::PointXYZRGBA>(compressionProfile, showStatistics);
		PointCloudDecoder = new pcl::octree::PointCloudCompression<pcl::PointXYZRGBA>();
		//创建从 OpenNI获取点云的对象
		pcl::Grabber* interface = new pcl::OpenNIGrabber();
		//建立回调函数
		boost::function<void
		(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);
		// 建立回调函数与回调信号之间联系
		boost::signals2::connection c = interface->registerCallback(f);
		// 开始接收点云数据流
		interface->start();
		while (!viewer.wasStopped())
		{
			sleep(1);
		}
		interface->stop();
		delete(PointCloudEncoder);
		delete(PointCloudDecoder);
	}
	pcl::visualization::CloudViewer viewer;
	pcl::octree::PointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;
	pcl::octree::PointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;
};


int main()
{
	SimpleOpenNIViewer v;
	v.run();
	return(0);
}
           

三、基于八叉树的空间划分及搜索操作

八叉树octree是一种用于管理稀疏3D数据的树状数据结构,每个内部节点都正好由八个子节点。

空间划分及近邻搜索:

  • 体素内近邻搜索(Neighbors within Voxel Search)
  • K近邻搜索(K Nearest Neighbor Search)
  • 半径内近邻搜索(Neighbors within Radius Search)
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char** argv)
{
	srand((unsigned int)time(NULL));  // 利用系统时间初始化rand()函数的随机数种子。
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 实例化一个PointCloud指针对象。

	// 创建点云数据
	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);
	}

	// 创建八叉树实例,设置分辨率进行初始化,该八叉树用它的叶节点存放点索引向量
	float resolution = 128.0f;  // 该分辨率参数描述最低一级八叉树的最小体素的尺寸。因此八叉树的深度是分辨率和点云空间
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 初始化八叉树
	octree.setInputCloud(cloud);  // 设置输入点云
	octree.addPointsFromInputCloud();  // 构建八叉树。 此时将PointCloud和八叉树联系在一起,就可以进行搜索了。

	// 创建一个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);
	// 体素内近邻搜索
	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 (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近邻搜索
	int K = 10;
	std::vector<int>pointIdxNKNSearch;  // 存储k近邻搜索点索引结果
	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 (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;
	}

	//半径内近邻搜索
	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 (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;
	}
}
           
float resolution = 128.0f;  // 该分辨率参数描述最低一级八叉树的最小体素的尺寸。
           

八叉树的深度是分辨率和点云空间维数的函数,如果知道点云的边界框,应该用defineBoundingBox方法把它分配给八叉树,然后通过点云指针把所有点增加到八叉树中。

if (octree.voxelSearch(searchPoint, pointIdxVec))
           

体素内近邻搜索octree.voxelSearch的返回值类型为bool类型。返回结果存在第二个参数中。

if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
           

K近邻搜索octree.nearestKSearch返回符合条件的点的个数。

if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
           

R半径内近邻搜索octree.radiusSearch返回符合条件的点的个数。

输出

Neighbors within voxel search at (809.531 493.688 681.438)
    706.969 481.656 742.625
    716.781 531.656 719.625
K nearest neighbor search at (809.531 493.688 681.438) with K=10
    829.938 532.656 749.719 (squared distance: 6597.31)
    767.313 571.719 685.594 (squared distance: 7888.57)
    792.469 466.5 593.938 (squared distance: 8686.54)
    903.938 505.719 684.531 (squared distance: 9066.86)
    718.156 450.563 649.594 (squared distance: 11223.2)
    716.781 531.656 719.625 (squared distance: 11502.5)
    706.969 481.656 742.625 (squared distance: 14407.7)
    802 604.313 735.25 (squared distance: 15190.4)
    891.406 458.344 769.438 (squared distance: 15696.7)
    816.75 497.25 808.563 (squared distance: 16225.6)
Neighbors within radius search at (809.531 493.688 681.438) with radius=114.75
    718.156 450.563 649.594 (squared distance: 11223.2)
    792.469 466.5 593.938 (squared distance: 8686.54)
    716.781 531.656 719.625 (squared distance: 11502.5)
    903.938 505.719 684.531 (squared distance: 9066.86)
    829.938 532.656 749.719 (squared distance: 6597.31)
    767.313 571.719 685.594 (squared distance: 7888.57)
           

八叉树部分类关键点说明:

PCLoctree组件提供了几个八叉树类型。他们各自的叶节点特征基本上是不同的。
  • OctreePointCloudPointVector(等于OctreePointCloud):该八叉树能够保存每一个叶节点上的点索引列。
  • OctreePointCloudSinglePoint:该八叉树类仅仅保存每一个叶节点上的单个点索引。仅仅保存最后分配给叶节点的点索引。
  • OctreePointCloudOccupancy:该八叉树不存储它的叶节点上的任何点信息。他能用于空间填充情况检查。
  • OctreePointCloudDensity:该八叉树存储每一个叶节点体素中的点的数目。它可以进行空间点集密集程序查询。

如果需要高频率创建八叉树:八叉树双缓冲技术实现(Octree2BufBase类)。该类在内存中同时保存了两个类似的八叉树对象。除了搜索操作之外,也可以用于空间变化检测。

此外高级内存管理减少了八叉树建立过程中的内存分配和释放操作。双缓冲技术对八叉树的实现可以通过设置模板参数“OctreeT”实例化不同的OctreePointCloud类。所有的八叉树都支持八叉树结构和八叉树内容的串行化和反串行化。

四、无序点云数据集的空间变化检测

利用八叉树实现用于多个无序点云之间的空间变化检测。

这些点云可能在尺寸、分辨率、密度和点顺序等方面有所差异。通过递归地比较八叉树的树结构,可以鉴定出由八叉树产生的体素组成之间的区别所代表的空间变化。

使用PCL八叉树“双缓冲”技术,以便能实时地探测多个点云之间的空间组成差异。
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
using namespace std;


int main()
{
	srand((unsigned int)time(NULL));  // 利用系统时间初始化rand()函数的随机数种子。
	float resolution = 32.0f; // 八叉树分辨率 即体素的大小

	// 初始化空间变化检测对象
	pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution); // 初始化空间变化检测对象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
	
	//为cloudA创建点云
	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);
	}
	//添加点云到八叉树,建立八叉树
	octree.setInputCloud(cloudA);
	octree.addPointsFromInputCloud();
	// 交换八叉树缓存,但是cloudA对应的八叉树仍在内存中
	octree.switchBuffers();

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
	// 为cloudB创建点云
	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);
	}
	//添加 cloudB到八叉树
	octree.setInputCloud(cloudB);
	octree.addPointsFromInputCloud();

	std::vector<int>newPointIdxVector;  // 保存新增点索引
	//获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
	octree.getPointIndicesFromNewVoxels(newPointIdxVector);
	//打印输出点
	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;
}
           

点云cloudA是参考点云,用其建立的八叉树对象描述它的空间分布OctreePointCloudChangeDetector类继承自Octree2BufBase类,Octree2BufBase类允许同时在内存中保存和管理两个八叉树,另外,它应用了内存池,该机制能够重新利用已经分配了的节点对象,因此减少了在生成多个点云八叉树对象时昂贵的内存分配和释放操作。

octree.switchBuffers();	// 交换八叉树缓存,但是cloudA对应的八叉树仍在内存中
           

通过访问"octree.switchBuffers()"重置了八叉树对象的缓冲区,把之前的八叉树数据仍保留在内存中。

第二个点云对象cloudB用于建立新的八叉树结构,该新的八叉树与前一个cloudA对应的八叉树共享八叉树对象,但同时在内存中驻留。
octree.getPointIndicesFromNewVoxels(newPointIdxVector);
           

通过调用octree.getPointIndicesFromNewVoxels()方法,探测两个八叉树之间体素的不同。返回的时cloudB中新添加点的索引的向量,通过索引向量可以获取R点集(cloudA中没有的点)。此方法只能探测到在cloudA基础上,新增的点,而不能判断减少的点。

继续阅读