点云目标检测算法 知乎 点云变化检测

一、简介

  八叉树可以实现多个无序点云之间的空间变换检测，这些点云可能在尺寸、分辨率、密度和点顺序方面有所差异。通过递归地比较八叉树的树结构，可以鉴定出由八叉树产生的体素组成之间的区别所代表的空间变化。此外，使用PCL的双缓冲技术，以便实时地探测多个点云之间的空间组成差异。

二、代码分析

  1）首先，实例化OctreePointCloudChangeDetector类，并定义它的体素分辨率：

srand ((unsigned int) time (NULL));
// 八叉树分辨率 即体素的大小
float resolution =32.0f;
// 初始化空间变化检测对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution);

  2）然后，创建点云实例cloudA，用随机数据进行填充，生成的点数据用于建立八叉树形象：

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 ();

  3）点云cloudA是参考点云，用其建立的八叉树对象描述它的空间分布OctreePointCloud ChangeDetector类继承自Octree2BufBase类，Octree2BufBase类允许同时在内存中保存和管理两个八叉树。另外，它应用了内存池，该机制能够重新利用已经分配了的节点对象，因此减少了在生成多个点云八叉树对象时昂贵的内存分配和释放操作。通过访问“octree.switchBuffers()”，重置了八叉树对象的缓冲区，但把之前的八叉树数据仍保留在内存中：

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

  4）现在我们实例化第二个点云对象cloudB并用随机点数据填充它，该点云用于建立新的八叉树结构，该新的八叉树与前一个cloudA对应的八叉树共享八叉树对象，但同时在内存中驻留：

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 ();

  5）为了获取存在于cloudB的点集R（此R并没有cloudA中元素），可以调用getPointIndicesFrom NewVoxels方法，通过探测两个八叉树之间体素的不同来实现。它返回cloudB中新加点的索引的向量，通过索引向量可以获取R点集。很明显，这样就探测了cloudB相对于cloudA变化的点集，但是只能探测在cloudA上增加的点集，而不能探测在cloudA上减少的点集：

std::vector<int>newPointIdxVector;
//获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  6）最后，把结果输出到std::cout数据流，打印到输出设备上：

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;

  7）整体代码如下：

#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/octree/octree.h>

int main(int argc, char** argv)
{
	srand((unsigned int)time(NULL));

	float resolution = 32.0f;																//设置八叉树最小一级的体素的像素大小
	pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree(resolution);			//初始化八叉树
	
	pcl::PointCloud<pcl::PointXYZ> ::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);		//随机数填充一个点云
	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();														

	octree.switchBuffers();																	//重置八叉树对象的缓冲区，把之前的八叉树数据仍旧保留在内存
	pcl::PointCloud<pcl::PointXYZ> ::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);		//实例化第二个点云
	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);
	}

	octree.setInputCloud(cloudB);															//第二个点云与第一个点云共享同一个八叉树对象，但同时在内存中驻留
	octree.addPointsFromInputCloud();

	std::vector<int>newPointIdxVector;
	octree.getPointIndicesFromNewVoxels(newPointIdxVector);									//探测第二个点云相对于第一个点云变化的点集（只探测增加，不探测减少）

	std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
	for (size_t i = 0;i < newPointIdxVector.size();++i)
	{
	std::cout << "# index:" << newPointIdxVector[i] << "# point:" << cloudB->points[newPointIdxVector[i]].x << " " << cloudB->points[newPointIdxVector[i]].y << " " << cloudB->points[newPointIdxVector[i]].z << std::endl;
	}
	
	return(0);

}

三、输出结果