点云 SSPF 描述子的代码实现

SSPF 描述子

（一）定义

SSPF 的主要思想是将点的邻域划分为多尺度的球壳面形状，然后在这些不同尺度的球壳内提取多维特征。

上图为 SSPF 描述子的图解，对于给定点，以其为球心不同半径构建多个球壳，对于每对相邻球壳之间包围的点云，查找其最低点与最高点的高度，基于此高度构建两个平面，由此得到三个距离：

① 平面1与平面2之间的距离 d1。

② 给定点到平面1的距离 d2。

③ 给定点到平面2的距离 d3。

（二）特性

SSPF 具有如下特性：

① 旋转不变性

② 允许降采样

③ 对点的分布不敏感

④ 对噪声具有鲁棒性

SSPF 代码实现

SSPF 特征用数学公式表示如下：

其中 d23 表示 d2 和 d3 之间的较大值。

代码实现即求出每对相邻球壳的 d1 以及 d23。

（一）SSPF.h

#include <iostream>
#include <unordered_map>
#include <vector>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

class SSPF
{
private:
	pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;	// 操作的点云
	pcl::PointXYZ p0;				// 球壳中心
	int M;						// 球面壳总数
	std::vector<double> r;				// 球壳半径
	std::vector<double> d1;				// d1集合，d1是两球壳中点云最高点和最低点所在平面之间的距离
	std::vector<double> d23;			// d23集合，d23是p0到两平面的距离的最大值

public:
	SSPF(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointXYZ p, int m, std::vector<double> r);


	std::vector<double> getD1();


	std::vector<double> getD23();


	pcl::PointXYZ getP0();


	pcl::PointCloud<pcl::PointXYZ>::Ptr getCloud();


	std::vector<double> getR();
};

（二）SSPF.cpp

#include "SSPF.h"
#include <set>
#include <cmath>

#include <pcl/octree/octree_search.h>

	
std::vector<double> SSPF::getD1()
{
	return d1;
}


std::vector<double> SSPF::getD23()
{
	return d23;
}


pcl::PointXYZ SSPF::getP0()
{
	return p0;
}


pcl::PointCloud<pcl::PointXYZ>::Ptr SSPF::getCloud()
{
	return m_cloud;
}


std::vector<double> SSPF::getR()
{
	return r;
}


/**
 * @brief			构造函数
 * @param cloud		进行操作的点云
 * @param p		球壳中心点
 * @param m		球壳数量
 * @param r		球壳半径集合
*/
SSPF::SSPF(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointXYZ p, int m, std::vector<double> r)
	: m_cloud(cloud), p0(p), M(m), r(r)
{
	// M个球壳有M - 1个空间
	for (int i = 0; i < M - 1; i++)
	{
		// 分别计算两半径球体的点索引
		std::set<int> smallSphereIndices = rSearch(r[i], p0, cloud);
		std::set<int> largeSphereIndices = rSearch(r[i + 1], p0, cloud);

		// 提取两球壳包围的点
		pcl::PointCloud<pcl::PointXYZ>::Ptr btwCloud = betweenCloud(smallSphereIndices, largeSphereIndices, cloud);
		
		// 计算球壳包围的点的最大和最小Z值
		std::vector<double> zRange = zRangeOfCloud(btwCloud);

		// 计算d1和d23
		d1.push_back(zRange[1] - zRange[0]);
		d23.push_back(std::max(std::abs(p0.z - zRange[0]), std::abs(p0.z - zRange[1])));
	}
}


/**
 * @brief			        返回给定点、给定半径的球体中点的索引
 * @param r			球体半径
 * @param p			给定球心
 * @param cloud		        操作的点云
 * @return			球体中点的索引
*/
std::set<int> rSearch(double r, pcl::PointXYZ p, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	// 创建octree对象用于点云的搜索
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.5);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	// 存储球体内的点索引
	std::vector<int> pointIdx;
	std::vector<float> pointRadiusSquaredDistance;

	// 用set存放球体内点的索引，提高查找效率
	std::set<int> sphereIndices;

	// 执行半径搜索
	if (octree.radiusSearch(p, r, pointIdx, pointRadiusSquaredDistance) > 0)
	{
		sphereIndices.insert(pointIdx.begin(), pointIdx.end());
	}

	return sphereIndices;
}


/**
 * @brief			        提取两球壳内部的点云
 * @param smallSphereIndices	较小球体内点的索引
 * @param largeSphereIndices	较大球体内点的索引
 * @param cloud			原始点云
 * @return			两球壳中间的点
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr betweenCloud(std::set<int> smallSphereIndices, 
	std::set<int> largeSphereIndices, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr resultCloud(new pcl::PointCloud<pcl::PointXYZ>);
	for (const auto& idx : largeSphereIndices)
	{
		// 点在大球内但不在小球内，即所要提取的点
		if (smallSphereIndices.find(idx) == smallSphereIndices.end())
		{
			resultCloud->push_back(cloud->points[idx]);
		}
	}

	// 更新点云信息
	resultCloud->width = resultCloud->points.size();
	resultCloud->height = 1;

	return resultCloud;
}


/**
 * @brief			找到给定点云的Z值范围
 * @param cloud		给定点云
 * @return		点云中的Z最小值和最大值数值集合，vector中第一个值是最小值，第二个数是最大值
*/
std::vector<double> zRangeOfCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	double minZ = cloud->points[0].z, maxZ = cloud->points[0].z;
	for (const auto& p : cloud->points)
	{
		if (p.z > maxZ)
			maxZ = p.z;
		if (p.z < minZ)
			minZ = p.z;
	}

	std::vector<double> result;
	result.push_back(minZ);
	result.push_back(maxZ);
	
	return result;
}

（三）main.cpp

#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include "SSPF.h"

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::io::loadPCDFile("C:\\Users\\10704\\Desktop\\dezhou1\\原始数据\\dezhou1_1-3_z_passThrough_downsample.pcd", *cloud);
	pcl::PointXYZ p(442429.406, 4130679.5, 14.930);
	std::vector<double> r{0.2, 0.5, 0.7, 1.0};
	SSPF sspf(cloud, p, 4, r);

	std::vector<double> d1 = sspf.getD1();

	std::vector<double> d23 = sspf.getD23();

	std::cout << "----------  d1  ----------" << std::endl;
	for (const auto& it : d1)
		std::cout << it << std::endl;

	std::cout << "----------  d23  ----------" << std::endl;
	for (const auto& it : d23)
		std::cout << it << std::endl;

	system("pause");
	return 0;
}

（四）实验结果

蓝、黄、红色的点云分别表示不同相邻球壳之间的点云。